Added right-click option to select MAV, ensured that waypoints get updated properly on switching MAV

src/comm/MAVLinkSimulationLink.cc

@@ -632,7 +632,7 @@ void MAVLinkSimulationLink::mainloop()
         // HEARTBEAT VEHICLE 2
 
         // Pack message and get size of encoded byte string
-        messageSize = mavlink_msg_heartbeat_pack(54, componentId, &msg, MAV_FIXED_WING, MAV_AUTOPILOT_ARDUPILOTMEGA);
+        messageSize = mavlink_msg_heartbeat_pack(54, componentId, &msg, MAV_HELICOPTER, MAV_AUTOPILOT_ARDUPILOTMEGA);
         // Allocate buffer with packet data
         bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
         //add data into datastream

src/comm/MAVLinkSimulationMAV.cc

@@ -50,6 +50,13 @@ void MAVLinkSimulationMAV::mainloop()
 
     //        double xNew = // (nextSPX - previousSPX)
 
+    if (flying)
+    {
+        sys_state = MAV_STATE_ACTIVE;
+        sys_mode = MAV_MODE_AUTO;
+        nav_mode = MAV_NAV_WAYPOINT;
+    }
+
     // 1 Hz execution
     if (timer1Hz <= 0)
     {
@@ -76,17 +83,6 @@ void MAVLinkSimulationMAV::mainloop()
             //float trueyaw = atan2f(xm, ym);
 
             float newYaw = atan2f(xm, ym);
-            // Choose shortest direction
-//            if (yaw - newYaw < -180.0f)
-//            {
-//                yaw = newYaw + 180.0f;
-//            }
-
-//            if (yaw - newYaw > 180.0f)
-//            {
-//                yaw = newYaw - 180.0f;
-//            }
-
 
             if (fabs(yaw - newYaw) < 90)
             {
@@ -152,6 +148,7 @@ void MAVLinkSimulationMAV::mainloop()
         status.status = sys_state;
 
         mavlink_msg_sys_status_encode(systemid, MAV_COMP_ID_IMU, &msg, &status);
+        link->sendMAVLinkMessage(&msg);
         timer10Hz = 5;
     }
 

src/ui/MapWidget.cc

@@ -157,8 +157,8 @@ MapWidget::MapWidget(QWidget *parent) :
     zoomout->setStyleSheet(buttonStyle);
     createPath = new QPushButton(QIcon(":/images/actions/go-bottom.svg"), "", this);
     createPath->setStyleSheet(buttonStyle);
-    clearTracking = new QPushButton(QIcon(""), "", this);
-    clearTracking->setStyleSheet(buttonStyle);
+//    clearTracking = new QPushButton(QIcon(""), "", this);
+//    clearTracking->setStyleSheet(buttonStyle);
     followgps = new QPushButton(QIcon(":/images/actions/system-lock-screen.svg"), "", this);
     followgps->setStyleSheet(buttonStyle);
     QPushButton* goToButton = new QPushButton(QIcon(""), "T", this);
@@ -167,7 +167,7 @@ MapWidget::MapWidget(QWidget *parent) :
     zoomin->setMaximumWidth(30);
     zoomout->setMaximumWidth(30);
     createPath->setMaximumWidth(30);
-    clearTracking->setMaximumWidth(30);
+//    clearTracking->setMaximumWidth(30);
     followgps->setMaximumWidth(30);
     goToButton->setMaximumWidth(30);
 
@@ -185,7 +185,7 @@ MapWidget::MapWidget(QWidget *parent) :
     innerlayout->addWidget(zoomout, 1, 0);
     innerlayout->addWidget(followgps, 2, 0);
     innerlayout->addWidget(createPath, 3, 0);
-    innerlayout->addWidget(clearTracking, 4, 0);
+    //innerlayout->addWidget(clearTracking, 4, 0);
     // Add spacers to compress buttons on the top left
     innerlayout->addItem(new QSpacerItem(0, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 5, 0);
     innerlayout->addItem(new QSpacerItem(0, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 0, 1, 0, 7);
@@ -692,7 +692,7 @@ void MapWidget::redoWaypoints(int uas)
     // Get waypoint list for this MAV
 
     // Clear all waypoints
-    clearWaypoints();
+    //clearWaypoints();
     // Re-add the updated waypoints
 
     //    }
@@ -714,16 +714,17 @@ void MapWidget::activeUASSet(UASInterface* uas)
     if (uas)
     {
         mav = uas;
-        QColor color = mav->getColor().lighter(100);
-        color.setAlphaF(0.6);
+        QColor color = mav->getColor();
+        color.setAlphaF(0.9);
         QPen* pen = new QPen(color);
-        pen->setWidth(2.0);
+        pen->setWidth(3.0);
         mavPens.insert(mav->getUASID(), pen);
         // FIXME Remove after refactoring
         waypointPath->setPen(pen);
 
         // Delete all waypoints and add waypoint from new system
-        redoWaypoints();
+        //redoWaypoints();
+        updateWaypointList(uas->getUASID());
 
         // Connect the waypoint manager / data storage to the UI
         connect(mav->getWaypointManager(), SIGNAL(waypointListChanged(int)), this, SLOT(updateWaypointList(int)));
@@ -731,6 +732,7 @@ void MapWidget::activeUASSet(UASInterface* uas)
         connect(this, SIGNAL(waypointCreated(Waypoint*)), mav->getWaypointManager(), SLOT(addWaypoint(Waypoint*)));
 
         updateSelectedSystem(mav->getUASID());
+        mc->updateRequest(waypointPath->boundingBox().toRect());
     }
 }
 

src/ui/map/MAV2DIcon.cc

@@ -90,28 +90,25 @@ void MAV2DIcon::drawIcon(QPen* pen)
 //        QPen selPen(color);
 //        int width = 5;
 //        selPen.setWidth(width);
-        painter.setPen(Qt::yellow);
+        QPen pen(Qt::yellow);
+        pen.setWidth(2);
+        painter.setPen(pen);
         painter.drawEllipse(QPoint(0, 0), radius/2-1, radius/2-1);
         //qDebug() << "Painting ellipse" << radius/2-width << width;
         //selPen->deleteLater();
     }
 
-    float yawRotate = (yaw/(float)M_PI)*180.0f + 180.0f+180.0f;
-
-    painter.rotate(yawRotate);
-
-    if (this->name() == "1") qDebug() << "uas icon" << name() << "yaw in:" << yaw << "rotate:" << yawRotate;
-
     switch (type)
     {
-    case MAV_ICON_GENERIC:
     case MAV_ICON_AIRPLANE:
-    case MAV_ICON_QUADROTOR:
-    case MAV_ICON_ROTARY_WING:
-    default:
         {
             // DRAW AIRPLANE
 
+            // Rotate 180 degs more since the icon is oriented downwards
+            float yawRotate = (yaw/(float)M_PI)*180.0f + 180.0f+180.0f;
+
+            painter.rotate(yawRotate);
+
             //qDebug() << "ICON SIZE:" << radius;
 
             float iconSize = radius*0.9f;
@@ -141,30 +138,70 @@ void MAV2DIcon::drawIcon(QPen* pen)
             poly.replace(22, QPointF(-0.031250f*iconSize, -0.262500f*iconSize));
             poly.replace(23, QPointF(0.000000f*iconSize, -0.312500f*iconSize));
 
-            //    // Select color based on if this is the current waypoint
-            //    if (list.at(i)->getCurrent())
-            //    {
-            //        color = QGC::colorCyan;//uas->getColor();
-            //        pen.setWidthF(refLineWidthToPen(0.8f));
-            //    }
-            //    else
-            //    {
-            //        color = uas->getColor();
-            //        pen.setWidthF(refLineWidthToPen(0.4f));
-            //    }
-
-            //pen.setColor(color);
-//            if (pen)
-//            {
-//                pen->setWidthF(2);
-//                painter.setPen(*pen);
-//            }
-//            else
-//            {
-//                QPen pen2(Qt::red);
-//                pen2.setWidth(0);
-//                painter.setPen(pen2);
-//            }
+            painter.setBrush(QBrush(iconColor));
+            QPen iconPen(Qt::black);
+            iconPen.setWidthF(1.0f);
+            painter.setPen(iconPen);
+
+            painter.drawPolygon(poly);
+        }
+        break;
+    case MAV_ICON_QUADROTOR:
+        {
+            // QUADROTOR
+            float iconSize = radius*0.9f;
+            float yawRotate = (yaw/(float)M_PI)*180.0f + 180.0f;
+
+            painter.rotate(yawRotate);
+
+            //qDebug() << "ICON SIZE:" << radius;
+
+            QPointF front(0, 0.2);
+            front = front *iconSize;
+            QPointF left(-0.2, 0);
+            left = left * iconSize;
+            QPointF right(0.2, 0.0);
+            right *= iconSize;
+            QPointF back(0, -0.2);
+            back *= iconSize;
+
+            QPolygonF poly(0);
+
+
+
+            painter.setBrush(QBrush(iconColor));
+            QPen iconPen(Qt::black);
+            iconPen.setWidthF(1.0f);
+            painter.setPen(iconPen);
+
+            painter.drawPolygon(poly);
+
+            painter.drawEllipse(left, radius/4/2, radius/4/2);
+            painter.drawEllipse(right, radius/4/2, radius/4/2);
+            painter.drawEllipse(back, radius/4/2, radius/4/2);
+
+            painter.setBrush(Qt::red);
+            painter.drawEllipse(front, radius/4/2, radius/4/2);
+        }
+        break;
+    case MAV_ICON_ROTARY_WING:
+        case MAV_ICON_GENERIC:
+    default:
+        {
+            // GENERIC
+
+            float yawRotate = (yaw/(float)M_PI)*180.0f + 180.0f;
+
+            painter.rotate(yawRotate);
+
+            //qDebug() << "ICON SIZE:" << radius;
+
+            float iconSize = radius*0.9f;
+            QPolygonF poly(3);
+            poly.replace(0, QPointF(0.0f*iconSize, 0.3f*iconSize));
+            poly.replace(1, QPointF(-0.2f*iconSize, -0.2f*iconSize));
+            poly.replace(2, QPointF(0.2f*iconSize, -0.2f*iconSize));
+
             painter.setBrush(QBrush(iconColor));
             QPen iconPen(Qt::black);
             iconPen.setWidthF(1.0f);

src/ui/uas/UASView.cc

@@ -66,6 +66,7 @@ UASView::UASView(UASInterface* uas, QWidget *parent) :
         localFrame(false),
         removeAction(new QAction("Delete this system", this)),
         renameAction(new QAction("Rename..", this)),
+        selectAction(new QAction("Select this system", this )),
         m_ui(new Ui::UASView)
 {
     m_ui->setupUi(this);
@@ -102,6 +103,7 @@ UASView::UASView(UASInterface* uas, QWidget *parent) :
     // Allow to delete this widget
     connect(removeAction, SIGNAL(triggered()), this, SLOT(deleteLater()));
     connect(renameAction, SIGNAL(triggered()), this, SLOT(rename()));
+    connect(selectAction, SIGNAL(triggered()), uas, SLOT(setSelected()));
     connect(uas, SIGNAL(systemRemoved()), this, SLOT(deleteLater()));
 
     // Name changes
@@ -135,6 +137,9 @@ UASView::UASView(UASInterface* uas, QWidget *parent) :
 UASView::~UASView()
 {
     delete m_ui;
+    delete removeAction;
+    delete renameAction;
+    delete selectAction;
 }
 
 void UASView::heartbeatTimeout()
@@ -413,6 +418,7 @@ void UASView::contextMenuEvent (QContextMenuEvent* event)
     {
         menu.addAction(removeAction);
     }
+    menu.addAction(selectAction);
     menu.exec(event->globalPos());
 }
 

src/ui/uas/UASView.h

@@ -110,6 +110,7 @@ protected:
     bool localFrame;
     QAction* removeAction;
     QAction* renameAction;
+    QAction* selectAction;
     static const int updateInterval = 300;