Added full support for multi-MAV wp handling, cleaned up simulation

qgroundcontrol.pro

@@ -154,7 +154,8 @@ FORMS += src/ui/MainWindow.ui \
     src/ui/designer/QGCToolWidget.ui \
     src/ui/designer/QGCParamSlider.ui \
     src/ui/designer/QGCActionButton.ui \
-    src/ui/QGCMAVLinkLogPlayer.ui
+    src/ui/QGCMAVLinkLogPlayer.ui \
+    src/ui/QGCWaypointListMulti.ui
 
 INCLUDEPATH += src \
     src/ui \
@@ -263,7 +264,8 @@ HEADERS += src/MG.h \
     src/ui/QGCMAVLinkLogPlayer.h \
     src/comm/MAVLinkSimulationWaypointPlanner.h \
     src/comm/MAVLinkSimulationMAV.h \
-    src/uas/QGCMAVLinkUASFactory.h
+    src/uas/QGCMAVLinkUASFactory.h \
+    src/ui/QGCWaypointListMulti.h
 
 # Google Earth is only supported on Mac OS and Windows with Visual Studio Compiler
 macx|win32-msvc2008: {
@@ -388,6 +390,7 @@ SOURCES += src/main.cc \
     src/comm/MAVLinkSimulationWaypointPlanner.cc \
     src/comm/MAVLinkSimulationMAV.cc \
     src/uas/QGCMAVLinkUASFactory.cc \
+    src/ui/QGCWaypointListMulti.cc
 
 macx|win32-msvc2008: {
     SOURCES += src/ui/map3D/QGCGoogleEarthView.cc

src/comm/MAVLinkSimulationLink.cc

@@ -444,16 +444,16 @@ void MAVLinkSimulationLink::mainloop()
         memcpy(stream+streampointer,buffer, bufferlength);
         streampointer += bufferlength;
 
-        // GLOBAL POSITION VEHICLE 2
-        mavlink_msg_global_position_int_pack(54, componentId, &ret, (473780.28137103+(x+0.002))*1E3, (85489.9892510421+((y/2)+0.3))*1E3, (z+570.0)*1000.0, xSpeed, ySpeed, zSpeed);
-        bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
-        //add data into datastream
-        memcpy(stream+streampointer,buffer, bufferlength);
-        streampointer += bufferlength;
+//        // GLOBAL POSITION VEHICLE 2
+//        mavlink_msg_global_position_int_pack(54, componentId, &ret, (473780.28137103+(x+0.002))*1E3, (85489.9892510421+((y/2)+0.3))*1E3, (z+570.0)*1000.0, xSpeed, ySpeed, zSpeed);
+//        bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
+//        //add data into datastream
+//        memcpy(stream+streampointer,buffer, bufferlength);
+//        streampointer += bufferlength;
 
-        // ATTITUDE VEHICLE 2
-        mavlink_msg_attitude_pack(54, MAV_COMP_ID_IMU, &ret, 0, 0, 0, atan2((y/2)+0.3, (x+0.002)), 0, 0, 0);
-        sendMAVLinkMessage(&ret);
+//        // ATTITUDE VEHICLE 2
+//        mavlink_msg_attitude_pack(54, MAV_COMP_ID_IMU, &ret, 0, 0, 0, atan2((y/2)+0.3, (x+0.002)), 0, 0, 0);
+//        sendMAVLinkMessage(&ret);
 
 
 //        // GLOBAL POSITION VEHICLE 3
@@ -629,15 +629,15 @@ void MAVLinkSimulationLink::mainloop()
 
 
 
-        // HEARTBEAT VEHICLE 2
+//        // HEARTBEAT VEHICLE 2
 
-        // Pack message and get size of encoded byte string
-        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
-        memcpy(stream+streampointer,buffer, bufferlength);
-        streampointer += bufferlength;
+//        // Pack message and get size of encoded byte string
+//        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
+//        memcpy(stream+streampointer,buffer, bufferlength);
+//        streampointer += bufferlength;
 
 //        // HEARTBEAT VEHICLE 3
 
@@ -956,8 +956,8 @@ bool MAVLinkSimulationLink::connect()
     emit connected(true);
 
     start(LowPriority);
-    MAVLinkSimulationMAV* mav1 = new MAVLinkSimulationMAV(this, 1);
-    MAVLinkSimulationMAV* mav3 = new MAVLinkSimulationMAV(this, 2);
+    MAVLinkSimulationMAV* mav1 = new MAVLinkSimulationMAV(this, 1, 47.376, 8.548);
+    MAVLinkSimulationMAV* mav2 = new MAVLinkSimulationMAV(this, 2);
     Q_UNUSED(mav1);
     //    timer->start(rate);
     return true;

src/comm/MAVLinkSimulationMAV.cc

@@ -3,7 +3,7 @@
 
 #include "MAVLinkSimulationMAV.h"
 
-MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int systemid) :
+MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int systemid, double lat, double lon) :
         QObject(parent),
         link(parent),
         planner(parent, systemid),
@@ -11,11 +11,11 @@ MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int sy
         timer25Hz(0),
         timer10Hz(0),
         timer1Hz(0),
-        latitude(47.376389),
-        longitude(8.548056),
+        latitude(lat),
+        longitude(lon),
         altitude(0.0),
-        x(8.548056),
-        y(47.376389),
+        x(lon),
+        y(lat),
         z(550),
         roll(0.0),
         pitch(0.0),

src/comm/MAVLinkSimulationMAV.h

@@ -11,7 +11,7 @@ class MAVLinkSimulationMAV : public QObject
 {
     Q_OBJECT
 public:
-    explicit MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int systemid);
+    explicit MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int systemid, double lat=47.376389, double lon=8.548056);
 
 signals:
 

src/uas/UAS.cc

@@ -213,6 +213,21 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
             if (this->type != mavlink_msg_heartbeat_get_type(&message))
             {
                 this->type = mavlink_msg_heartbeat_get_type(&message);
+                if (airframe == 0)
+                {
+                    switch (type)
+                    {
+                    case MAV_FIXED_WING:
+                        setAirframe(UASInterface::QGC_AIRFRAME_EASYSTAR);
+                        break;
+                    case MAV_QUADROTOR:
+                        setAirframe(UASInterface::QGC_AIRFRAME_CHEETAH);
+                        break;
+                    default:
+                        // Do nothing
+                        break;
+                    }
+                }
                 this->autopilot = mavlink_msg_heartbeat_get_autopilot(&message);
                 emit systemTypeSet(this, type);
             }

src/ui/MainWindow.cc

@@ -22,7 +22,7 @@
 #include "UDPLink.h"
 #include "MAVLinkProtocol.h"
 #include "CommConfigurationWindow.h"
-#include "WaypointList.h"
+#include "QGCWaypointListMulti.h"
 #include "MainWindow.h"
 #include "JoystickWidget.h"
 #include "GAudioOutput.h"
@@ -305,7 +305,7 @@ void MainWindow::buildCommonWidgets()
     if (!waypointsDockWidget)
     {
         waypointsDockWidget = new QDockWidget(tr("Waypoint List"), this);
-        waypointsDockWidget->setWidget( new WaypointList(this, NULL) );
+        waypointsDockWidget->setWidget( new QGCWaypointListMulti(this) );
         waypointsDockWidget->setObjectName("WAYPOINT_LIST_DOCKWIDGET");
         addToToolsMenu (waypointsDockWidget, tr("Waypoints List"), SLOT(showToolWidget(bool)), MENU_WAYPOINTS, Qt::BottomDockWidgetArea);
     }

src/ui/QGCWaypointListMulti.cc

@@ -0,0 +1,62 @@
+#include "QGCWaypointListMulti.h"
+#include "ui_QGCWaypointListMulti.h"
+#include "UASManager.h"
+
+QGCWaypointListMulti::QGCWaypointListMulti(QWidget *parent) :
+    QWidget(parent),
+    ui(new Ui::QGCWaypointListMulti)
+{
+    ui->setupUi(this);
+    connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), this, SLOT(systemCreated(UASInterface*)));
+    connect(UASManager::instance(), SIGNAL(activeUASSet(int)), this, SLOT(systemSetActive(int)));
+}
+
+void QGCWaypointListMulti::systemDeleted(QObject* uas)
+{
+    UASInterface* mav = dynamic_cast<UASInterface*>(uas);
+    if (mav)
+    {
+        int id = mav->getUASID();
+        WaypointList* list = lists.value(id, NULL);
+        if (list)
+        {
+            delete list;
+            lists.remove(id);
+        }
+    }
+}
+
+void QGCWaypointListMulti::systemCreated(UASInterface* uas)
+{
+    WaypointList* list = new WaypointList(ui->stackedWidget, uas);
+    lists.insert(uas->getUASID(), list);
+    ui->stackedWidget->addWidget(list);
+    // Ensure widget is deleted when system is deleted
+    connect(uas, SIGNAL(destroyed(QObject*)), this, SLOT(systemDeleted(QObject*)));
+}
+
+void QGCWaypointListMulti::systemSetActive(int uas)
+{
+    WaypointList* list = lists.value(uas, NULL);
+    if (list)
+    {
+        ui->stackedWidget->setCurrentWidget(list);
+    }
+}
+
+QGCWaypointListMulti::~QGCWaypointListMulti()
+{
+    delete ui;
+}
+
+void QGCWaypointListMulti::changeEvent(QEvent *e)
+{
+    QWidget::changeEvent(e);
+    switch (e->type()) {
+    case QEvent::LanguageChange:
+        ui->retranslateUi(this);
+        break;
+    default:
+        break;
+    }
+}

src/ui/QGCWaypointListMulti.h

@@ -0,0 +1,35 @@
+#ifndef QGCWAYPOINTLISTMULTI_H
+#define QGCWAYPOINTLISTMULTI_H
+
+#include <QWidget>
+#include <QMap>
+
+#include "WaypointList.h"
+#include "UASInterface.h"
+
+namespace Ui {
+    class QGCWaypointListMulti;
+}
+
+class QGCWaypointListMulti : public QWidget
+{
+    Q_OBJECT
+
+public:
+    explicit QGCWaypointListMulti(QWidget *parent = 0);
+    ~QGCWaypointListMulti();
+
+public slots:
+    void systemDeleted(QObject* uas);
+    void systemCreated(UASInterface* uas);
+    void systemSetActive(int uas);
+
+protected:
+    void changeEvent(QEvent *e);
+    QMap<int, WaypointList*> lists;
+
+private:
+    Ui::QGCWaypointListMulti *ui;
+};
+
+#endif // QGCWAYPOINTLISTMULTI_H

src/ui/QGCWaypointListMulti.ui

@@ -0,0 +1,27 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>QGCWaypointListMulti</class>
+ <widget class="QWidget" name="QGCWaypointListMulti">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>400</width>
+    <height>300</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>Form</string>
+  </property>
+  <layout class="QHBoxLayout" name="horizontalLayout">
+   <property name="margin">
+    <number>0</number>
+   </property>
+   <item>
+    <widget class="QStackedWidget" name="stackedWidget"/>
+   </item>
+  </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>

src/ui/WaypointList.cc

@@ -57,8 +57,6 @@ WaypointList::WaypointList(QWidget *parent, UASInterface* uas) :
     listLayout->setAlignment(Qt::AlignTop);
     m_ui->listWidget->setLayout(listLayout);
 
-    this->uas = NULL;
-
     // ADD WAYPOINT
     // Connect add action, set right button icon and connect action to this class
     connect(m_ui->addButton, SIGNAL(clicked()), m_ui->actionAddWaypoint, SIGNAL(triggered()));
@@ -77,7 +75,7 @@ WaypointList::WaypointList(QWidget *parent, UASInterface* uas) :
     connect(m_ui->saveButton, SIGNAL(clicked()), this, SLOT(saveWaypoints()));
     connect(m_ui->loadButton, SIGNAL(clicked()), this, SLOT(loadWaypoints()));
 
-    connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setUAS(UASInterface*)));
+    //connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setUAS(UASInterface*)));