Plot recoloring, substantial fixes to HSI widget

qgroundcontrol.pro

@@ -321,7 +321,8 @@ HEADERS += src/MG.h \
     src/ui/map/QGCMapToolBar.h \
     src/libs/qextserialport/qextserialenumerator.h \
     src/QGCGeo.h \
-    src/ui/QGCToolBar.h
+    src/ui/QGCToolBar.h \
+    src/ui/linechart/QGCLineChartCurveLabel.h
 
 # Google Earth is only supported on Mac OS and Windows with Visual Studio Compiler
 macx|win32-msvc2008|win32-msvc2010::HEADERS += src/ui/map3D/QGCGoogleEarthView.h
@@ -446,7 +447,8 @@ SOURCES += src/main.cc \
     src/ui/map/Waypoint2DIcon.cc \
     src/ui/map/QGCMapTool.cc \
     src/ui/map/QGCMapToolBar.cc \
-    src/ui/QGCToolBar.cc
+    src/ui/QGCToolBar.cc \
+    src/ui/linechart/QGCLineChartCurveLabel.cc
 
 # Enable Google Earth only on Mac OS and Windows with Visual Studio compiler
 macx|win32-msvc2008|win32-msvc2010::SOURCES += src/ui/map3D/QGCGoogleEarthView.cc

src/comm/MAVLinkProtocol.cc

@@ -207,6 +207,15 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
             // ORDER MATTERS HERE!
             // If the matching UAS object does not yet exist, it has to be created
             // before emitting the packetReceived signal
+
+            // BIG NASTY HACK
+            //TODO
+            //BUG
+            //BAD
+            //FIXME
+            if (message.sysid == 35)
+                message.sysid = 42;
+
             UASInterface* uas = UASManager::instance()->getUASForId(message.sysid);
 
             // Check and (if necessary) create UAS object

src/uas/UAS.cc

@@ -72,7 +72,7 @@ pitch(0.0),
 yaw(0.0),
 statusTimeout(new QTimer(this)),
 paramsOnceRequested(false),
-airframe(0),
+airframe(QGC_AIRFRAME_EASYSTAR),
 attitudeKnown(false),
 paramManager(NULL),
 attitudeStamped(false),
@@ -1162,7 +1162,7 @@ void UAS::setLocalPositionSetpoint(float x, float y, float z, float yaw)
 {
 #ifdef MAVLINK_ENABLED_PIXHAWK
     mavlink_message_t msg;
-    mavlink_msg_position_control_setpoint_set_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, 0, x, y, z, yaw);
+    mavlink_msg_local_position_setpoint_set_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, x, y, z, yaw/M_PI*180.0);
     sendMessage(msg);
 #else
     Q_UNUSED(x);

src/ui/HSIDisplay.cc

@@ -100,7 +100,8 @@ HSIDisplay::HSIDisplay(QWidget *parent) :
     mavInitialized(false),
     bottomMargin(10.0f),
     topMargin(12.0f),
-    userSetPointSet(false)
+    userSetPointSet(false),
+    dragStarted(false)
 {
     refreshTimer->setInterval(updateInterval);
 
@@ -121,6 +122,9 @@ HSIDisplay::HSIDisplay(QWidget *parent) :
 
     // Do first update
     setMetricWidth(metricWidth);
+    // Set tooltip
+    setToolTip(tr("View from top in body frame. Scroll with mouse wheel to change the horizontal field of view of the widget."));
+    setStatusTip(tr("View from top in body frame. Scroll with mouse wheel to change the horizontal field of view of the widget."));
 }
 
 void HSIDisplay::resetMAVState()
@@ -200,8 +204,9 @@ void HSIDisplay::renderOverlay()
     painter.setPen(pen);
     const int ringCount = 2;
     for (int i = 0; i < ringCount; i++) {
-        float radius = (vwidth - (topMargin + bottomMargin)*0.3f) / (1.3f * i+1) / 2.0f - bottomMargin / 2.0f;
+        float radius = (vwidth - (topMargin + bottomMargin)*0.3f) / (1.35f * i+1) / 2.0f - bottomMargin / 2.0f;
         drawCircle(xCenterPos, yCenterPos, radius, 0.1f, ringColor, &painter);
+        paintText(tr("%1 m").arg(refToMetric(radius), 5, 'f', 1, ' '), QGC::colorCyan, 1.6f, vwidth/2-4, vheight/2+radius+2.2, &painter);
     }
 
     // Draw orientation labels
@@ -217,6 +222,15 @@ void HSIDisplay::renderOverlay()
     painter.rotate(+yawRotate);
     painter.translate(-(xCenterPos)*scalingFactor, -(yCenterPos)*scalingFactor);
 
+    // Draw trail
+//    QPointF m(bodyXSetCoordinate, bodyYSetCoordinate);
+//    // Transform from body to world coordinates
+//    m = metricWorldToBody(m);
+//    // Scale from metric body to screen reference units
+//    QPointF s = metricBodyToRef(m);
+//    drawLine(s.x(), s.y(), xCenterPos, yCenterPos, 1.5f, uas->getColor(), &painter);
+
+
     // Draw center indicator
 //    QPolygonF p(3);
 //    p.replace(0, QPointF(xCenterPos, yCenterPos-4.0f));
@@ -252,28 +266,31 @@ void HSIDisplay::renderOverlay()
 
     // Draw position setpoints in body coordinates
 
-    if (userSetPointSet) {
+    if (userSetPointSet)
+    {
         QColor spColor(150, 150, 150);
         drawSetpointXY(uiXSetCoordinate, uiYSetCoordinate, uiYawSet, spColor, painter);
     }
 
-    if (positionSetPointKnown) {
+    // Labels on outer part and bottom
+
+    // Draw waypoints
+    drawWaypoints(painter);
+
+    // Draw setpoint over waypoints
+    if (positionSetPointKnown)
+    {
         // Draw setpoint
-        drawSetpointXY(bodyXSetCoordinate, bodyYSetCoordinate, bodyYawSet, QGC::colorCyan, painter);
+        drawSetpointXY(bodyXSetCoordinate, bodyYSetCoordinate, bodyYawSet, uas->getColor(), painter);
         // Draw travel direction line
         QPointF m(bodyXSetCoordinate, bodyYSetCoordinate);
         // Transform from body to world coordinates
         m = metricWorldToBody(m);
         // Scale from metric body to screen reference units
         QPointF s = metricBodyToRef(m);
-        drawLine(s.x(), s.y(), xCenterPos, yCenterPos, 1.5f, QGC::colorCyan, &painter);
+        drawLine(s.x(), s.y(), xCenterPos, yCenterPos, 1.5f, uas->getColor(), &painter);
     }
 
-    // Labels on outer part and bottom
-
-    // Draw waypoints
-    drawWaypoints(painter);
-
     // Draw status flags
     drawStatusFlag(2,  1, tr("ATT"), attControlEnabled, attControlKnown, painter);
     drawStatusFlag(22, 1, tr("PXY"), xyControlEnabled,  xyControlKnown,  painter);
@@ -300,20 +317,20 @@ void HSIDisplay::renderOverlay()
         // Position
         QString str;
         str.sprintf("%05.2f %05.2f %05.2f m", x, y, z);
-        paintText(tr("POS"), QGC::colorCyan, 2.6f, 2, vheight- 5.0f, &painter);
-        paintText(str, Qt::white, 2.6f, 10, vheight - 5.0f, &painter);
+        paintText(tr("POS"), QGC::colorCyan, 2.6f, 2, vheight- 4.0f, &painter);
+        paintText(str, Qt::white, 2.6f, 10, vheight - 4.0f, &painter);
     }
 
     if (globalAvailable > 0) {
         // Position
         QString str;
         str.sprintf("lat: %05.2f lon: %06.2f alt: %06.2f", lat, lon, alt);
-        paintText(tr("GPS"), QGC::colorCyan, 2.6f, 2, vheight- 5.0f, &painter);
-        paintText(str, Qt::white, 2.6f, 10, vheight - 5.0f, &painter);
+        paintText(tr("GPS"), QGC::colorCyan, 2.6f, 2, vheight- 4.0f, &painter);
+        paintText(str, Qt::white, 2.6f, 10, vheight - 4.0f, &painter);
     }
 
     // Draw Field of view to bottom right
-    paintText(tr("FOV %1 m").arg(metricWidth, 5, 'f', 1, ' '), QGC::colorCyan, 2.6f, 55, vheight- 5.0f, &painter);
+    //paintText(tr("FOV %1 m").arg(metricWidth, 5, 'f', 1, ' '), QGC::colorCyan, 2.6f, 55, vheight- 5.0f, &painter);
 }
 
 void HSIDisplay::drawStatusFlag(float x, float y, QString label, bool status, bool known, QPainter& painter)
@@ -466,13 +483,13 @@ QPointF HSIDisplay::metricBodyToWorld(QPointF body)
 {
     // First rotate into world coordinates
     // then translate to world position
-    QPointF result((cos(yaw) * body.x()) + (sin(yaw) * body.y()) + x, (-sin(yaw) * body.x()) + (cos(yaw) * body.y()) + y);
+    QPointF result((cos(-yaw) * body.x()) + (sin(-yaw) * body.y()) + x, (-sin(-yaw) * body.x()) + (cos(-yaw) * body.y()) + y);
     return result;
 }
 
 QPointF HSIDisplay::screenToMetricBody(QPointF ref)
 {
-    return QPointF(-((screenToRefY(ref.y()) - yCenterPos)/ vwidth) * metricWidth - x, ((screenToRefX(ref.x()) - xCenterPos) / vwidth) * metricWidth - y);
+    return QPointF(-((screenToRefY(ref.y()) - yCenterPos)/ vwidth) * metricWidth, ((screenToRefX(ref.x()) - xCenterPos) / vwidth) * metricWidth);
 }
 
 QPointF HSIDisplay::refToMetricBody(QPointF &ref)
@@ -488,6 +505,16 @@ QPointF HSIDisplay::metricBodyToRef(QPointF &metric)
     return QPointF(((metric.y())/ metricWidth) * vwidth + xCenterPos, ((-metric.x()) / metricWidth) * vwidth + yCenterPos);
 }
 
+double HSIDisplay::metricToRef(double metric)
+{
+    return (metric / metricWidth) * vwidth;
+}
+
+double HSIDisplay::refToMetric(double ref)
+{
+    return (ref/vwidth) * metricWidth;
+}
+
 QPointF HSIDisplay::metricBodyToScreen(QPointF metric)
 {
     QPointF ref = metricBodyToRef(metric);
@@ -498,30 +525,55 @@ QPointF HSIDisplay::metricBodyToScreen(QPointF metric)
 
 void HSIDisplay::mouseDoubleClickEvent(QMouseEvent * event)
 {
-    static bool dragStarted;
-    static float startX;
-
-    if (event->MouseButtonDblClick) {
-        //setBodySetpointCoordinateXY(-refToMetric(screenToRefY(event->y()) - yCenterPos), refToMetric(screenToRefX(event->x()) - xCenterPos));
-
+    if (event->type() == QMouseEvent::MouseButtonDblClick)
+    {
         QPointF p = screenToMetricBody(event->posF());
         setBodySetpointCoordinateXY(p.x(), p.y());
         qDebug() << "Double click at x: " << screenToRefX(event->x()) - xCenterPos << "y:" << screenToRefY(event->y()) - yCenterPos;
-    } else if (event->MouseButtonPress) {
-        startX = event->globalX();
-        if (event->button() == Qt::RightButton) {
+    }
+}
+
+void HSIDisplay::mouseReleaseEvent(QMouseEvent * event)
+{
+    if (event->type() == QMouseEvent::MouseButtonRelease && event->button() == Qt::RightButton)
+    {
+        qDebug() << "YAW CHANGED" << uiYawSet;
+        setBodySetpointCoordinateYaw(uiYawSet);
+        dragStarted = false;
+    }
+}
+
+void HSIDisplay::mousePressEvent(QMouseEvent * event)
+{
+    if (event->type() == QMouseEvent::MouseButtonPress)
+    {
+        if (event->button() == Qt::RightButton)
+        {
+            startX = event->x();
             // Start tracking mouse move
             dragStarted = true;
-        } else if (event->button() == Qt::LeftButton) {
+            qDebug() << "DRAG STARTED";
+        }
+        else if (event->button() == Qt::LeftButton)
+        {
 
         }
-    } else if (event->MouseButtonRelease) {
-        dragStarted = false;
-    } else if (event->MouseMove) {
-        if (dragStarted) uiYawSet += (startX - event->globalX()) / this->frameSize().width();
     }
 }
 
+void HSIDisplay::mouseMoveEvent(QMouseEvent * event)
+{
+    if (event->type() == QMouseEvent::MouseMove)
+    {
+        if (dragStarted) uiYawSet += 0.5*(startX - event->x()) / this->frameSize().width();
+    }
+}
+
+void HSIDisplay::contextMenuEvent (QContextMenuEvent* event)
+{
+    event->ignore();
+}
+
 void HSIDisplay::setMetricWidth(double width)
 {
     if (width != metricWidth) {
@@ -593,6 +645,7 @@ void HSIDisplay::updateSpeed(UASInterface* uas, double vx, double vy, double vz,
 
 void HSIDisplay::setBodySetpointCoordinateXY(double x, double y)
 {
+    userSetPointSet = true;
     // Set coordinates and send them out to MAV
 
     QPointF sp(x, y);
@@ -602,7 +655,9 @@ void HSIDisplay::setBodySetpointCoordinateXY(double x, double y)
 
     qDebug() << "Attempting to set new setpoint at x: " << x << "metric y:" << y;
 
-    if (uas && mavInitialized) {
+    if (uas && mavInitialized)
+    {
+        uiZSetCoordinate = -0.65;
         uas->setLocalPositionSetpoint(uiXSetCoordinate, uiYSetCoordinate, uiZSetCoordinate, uiYawSet);
         qDebug() << "Setting new setpoint at x: " << x << "metric y:" << y;
     }
@@ -610,8 +665,19 @@ void HSIDisplay::setBodySetpointCoordinateXY(double x, double y)
 
 void HSIDisplay::setBodySetpointCoordinateZ(double z)
 {
+    userSetPointSet = true;
     // Set coordinates and send them out to MAV
     uiZSetCoordinate = z;
+    uas->setLocalPositionSetpoint(uiXSetCoordinate, uiYSetCoordinate, uiZSetCoordinate, uiYawSet);
+}
+
+void HSIDisplay::setBodySetpointCoordinateYaw(double yaw)
+{
+    userSetPointSet = true;
+    // Set coordinates and send them out to MAV
+    uiYawSet = atan2(sin(yaw), cos(yaw));
+    qDebug() << "YAW IN" << yaw << "YAW OUT" << uiYawSet;
+    uas->setLocalPositionSetpoint(uiXSetCoordinate, uiYSetCoordinate, uiZSetCoordinate, uiYawSet);
 }
 
 void HSIDisplay::sendBodySetPointCoordinates()
@@ -751,7 +817,7 @@ QColor HSIDisplay::getColorForSNR(float snr)
 void HSIDisplay::drawSetpointXY(float x, float y, float yaw, const QColor &color, QPainter &painter)
 {
     if (setPointKnown) {
-        float radius = vwidth / 20.0f;
+        float radius = vwidth / 18.0f;
         QPen pen(color);
         pen.setWidthF(refLineWidthToPen(0.4f));
         pen.setColor(color);
@@ -762,9 +828,23 @@ void HSIDisplay::drawSetpointXY(float x, float y, float yaw, const QColor &color
         in = metricWorldToBody(in);
         // Scale from metric to screen reference coordinates
         QPointF p = metricBodyToRef(in);
-        drawCircle(p.x(), p.y(), radius, 0.4f, color, &painter);
+        //drawCircle(p.x(), p.y(), radius, 0.4f, color, &painter);
+
+        QPolygonF poly(4);
+        // Top point
+        poly.replace(0, QPointF(p.x()-radius, p.y()-radius));
+        // Right point
+        poly.replace(1, QPointF(p.x()+radius, p.y()-radius));
+        // Bottom point
+        poly.replace(2, QPointF(p.x()+radius, p.y()+radius));
+        poly.replace(3, QPointF(p.x()-radius, p.y()+radius));
+
+        drawPolygon(poly, &painter);
+
         radius *= 0.8f;
-        drawLine(p.x(), p.y(), p.x()+sin(yaw) * radius, p.y()-cos(yaw) * radius, refLineWidthToPen(0.4f), color, &painter);
+        drawLine(p.x(), p.y(), p.x()+cos(yaw) * radius, p.y()-sin(yaw) * radius, refLineWidthToPen(0.4f), color, &painter);
+
+        // Draw center dot
         painter.setBrush(color);
         drawCircle(p.x(), p.y(), radius * 0.1f, 0.1f, color, &painter);
     }
@@ -826,25 +906,27 @@ void HSIDisplay::drawWaypoints(QPainter& painter)
 
             // Select color based on if this is the current waypoint
             if (list.at(i)->getCurrent()) {
-                color = QGC::colorCyan;//uas->getColor();
+                color = QGC::colorYellow;//uas->getColor();
                 pen.setWidthF(refLineWidthToPen(0.8f));
             } else {
-                color = uas->getColor();
+                color = QGC::colorCyan;
                 pen.setWidthF(refLineWidthToPen(0.4f));
             }
 
             pen.setColor(color);
             painter.setPen(pen);
             float radius = (waypointSize/2.0f) * 0.8 * (1/sqrt(2.0f));
-            drawLine(p.x(), p.y(), p.x()+sin(list.at(i)->getYaw()+yaw) * radius, p.y()-cos(list.at(i)->getYaw()+yaw) * radius, refLineWidthToPen(0.4f), color, &painter);
+            drawLine(p.x(), p.y(), p.x()+sin(list.at(i)->getYaw()/180.0*M_PI-yaw) * radius, p.y()-cos(list.at(i)->getYaw()/180.0*M_PI-yaw) * radius, refLineWidthToPen(0.4f), color, &painter);
             drawPolygon(poly, &painter);
+            float acceptRadius = list.at(i)->getAcceptanceRadius();
+            drawCircle(p.x(), p.y(), metricToRef(acceptRadius), 1.0, Qt::green, &painter);
 
             // DRAW CONNECTING LINE
             // Draw line from last waypoint to this one
             if (!lastWaypoint.isNull()) {
                 pen.setWidthF(refLineWidthToPen(0.4f));
                 painter.setPen(pen);
-                color = uas->getColor();
+                color = QGC::colorCyan;
                 drawLine(lastWaypoint.x(), lastWaypoint.y(), p.x(), p.y(), refLineWidthToPen(0.4f), color, &painter);
             }
             lastWaypoint = p;

src/ui/HSIDisplay.h

@@ -101,6 +101,7 @@ protected slots:
     /** @brief Draw a position lock indicator */
     void drawPositionLock(float x, float y, QString label, int status, bool known, QPainter& painter);
     void setBodySetpointCoordinateXY(double x, double y);
+    void setBodySetpointCoordinateYaw(double yaw);
     void setBodySetpointCoordinateZ(double z);
     /** @brief Send the current ui setpoint coordinates as new setpoint to the MAV */
     void sendBodySetPointCoordinates();
@@ -112,8 +113,13 @@ protected slots:
     void drawSafetyArea(const QPointF &topLeft, const QPointF &bottomRight,  const QColor &color, QPainter &painter);
     /** @brief Receive mouse clicks */
     void mouseDoubleClickEvent(QMouseEvent* event);
+    void mousePressEvent(QMouseEvent * event);
+    void mouseReleaseEvent(QMouseEvent * event);
+    void mouseMoveEvent(QMouseEvent * event);
     /** @brief Receive mouse wheel events */
     void wheelEvent(QWheelEvent* event);
+    /** @brief Ignore context menu event */
+    void contextMenuEvent (QContextMenuEvent* event);
 
 protected:
 
@@ -131,6 +137,10 @@ protected:
     QPointF refToMetricBody(QPointF &ref);
     /** @brief Metric coordinates to reference coordinates */
     QPointF metricBodyToRef(QPointF &metric);
+    /** @brief Metric length to reference coordinates */
+    double metricToRef(double metric);
+    /** @bried Reference coordinates to metric length */
+    double refToMetric(double ref);
     /** @brief Metric body coordinates to screen coordinates */
     QPointF metricBodyToScreen(QPointF metric);
     QMap<int, QString> objectNames;
@@ -138,6 +148,8 @@ protected:
     QMap<int, float> objectQualities;
     QMap<int, float> objectBearings;
     QMap<int, float> objectDistances;
+    bool dragStarted;
+    float startX;
 
     /**
      * @brief Private data container class to be used within the HSI widget

src/ui/Linechart.ui

@@ -6,8 +6,8 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>662</width>
-    <height>381</height>
+    <width>833</width>
+    <height>585</height>
    </rect>
   </property>
   <property name="sizePolicy">
@@ -25,23 +25,8 @@
   <property name="windowTitle">
    <string>Form</string>
   </property>
-  <layout class="QHBoxLayout" name="horizontalLayout_2" stretch="10,20">
-   <property name="spacing">
-    <number>3</number>
-   </property>
-   <property name="leftMargin">
-    <number>2</number>
-   </property>
-   <property name="topMargin">
-    <number>0</number>
-   </property>
-   <property name="rightMargin">
-    <number>2</number>
-   </property>
-   <property name="bottomMargin">
-    <number>2</number>
-   </property>
-   <item>
+  <layout class="QGridLayout" name="gridLayout" columnstretch="5,5,5,5,100">
+   <item row="0" column="0" colspan="4">
     <widget class="QGroupBox" name="curveGroupBox">
      <property name="sizePolicy">
       <sizepolicy hsizetype="Expanding" vsizetype="MinimumExpanding">
@@ -114,8 +99,8 @@
           <rect>
            <x>0</x>
            <y>0</y>
-           <width>210</width>
-           <height>361</height>
+           <width>232</width>
+           <height>516</height>
           </rect>
          </property>
         </widget>
@@ -124,7 +109,7 @@
      </layout>
     </widget>
    </item>
-   <item>
+   <item row="0" column="4" rowspan="2">
     <widget class="QGroupBox" name="diagramGroupBox">
      <property name="sizePolicy">
       <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
@@ -149,6 +134,33 @@
      </property>
     </widget>
    </item>
+   <item row="1" column="0">
+    <widget class="QCheckBox" name="shortNameCheckBox">
+     <property name="text">
+      <string>Short Names</string>
+     </property>
+    </widget>
+   </item>
+   <item row="1" column="2" colspan="2">
+    <widget class="QPushButton" name="recolorButton">
+     <property name="text">
+      <string>Recolor</string>
+     </property>
+    </widget>
+   </item>
+   <item row="1" column="1">
+    <spacer name="horizontalSpacer">
+     <property name="orientation">
+      <enum>Qt::Horizontal</enum>
+     </property>
+     <property name="sizeHint" stdset="0">
+      <size>
+       <width>40</width>
+       <height>20</height>
+      </size>
+     </property>
+    </spacer>
+   </item>
   </layout>
  </widget>
  <resources/>

src/ui/QGCToolBar.cc

@@ -64,7 +64,7 @@ QGCToolBar::QGCToolBar(QWidget *parent) :
     toolBarBatteryBar->setMinimum(0);
     toolBarBatteryBar->setMaximum(100);
     toolBarBatteryBar->setMinimumWidth(20);
-    toolBarBatteryBar->setMaximumWidth(200);
+    toolBarBatteryBar->setMaximumWidth(100);
     toolBarBatteryVoltageLabel = new QLabel("xx.x V");
     toolBarBatteryVoltageLabel->setStyleSheet(QString("QLabel { margin: 0px 0px 0px 4px; font: 14px; color: %1; }").arg(QColor(Qt::green).name()));
     symbolButton->setStyleSheet("QWidget { background-color: #050508; color: #DDDDDF; background-clip: border; } QToolButton { font-weight: bold; font-size: 12px; border: 0px solid #999999; border-radius: 5px; min-width:22px; max-width: 22px; min-height: 22px; max-height: 22px; padding: 0px; margin: 0px 0px 0px 20px; background-color: none; }");

src/ui/WaypointList.cc

@@ -210,7 +210,7 @@ void WaypointList::addCurrentPositonWaypoint()
                 yawGlobal = last->getYaw();
             }
             // Create global frame waypoint per default
-            wp = new Waypoint(0, uas->getLatitude(), uas->getLongitude(), uas->getAltitude(), yawGlobal, acceptanceRadiusGlobal, holdTime, 0.0, true, true, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_NAV_WAYPOINT);
+            wp = new Waypoint(0, uas->getLatitude(), uas->getLongitude(), uas->getAltitude(), 0, acceptanceRadiusGlobal, holdTime, yawGlobal, true, true, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_NAV_WAYPOINT);
             uas->getWaypointManager()->addWaypoint(wp);
         }
         else if (uas->localPositionKnown())

src/ui/designer/QGCCommandButton.cc

@@ -53,6 +53,7 @@ QGCCommandButton::QGCCommandButton(QWidget *parent) :
     ui->editCommandComboBox->addItem("CUSTOM 13", 13);
     ui->editCommandComboBox->addItem("CUSTOM 14", 14);
     ui->editCommandComboBox->addItem("CUSTOM 15", 15);
+    ui->editCommandComboBox->addItem("NAV_WAYPOINT", 16);
     ui->editCommandComboBox->setEditable(true);
 }
 
@@ -75,6 +76,7 @@ void QGCCommandButton::sendCommand()
         int component = ui->editComponentSpinBox->value();
 
         QGCToolWidgetItem::uas->executeCommand(command, confirm, param1, param2, param3, param4, component);
+        qDebug() << __FILE__ << __LINE__ << "SENDING COMMAND" << index;
     } else {
         qDebug() << __FILE__ << __LINE__ << "NO UAS SET, DOING NOTHING";
     }

src/ui/linechart/LinechartPlot.cc

@@ -127,6 +127,7 @@ LinechartPlot::LinechartPlot(QWidget *parent, int plotid, quint64 interval): Qwt
     //updateTimer->start(DEFAULT_REFRESH_RATE);
 
     connect(&timeoutTimer, SIGNAL(timeout()), this, SLOT(removeTimedOutCurves()));
+    //timeoutTimer.start(5000);
 }
 
 LinechartPlot::~LinechartPlot()
@@ -208,7 +209,7 @@ void LinechartPlot::removeTimedOutCurves()
     foreach(QString key, lastUpdate.keys())
     {
         quint64 time = lastUpdate.value(key);
-        if (QGC::groundTimeMilliseconds() - time > 20000)
+        if (QGC::groundTimeMilliseconds() - time > 10000)
         {
             // Remove this curve
             // Delete curves
@@ -227,6 +228,7 @@ void LinechartPlot::removeTimedOutCurves()
             delete d;
             // Set the pointer null
             d = NULL;
+            emit curveRemoved(key);
         }
     }
 }

src/ui/linechart/LinechartPlot.h

@@ -269,6 +269,17 @@ public slots:
     void setAverageWindow(int windowSize);
     void removeTimedOutCurves();
 
+    /** @brief Reset color map */
+    void shuffleColors()
+    {
+        foreach (QwtPlotCurve* curve, curves)
+        {
+            QPen pen(curve->pen());
+            pen.setColor(getNextColor());
+            curve->setPen(pen);
+        }
+    }
+
     public:
     QColor getColorForCurve(QString id);
 

src/ui/linechart/LinechartWidget.cc

@@ -75,7 +75,7 @@ LinechartWidget::LinechartWidget(int systemid, QWidget *parent) : QWidget(parent
 {
     // Add elements defined in Qt Designer
     ui.setupUi(this);
-    this->setMinimumSize(300, 200);
+    this->setMinimumSize(200, 150);
 
     // Add and customize curve list elements (left side)
     curvesWidget = new QWidget(ui.curveListWidget);
@@ -103,7 +103,8 @@ LinechartWidget::LinechartWidget(int systemid, QWidget *parent) : QWidget(parent
     QLabel* mean;
     QLabel* variance;
 
-    //horizontalLayout->addWidget(checkBox);
+    connect(ui.recolorButton, SIGNAL(clicked()), this, SLOT(recolor()));
+    connect(ui.shortNameCheckBox, SIGNAL(clicked(bool)), this, SLOT(setShortNames(bool)));
 
     int labelRow = curvesWidgetLayout->rowCount();
 
@@ -169,6 +170,7 @@ void LinechartWidget::writeSettings()
     settings.beginGroup("LINECHART");
     if (timeButton) settings.setValue("ENFORCE_GROUNDTIME", timeButton->isChecked());
     if (unitsCheckBox) settings.setValue("SHOW_UNITS", unitsCheckBox->isChecked());
+    if (ui.shortNameCheckBox) settings.setValue("SHORT_NAMES", ui.shortNameCheckBox->isChecked());
     settings.endGroup();
     settings.sync();
 }
@@ -182,7 +184,8 @@ void LinechartWidget::readSettings()
         timeButton->setChecked(settings.value("ENFORCE_GROUNDTIME", timeButton->isChecked()).toBool());
         activePlot->enforceGroundTime(settings.value("ENFORCE_GROUNDTIME", timeButton->isChecked()).toBool());
     }
-    if (unitsCheckBox) unitsCheckBox->setChecked(settings.value("SHOW_UNITS").toBool());
+    if (unitsCheckBox) unitsCheckBox->setChecked(settings.value("SHOW_UNITS", unitsCheckBox->isChecked()).toBool());
+    if (ui.shortNameCheckBox) ui.shortNameCheckBox->setChecked(settings.value("SHORT_NAMES", ui.shortNameCheckBox->isChecked()).toBool());
     settings.endGroup();
 }
 
@@ -279,7 +282,8 @@ void LinechartWidget::createLayout()
     connect(this, SIGNAL(curveRemoved(QString)), activePlot, SLOT(hideCurve(QString)));
 
     // Update scrollbar when plot window changes (via translator method setPlotWindowPosition()
-    connect(activePlot, SIGNAL(windowPositionChanged(quint64)), this, SLOT(setPlotWindowPosition(quint64)));
+//    connect(activePlot, SIGNAL(windowPositionChanged(quint64)), this, SLOT(setPlotWindowPosition(quint64)));
+    connect(activePlot, SIGNAL(curveRemoved(QString)), this, SLOT(removeCurve(QString)));
 
     // Update plot when scrollbar is moved (via translator method setPlotWindowPosition()
     connect(this, SIGNAL(plotWindowPositionUpdated(quint64)), activePlot, SLOT(setWindowPosition(quint64)));
@@ -557,6 +561,8 @@ void LinechartWidget::addCurve(const QString& curve, const QString& unit)
     QLabel* mean;
     QLabel* variance;
 
+    curveNames.insert(curve+unit, curve);
+
     int labelRow = curvesWidgetLayout->rowCount();
 
     checkBox = new QCheckBox(this);
@@ -585,6 +591,9 @@ void LinechartWidget::addCurve(const QString& curve, const QString& unit)
     colorIcon->setStyleSheet(colorstyle);
     colorIcon->setAutoFillBackground(true);
 
+    // Label
+    curveNameLabels.insert(curve+unit, label);
+
     // Value
     value = new QLabel(this);
     value->setNum(0.00);
@@ -663,8 +672,96 @@ void LinechartWidget::addCurve(const QString& curve, const QString& unit)
 void LinechartWidget::removeCurve(QString curve)
 {
     Q_UNUSED(curve)
-    //TODO @todo Ensure that the button for a curve gets deleted when the original curve is deleted
-    // Remove name
+
+    QWidget* widget = NULL;
+    widget = curveLabels->take(curve);
+    curvesWidgetLayout->removeWidget(widget);
+    widget->deleteLater();
+    widget = curveMeans->take(curve);
+    curvesWidgetLayout->removeWidget(widget);
+    widget->deleteLater();
+    widget = curveMedians->take(curve);
+    curvesWidgetLayout->removeWidget(widget);
+    widget->deleteLater();
+    widget = curveVariances->take(curve);
+    curvesWidgetLayout->removeWidget(widget);
+    widget->deleteLater();
+//    widget = colorIcons->take(curve);
+//    curvesWidgetLayout->removeWidget(colorIcons->take(curve));
+    widget->deleteLater();
+//    intData->remove(curve);
+}
+
+void LinechartWidget::recolor()
+{
+    activePlot->shuffleColors();
+
+    foreach (QString key, colorIcons.keys())
+    {
+
+        // FIXME
+//        if (activePlot)
+        QString colorstyle;
+        QColor color = activePlot->getColorForCurve(key);
+        colorstyle = colorstyle.sprintf("QWidget { background-color: #%X%X%X; }", color.red(), color.green(), color.blue());
+        QWidget* colorIcon = colorIcons.value(key, 0);
+        if (colorIcon)
+        {
+            colorIcon->setStyleSheet(colorstyle);
+            colorIcon->setAutoFillBackground(true);
+        }
+    }
+}
+
+void LinechartWidget::setShortNames(bool enable)
+{
+    foreach (QString key, curveNames.keys())
+    {
+        QString name;
+        if (enable)
+        {
+            QStringList parts = curveNames.value(key).split(".");
+            if (parts.length() > 1)
+            {
+                name = parts.at(1);
+            }
+            else
+            {
+                name = parts.at(0);
+            }
+
+            const unsigned int sizeLimit = 10;
+
+            // Replace known words with abbreviations
+            if (name.length() > sizeLimit)
+            {
+                name.replace("gyroscope", "gyro");
+                name.replace("accelerometer", "acc");
+                name.replace("magnetometer", "mag");
+                name.replace("distance", "dist");
+                name.replace("altitude", "alt");
+                name.replace("waypoint", "wp");
+                name.replace("error", "err");
+                name.replace("message", "msg");
+                name.replace("source", "src");
+            }
+
+            // Check if sub-part is still exceeding N chars
+            if (name.length() > sizeLimit)
+            {
+                name.replace("a", "");
+                name.replace("e", "");
+                name.replace("i", "");
+                name.replace("o", "");
+                name.replace("u", "");
+            }
+        }
+        else
+        {
+            name = curveNames.value(key);
+        }
+        curveNameLabels.value(key)->setText(name);
+    }
 }
 
 void LinechartWidget::showEvent(QShowEvent* event)

src/ui/linechart/LinechartWidget.h

@@ -73,6 +73,10 @@ public:
 public slots:
     void addCurve(const QString& curve, const QString& unit);
     void removeCurve(QString curve);
+    /** @brief Recolor all curves */
+    void recolor();
+    /** @brief Set short names for curves */
+    void setShortNames(bool enable);
     /** @brief Append data without unit */
     void appendData(int uasId, QString curve, double data, quint64 usec);
     /** @brief Append data with unit */
@@ -119,6 +123,8 @@ protected:
     int curveListCounter;                 ///< Counter of curves in curve list
     QList<QString>* listedCurves;         ///< Curves listed
     QMap<QString, QLabel*>* curveLabels;  ///< References to the curve labels
+    QMap<QString, QLabel*> curveNameLabels;  ///< References to the curve labels
+    QMap<QString, QString> curveNames;    ///< Full curve names
     QMap<QString, QLabel*>* curveMeans;   ///< References to the curve means
     QMap<QString, QLabel*>* curveMedians; ///< References to the curve medians
     QMap<QString, QLabel*>* curveVariances; ///< References to the curve variances

src/ui/linechart/QGCLineChartCurveLabel.cc

@@ -0,0 +1,7 @@
+#include "QGCLineChartCurveLabel.h"
+
+QGCLineChartCurveLabel::QGCLineChartCurveLabel(QWidget *parent) :
+    QWidget(parent)
+{
+
+}

src/ui/linechart/QGCLineChartCurveLabel.h

@@ -0,0 +1,18 @@
+#ifndef QGCLINECHARTCURVELABEL_H
+#define QGCLINECHARTCURVELABEL_H
+
+#include <QWidget>
+
+class QGCLineChartCurveLabel : public QWidget
+{
+    Q_OBJECT
+public:
+    explicit QGCLineChartCurveLabel(QWidget *parent = 0);
+
+signals:
+
+public slots:
+
+};
+
+#endif // QGCLINECHARTCURVELABEL_H

src/ui/uas/UASListWidget.cc

@@ -113,8 +113,8 @@ void UASListWidget::activeUAS(UASInterface* uas)
 
 void UASListWidget::removeUAS(UASInterface* uas)
 {
-    uasViews.remove(uas);
-    listLayout->removeWidget(uasViews.value(uas));
-    uasViews.value(uas)->deleteLater();
+//    uasViews.remove(uas);
+//    listLayout->removeWidget(uasViews.value(uas));
+//    uasViews.value(uas)->deleteLater();
 }