Improvements across application

src/QGC.cc

@@ -22,7 +22,6 @@ This file is part of the QGROUNDCONTROL project
 ======================================================================*/
 
 #include "QGC.h"
-
 #include <qmath.h>
 #include <float.h>
 

src/QGC.h

@@ -69,7 +69,7 @@ const QColor colorRed(154, 20, 20);
 const QColor colorGreen(20, 200, 20);
 const QColor colorYellow(255, 255, 0);
 const QColor colorOrange(255, 140, 0);
-const QColor colorMagenta(255, 0, 65);
+const QColor colorMagenta(255, 0, 55);
 const QColor colorDarkWhite(240, 240, 240);
 const QColor colorDarkYellow(180, 180, 0);
 const QColor colorBackground("#050508");

src/uas/UAS.cc

@@ -909,13 +909,13 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
         {
             mavlink_mission_count_t wpc;
             mavlink_msg_mission_count_decode(&message, &wpc);
-            if (wpc.target_system == mavlink->getSystemId())
+            if(wpc.target_system == mavlink->getSystemId() || wpc.target_system == 0)
             {
                 waypointManager.handleWaypointCount(message.sysid, message.compid, wpc.count);
             }
             else
             {
-                qDebug() << "Got waypoint message, but was not for me";
+                qDebug() << "Got waypoint message, but was wrong system id" << wpc.target_system;
             }
         }
             break;
@@ -925,13 +925,13 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
             mavlink_mission_item_t wp;
             mavlink_msg_mission_item_decode(&message, &wp);
             //qDebug() << "got waypoint (" << wp.seq << ") from ID " << message.sysid << " x=" << wp.x << " y=" << wp.y << " z=" << wp.z;
-            if(wp.target_system == mavlink->getSystemId())
+            if(wp.target_system == mavlink->getSystemId() || wp.target_system == 0)
             {
                 waypointManager.handleWaypoint(message.sysid, message.compid, &wp);
             }
             else
             {
-                qDebug() << "Got waypoint message, but was not for me";
+                qDebug() << "Got waypoint message, but was wrong system id" << wp.target_system;
             }
         }
             break;
@@ -940,7 +940,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
         {
             mavlink_mission_ack_t wpa;
             mavlink_msg_mission_ack_decode(&message, &wpa);
-            if(wpa.target_system == mavlink->getSystemId() && wpa.target_component == mavlink->getComponentId())
+            if((wpa.target_system == mavlink->getSystemId() || wpa.target_system == 0) &&
+                    (wpa.target_component == mavlink->getComponentId() || wpa.target_component == 0))
             {
                 waypointManager.handleWaypointAck(message.sysid, message.compid, &wpa);
             }
@@ -951,13 +952,13 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
         {
             mavlink_mission_request_t wpr;
             mavlink_msg_mission_request_decode(&message, &wpr);
-            if(wpr.target_system == mavlink->getSystemId())
+            if(wpr.target_system == mavlink->getSystemId() || wpr.target_system == 0)
             {
                 waypointManager.handleWaypointRequest(message.sysid, message.compid, &wpr);
             }
             else
             {
-                qDebug() << "Got waypoint message, but was not for me";
+                qDebug() << "Got waypoint message, but was wrong system id" << wpr.target_system;
             }
         }
             break;

src/ui/HSIDisplay.cc

@@ -1116,18 +1116,67 @@ void HSIDisplay::drawSetpointXYZYaw(float x, float y, float z, float yaw, const
     }
 }
 
+void HSIDisplay::drawWaypoint(QPainter& painter, const QColor& color, float width, const QVector<Waypoint*>& list, int i, const QPointF& p)
+{
+    painter.setBrush(Qt::NoBrush);
+
+    // Setup pen for background
+    QPen penBg(color);
+    penBg.setWidthF(width*2.0f);
+
+    // Setup pen for foreground
+    QPen pen(color);
+    pen.setWidthF(width);
+
+    // DRAW WAYPOINT
+    float waypointSize = vwidth / 20.0f * 2.0f;
+    QPolygonF poly(4);
+    // Top point
+    poly.replace(0, QPointF(p.x(), p.y()-waypointSize/2.0f));
+    // Right point
+    poly.replace(1, QPointF(p.x()+waypointSize/2.0f, p.y()));
+    // Bottom point
+    poly.replace(2, QPointF(p.x(), p.y() + waypointSize/2.0f));
+    poly.replace(3, QPointF(p.x() - waypointSize/2.0f, p.y()));
+
+    float radius = (waypointSize/2.0f) * 0.8 * (1/sqrt(2.0f));
+    float acceptRadius = list.at(i)->getAcceptanceRadius();
+
+    // Draw background
+    pen.setColor(Qt::black);
+    painter.setPen(penBg);
+    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);
+    drawCircle(p.x(), p.y(), metricToRef(acceptRadius), 1.0, Qt::black, &painter);
+
+    // Draw foreground
+    pen.setColor(color);
+    painter.setPen(pen);
+    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);
+    drawCircle(p.x(), p.y(), metricToRef(acceptRadius), 1.0, Qt::green, &painter);
+}
+
 void HSIDisplay::drawWaypoints(QPainter& painter)
 {
     if (uas)
     {
         const QVector<Waypoint*>& list = uas->getWaypointManager()->getWaypointEditableList();
 
+        // Do not work on empty lists
+        if (list.size() == 0) return;
+
         QColor color;
         painter.setBrush(Qt::NoBrush);
 
+        // XXX Ugly hacks, needs rewrite
+
         QPointF lastWaypoint;
+        QPointF currentWaypoint;
+        int currentIndex = 0;
 
-        for (int i = 0; i < list.size(); i++) {
+        for (int i = 0; i < list.size(); i++)
+        {
             QPointF in;
             if (list.at(i)->getFrame() == MAV_FRAME_LOCAL_NED)
             {
@@ -1144,42 +1193,22 @@ void HSIDisplay::drawWaypoints(QPainter& painter)
             // Scale from metric to screen reference coordinates
             QPointF p = metricBodyToRef(in);
 
-            // Setup pen
-            QPen pen(color);
-            painter.setBrush(Qt::NoBrush);
-
-            // DRAW WAYPOINT
-            float waypointSize = vwidth / 20.0f * 2.0f;
-            QPolygonF poly(4);
-            // Top point
-            poly.replace(0, QPointF(p.x(), p.y()-waypointSize/2.0f));
-            // Right point
-            poly.replace(1, QPointF(p.x()+waypointSize/2.0f, p.y()));
-            // Bottom point
-            poly.replace(2, QPointF(p.x(), p.y() + waypointSize/2.0f));
-            poly.replace(3, QPointF(p.x() - waypointSize/2.0f, p.y()));
-
             // Select color based on if this is the current waypoint
-            if (list.at(i)->getCurrent()) {
-                color = QGC::colorYellow;//uas->getColor();
-                pen.setWidthF(refLineWidthToPen(0.8f));
-            } else {
-                color = QGC::colorCyan;
-                pen.setWidthF(refLineWidthToPen(0.4f));
+            if (list.at(i)->getCurrent())
+            {
+                currentIndex = i;
+                currentWaypoint = p;
+            }
+            else
+            {
+                drawWaypoint(painter, QGC::colorCyan, refLineWidthToPen(0.4f), list, i, p);
             }
-
-            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()/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())
             {
+                QPen pen(QGC::colorCyan);
                 pen.setWidthF(refLineWidthToPen(0.4f));
                 painter.setPen(pen);
                 color = QGC::colorCyan;
@@ -1187,6 +1216,8 @@ void HSIDisplay::drawWaypoints(QPainter& painter)
             }
             lastWaypoint = p;
         }
+
+        drawWaypoint(painter, QGC::colorYellow, refLineWidthToPen(0.8f), list, currentIndex, currentWaypoint);
     }
 }
 

src/ui/HSIDisplay.h

@@ -120,6 +120,8 @@ protected slots:
     void drawSetpointXYZYaw(float x, float y, float z, float yaw, const QColor &color, QPainter &painter);
     /** @brief Draw waypoints of this system */
     void drawWaypoints(QPainter& painter);
+    /** @brief Draw one waypoint */
+    void drawWaypoint(QPainter& painter, const QColor& color, float width, const QVector<Waypoint*>& list, int i, const QPointF& p);
     /** @brief Draw the limiting safety area */
     void drawSafetyArea(const QPointF &topLeft, const QPointF &bottomRight,  const QColor &color, QPainter &painter);
     /** @brief Receive mouse clicks */

src/ui/MAVLinkDecoder.cc

@@ -41,6 +41,7 @@ MAVLinkDecoder::MAVLinkDecoder(MAVLinkProtocol* protocol, QObject *parent) :
     textMessageFilter.insert(MAVLINK_MSG_ID_DEBUG_VECT, false);
     textMessageFilter.insert(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, false);
     textMessageFilter.insert(MAVLINK_MSG_ID_NAMED_VALUE_INT, false);
+//    textMessageFilter.insert(MAVLINK_MSG_ID_HIGHRES_IMU, false);
 
     connect(protocol, SIGNAL(messageReceived(LinkInterface*,mavlink_message_t)), this, SLOT(receiveMessage(LinkInterface*,mavlink_message_t)));
 }
@@ -239,6 +240,13 @@ void MAVLinkDecoder::emitFieldValue(mavlink_message_t* msg, int fieldid, quint64
         name = name.arg(debug.name).arg(fieldName);
         time = getUnixTimeFromMs(msg->sysid, debug.time_boot_ms);
     }
+//    else if (msgid == MAVLINK_MSG_ID_HIGHRES_IMU)
+//    {
+//        mavlink_highres_imu_t d;
+//        mavlink_msg_highres_imu_decode(msg, &d);
+//        name = name.arg(debug.name).arg(fieldName);
+//        time = getUnixTimeFromMs(msg->sysid, debug.time_boot_ms);
+//    }
     else
     {
         name = name.arg(messageInfo[msgid].name, fieldName);