Enabled waypoints in simulation, implemented factory class for UAS objects

qgcunittest.pro

@@ -114,7 +114,8 @@ SOURCES +=  src/uas/UAS.cc \
             src/comm/SerialLink.cc \
             $$TESTDIR/SlugsMavUnitTest.cc \
             $$TESTDIR/testSuite.cc \
-            $$TESTDIR/UASUnitTest.cc
+            $$TESTDIR/UASUnitTest.cc \
+    src/uas/QGCMAVLinkUASFactory.cc
 
 
 HEADERS += src/uas/UASInterface.h \
@@ -136,7 +137,8 @@ HEADERS += src/uas/UASInterface.h \
             src/comm/SerialLink.h \
             $$TESTDIR//SlugsMavUnitTest.h \
             $$TESTDIR/AutoTest.h \
-            $$TESTDIR/UASUnitTest.h
+            $$TESTDIR/UASUnitTest.h \
+    src/uas/QGCMAVLinkUASFactory.h
 
 
 

qgroundcontrol.pro

@@ -262,7 +262,8 @@ HEADERS += src/MG.h \
     src/ui/designer/QGCToolWidgetItem.h \
     src/ui/QGCMAVLinkLogPlayer.h \
     src/comm/MAVLinkSimulationWaypointPlanner.h \
-    src/comm/MAVLinkSimulationMAV.h
+    src/comm/MAVLinkSimulationMAV.h \
+    src/uas/QGCMAVLinkUASFactory.h
 
 # Google Earth is only supported on Mac OS and Windows with Visual Studio Compiler
 macx|win32-msvc2008: {
@@ -385,7 +386,8 @@ SOURCES += src/main.cc \
     src/ui/designer/QGCToolWidgetItem.cc \
     src/ui/QGCMAVLinkLogPlayer.cc \
     src/comm/MAVLinkSimulationWaypointPlanner.cc \
-    src/comm/MAVLinkSimulationMAV.cc
+    src/comm/MAVLinkSimulationMAV.cc \
+    src/uas/QGCMAVLinkUASFactory.cc \
 
 macx|win32-msvc2008: {
     SOURCES += src/ui/map3D/QGCGoogleEarthView.cc

src/GAudioOutput.cc

@@ -153,7 +153,7 @@ void GAudioOutput::mute(bool mute)
 {
     this->muted = mute;
     QSettings settings;
-    settings.setValue(QGC_GAUDIOOUTPUT_KEY+"muted", muted);
+    settings.setValue(QGC_GAUDIOOUTPUT_KEY+"muted", this->muted);
     settings.sync();
 }
 

src/Waypoint.cc

@@ -55,7 +55,12 @@ Waypoint::~Waypoint()
 
 void Waypoint::save(QTextStream &saveStream)
 {
-    saveStream << this->getId() << "\t" << this->getFrame() << "\t" << this->getAction() << "\t"  << this->getOrbit() << "\t" << /*Orbit Direction*/ 0 << "\t" << this->getOrbit() << "\t" << this->getHoldTime() << "\t" << this->getCurrent() << "\t" << this->getX() << "\t" << this->getY()  << "\t" << this->getZ()  << "\t" << this->getYaw()  << "\t" << this->getAutoContinue() << "\r\n";
+    QString position("%1\t%2\t%3\t%4");
+    position = position.arg(x, 0, 'g', 18);
+    position = position.arg(y, 0, 'g', 18);
+    position = position.arg(z, 0, 'g', 18);
+    position = position.arg(yaw, 0, 'g', 8);
+    saveStream << this->getId() << "\t" << this->getFrame() << "\t" << this->getAction() << "\t"  << this->getOrbit() << "\t" << /*Orbit Direction*/ 0 << "\t" << this->getOrbit() << "\t" << this->getHoldTime() << "\t" << this->getCurrent() << "\t" << position  << "\t" << this->getAutoContinue() << "\r\n";
 }
 
 bool Waypoint::load(QTextStream &loadStream)

src/comm/MAVLinkProtocol.cc

@@ -27,7 +27,8 @@
 #include "configuration.h"
 #include "LinkManager.h"
 //#include "MainWindow.h"
-#include <QGCMAVLink.h>
+#include "QGCMAVLink.h"
+#include "QGCMAVLinkUASFactory.h"
 #include "QGC.h"
 
 /**
@@ -169,63 +170,8 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
                     continue;
                 }
 
-                switch (heartbeat.autopilot)
-                {
-                case MAV_AUTOPILOT_GENERIC:
-
-                    uas = new UAS(this, message.sysid);
-
-                    // Connect this robot to the UAS object
-                    connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), uas, SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
-                    break;
-                case MAV_AUTOPILOT_PIXHAWK:
-                    {
-                        // Fixme differentiate between quadrotor and coaxial here
-                        PxQuadMAV* mav = new PxQuadMAV(this, message.sysid);
-                        // Connect this robot to the UAS object
-                        // it is IMPORTANT here to use the right object type,
-                        // else the slot of the parent object is called (and thus the special
-                        // packets never reach their goal)
-                        connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
-                        uas = mav;
-                    }
-                    break;
-                case MAV_AUTOPILOT_SLUGS:
-                    {
-                        SlugsMAV* mav = new SlugsMAV(this, message.sysid);
-                        // Connect this robot to the UAS object
-                        // it is IMPORTANT here to use the right object type,
-                        // else the slot of the parent object is called (and thus the special
-                        // packets never reach their goal)
-                        connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
-                        uas = mav;
-                    }
-                    break;
-                case MAV_AUTOPILOT_ARDUPILOTMEGA:
-                    {
-                        ArduPilotMegaMAV* mav = new ArduPilotMegaMAV(this, message.sysid);
-                        // Connect this robot to the UAS object
-                        // it is IMPORTANT here to use the right object type,
-                        // else the slot of the parent object is called (and thus the special
-                        // packets never reach their goal)
-                        connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
-                        uas = mav;
-                    }
-                    break;
-                default:
-                    uas = new UAS(this, message.sysid);
-                    break;
-                }
-
-                // Set the autopilot type
-                uas->setAutopilotType((int)heartbeat.autopilot);
-
-                // Make UAS aware that this link can be used to communicate with the actual robot
-                uas->addLink(link);
-
-                // Now add UAS to "official" list, which makes the whole application aware of it
-                UASManager::instance()->addUAS(uas);
-
+                // Create a new UAS object
+                uas = QGCMAVLinkUASFactory::createUAS(this, link, message.sysid, &heartbeat);
             }
 
             // Only count message if UAS exists for this message

src/comm/MAVLinkSimulationLink.cc

@@ -153,14 +153,14 @@ void MAVLinkSimulationLink::sendMAVLinkMessage(const mavlink_message_t* msg)
     unsigned int bufferlength = mavlink_msg_to_send_buffer(buf, msg);
 
     // Pack to link buffer
-    readyBufferMutex.lock();
+//    readyBufferMutex.lock();
     for (unsigned int i = 0; i < bufferlength; i++)
     {
         readyBuffer.enqueue(*(buf + i));
     }
-    readyBufferMutex.unlock();
+//    readyBufferMutex.unlock();
 
-    //qDebug() << "SENT MAVLINK MESSAGE FROM SYSTEM" << msg->sysid << "COMP" << msg->compid;
+    qDebug() << "SENT MAVLINK MESSAGE FROM SYSTEM" << msg->sysid << "COMP" << msg->compid;
 }
 
 void MAVLinkSimulationLink::enqueue(uint8_t* stream, uint8_t* index, mavlink_message_t* msg)

src/comm/MAVLinkSimulationMAV.cc

@@ -1,4 +1,5 @@
 #include <QDebug>
+#include <cmath>
 
 #include "MAVLinkSimulationMAV.h"
 
@@ -6,28 +7,126 @@ MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int sy
     QObject(parent),
     link(parent),
     planner(parent, systemid),
-    systemid(systemid)
+    systemid(systemid),
+    timer25Hz(0),
+    timer10Hz(0),
+    timer1Hz(0),
+    latitude(0.0),
+    longitude(0.0),
+    altitude(0.0),
+    x(0.0),
+    y(0.0),
+    z(0.0),
+    roll(0.0),
+    pitch(0.0),
+    yaw(0.0),
+    globalNavigation(true),
+    firstWP(false),
+    previousSPX(0.0),
+    previousSPY(0.0),
+    previousSPZ(0.0),
+    previousSPYaw(0.0),
+    nextSPX(0.0),
+    nextSPY(0.0),
+    nextSPZ(0.0),
+    nextSPYaw(0.0)
 {
+    // Please note: The waypoint planner is running
     connect(&mainloopTimer, SIGNAL(timeout()), this, SLOT(mainloop()));
-    mainloopTimer.start(1000);
+    connect(&planner, SIGNAL(messageSent(mavlink_message_t)), this, SLOT(handleMessage(mavlink_message_t)));
     connect(link, SIGNAL(messageReceived(mavlink_message_t)), this, SLOT(handleMessage(mavlink_message_t)));
+    mainloopTimer.start(20);
     mainloop();
 }
 
 void MAVLinkSimulationMAV::mainloop()
 {
-    mavlink_message_t msg;
-    mavlink_msg_heartbeat_pack(systemid, MAV_COMP_ID_IMU, &msg, MAV_FIXED_WING, MAV_AUTOPILOT_PIXHAWK);
-    link->sendMAVLinkMessage(&msg);
+    // Calculate new simulator values
+//    double maxSpeed = 0.0001; // rad/s in earth coordinate frame
+
+//        double xNew = // (nextSPX - previousSPX)
+
+    if (!firstWP)
+    {
+        x += (nextSPX - previousSPX) * 0.01;
+        y += (nextSPY - previousSPY) * 0.01;
+        z += (nextSPZ - previousSPZ) * 0.1;
+    }
+    else
+    {
+        x = nextSPX;
+        y = nextSPY;
+        z = nextSPZ;
+        firstWP = false;
+    }
+
+    // 1 Hz execution
+    if (timer1Hz <= 0)
+    {
+        mavlink_message_t msg;
+        mavlink_msg_heartbeat_pack(systemid, MAV_COMP_ID_IMU, &msg, MAV_FIXED_WING, MAV_AUTOPILOT_PIXHAWK);
+        link->sendMAVLinkMessage(&msg);
+        timer1Hz = 50;
+    }
+
+    // 10 Hz execution
+    if (timer10Hz <= 0)
+    {
+        mavlink_message_t msg;
+        mavlink_global_position_int_t pos;
+        pos.alt = z*1000.0;
+        pos.lat = y*1E7;
+        pos.lon = x*1E7;
+        pos.vx = 0;
+        pos.vy = 0;
+        pos.vz = 0;
+        mavlink_msg_global_position_int_encode(systemid, MAV_COMP_ID_IMU, &msg, &pos);
+        link->sendMAVLinkMessage(&msg);
+        timer10Hz = 5;
+    }
+
+    // 25 Hz execution
+    if (timer25Hz <= 0)
+    {
+        timer25Hz = 2;
+    }
+
+    timer1Hz--;
+    timer10Hz--;
+    timer25Hz--;
 }
 
 void MAVLinkSimulationMAV::handleMessage(const mavlink_message_t& msg)
 {
-    qDebug() << "MAV:" << systemid << "RECEIVED MESSAGE FROM" << msg.sysid << "COMP" << msg.compid;
+    //qDebug() << "MAV:" << systemid << "RECEIVED MESSAGE FROM" << msg.sysid << "COMP" << msg.compid;
 
     switch(msg.msgid)
     {
     case MAVLINK_MSG_ID_ATTITUDE:
         break;
+    case MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET:
+        {
+            mavlink_local_position_setpoint_set_t sp;
+            mavlink_msg_local_position_setpoint_set_decode(&msg, &sp);
+            if (sp.target_system == this->systemid)
+            {
+                previousSPX = nextSPX;
+                previousSPY = nextSPY;
+                previousSPZ = nextSPZ;
+                nextSPX = sp.x;
+                nextSPY = sp.y;
+                nextSPZ = sp.z;
+
+                // Rotary wing
+                //nextSPYaw = sp.yaw;
+
+                // Airplane
+                yaw = atan2(previousSPX-nextSPX, previousSPY-nextSPY);
+
+                if (!firstWP) firstWP = true;
+            }
+            //qDebug() << "UPDATED SP:" << "X" << nextSPX << "Y" << nextSPY << "Z" << nextSPZ;
+        }
+        break;
     }
 }

src/comm/MAVLinkSimulationMAV.h

@@ -24,7 +24,31 @@ protected:
     MAVLinkSimulationWaypointPlanner planner;
     int systemid;
     QTimer mainloopTimer;
-
+    int timer25Hz;
+    int timer10Hz;
+    int timer1Hz;
+    double latitude;
+    double longitude;
+    double altitude;
+    double x;
+    double y;
+    double z;
+    double roll;
+    double pitch;
+    double yaw;
+
+    bool globalNavigation;
+    bool firstWP;
+
+    double previousSPX;
+    double previousSPY;
+    double previousSPZ;
+    double previousSPYaw;
+
+    double nextSPX;
+    double nextSPY;
+    double nextSPZ;
+    double nextSPYaw;
 
 };
 

src/comm/MAVLinkSimulationWaypointPlanner.cc

@@ -34,6 +34,7 @@ This file is part of the PIXHAWK project
 
 #include "MAVLinkSimulationWaypointPlanner.h"
 #include "QGC.h"
+#include <QDebug>
 
 #ifndef M_PI
 #define M_PI 3.14159265358979323846
@@ -225,7 +226,7 @@ void MAVLinkSimulationWaypointPlanner::send_waypoint_ack(uint8_t target_systemid
 
 
 
-        if (verbose) printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system);
+        if (verbose) qDebug("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system);
 }
 
 /*
@@ -253,7 +254,7 @@ void MAVLinkSimulationWaypointPlanner::send_waypoint_current(uint16_t seq)
 
 
 
-                if (verbose) printf("Broadcasted new current waypoint %u\n", wpc.seq);
+                if (verbose) qDebug("Broadcasted new current waypoint %u\n", wpc.seq);
         }
 }
 
@@ -291,10 +292,20 @@ void MAVLinkSimulationWaypointPlanner::send_setpoint(uint16_t seq)
                 }
                 else
                 {
-                        if (verbose) printf("No new set point sent to IMU because the new waypoint %u had no local coordinates\n", cur->seq);
+                        //if (verbose) qDebug("No new set point sent to IMU because the new waypoint %u had no local coordinates\n", cur->seq);
+                        PControlSetPoint.target_system = systemid;
+                        PControlSetPoint.target_component = MAV_COMP_ID_IMU;
+                        PControlSetPoint.x = cur->x;
+                        PControlSetPoint.y = cur->y;
+                        PControlSetPoint.z = cur->z;
+                        PControlSetPoint.yaw = cur->yaw;
+
+                        mavlink_msg_local_position_setpoint_set_encode(systemid, compid, &msg, &PControlSetPoint);
+                        link->sendMAVLinkMessage(&msg);
+                        emit messageSent(msg);
                 }
 
-                uint64_t now = QGC::groundTimeUsecs();
+                uint64_t now = QGC::groundTimeUsecs()/1000;
                 timestamp_last_send_setpoint = now;
         }
 }
@@ -311,7 +322,7 @@ void MAVLinkSimulationWaypointPlanner::send_waypoint_count(uint8_t target_system
         mavlink_msg_waypoint_count_encode(systemid, compid, &msg, &wpc);
         link->sendMAVLinkMessage(&msg);
 
-        if (verbose) printf("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system);
+        if (verbose) qDebug("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system);
 
 
 }
@@ -325,17 +336,17 @@ void MAVLinkSimulationWaypointPlanner::send_waypoint(uint8_t target_systemid, ui
                 wp->target_system = target_systemid;
                 wp->target_component = target_compid;
 
-                if (verbose) printf("Sent waypoint %u (%u / %u / %u / %u / %u / %f / %u / %f / %f / %u / %f / %f / %f / %f / %u)\n", wp->seq, wp->target_system, wp->target_component, wp->seq, wp->frame, wp->action, wp->orbit, wp->orbit_direction, wp->param1, wp->param2, wp->current, wp->x, wp->y, wp->z, wp->yaw, wp->autocontinue);
+                if (verbose) qDebug("Sent waypoint %u (%u / %u / %u / %u / %u / %f / %u / %f / %f / %u / %f / %f / %f / %f / %u)\n", wp->seq, wp->target_system, wp->target_component, wp->seq, wp->frame, wp->action, wp->orbit, wp->orbit_direction, wp->param1, wp->param2, wp->current, wp->x, wp->y, wp->z, wp->yaw, wp->autocontinue);
 
                 mavlink_msg_waypoint_encode(systemid, compid, &msg, wp);
                 link->sendMAVLinkMessage(&msg);
-                if (verbose) printf("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system);
+                if (verbose) qDebug("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system);
 
 
         }
         else
         {
-                if (verbose) printf("ERROR: index out of bounds\n");
+                if (verbose) qDebug("ERROR: index out of bounds\n");
         }
 }
 
@@ -348,7 +359,7 @@ void MAVLinkSimulationWaypointPlanner::send_waypoint_request(uint8_t target_syst
         wpr.seq = seq;
         mavlink_msg_waypoint_request_encode(systemid, compid, &msg, &wpr);
         link->sendMAVLinkMessage(&msg);
-        if (verbose) printf("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system);
+        if (verbose) qDebug("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system);
 
 
 }
@@ -370,7 +381,7 @@ void MAVLinkSimulationWaypointPlanner::send_waypoint_reached(uint16_t seq)
         mavlink_msg_waypoint_reached_encode(systemid, compid, &msg, &wp_reached);
         link->sendMAVLinkMessage(&msg);
 
-        if (verbose) printf("Sent waypoint %u reached message\n", wp_reached.seq);
+        if (verbose) qDebug("Sent waypoint %u reached message\n", wp_reached.seq);
 
 
 }
@@ -438,12 +449,12 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
 
         //check for timed-out operations
 
-        printf("MAV: %d WAYPOINTPLANNER GOT MESSAGE\n", systemid);
+        qDebug() << "MAV: %d WAYPOINTPLANNER GOT MESSAGE" << systemid;
 
-        uint64_t now = QGC::groundTimeUsecs();
+        uint64_t now = QGC::groundTimeUsecs()/1000;
         if (now-protocol_timestamp_lastaction > protocol_timeout && current_state != PX_WPP_IDLE)
         {
-                if (verbose) printf("Last operation (state=%u) timed out, changing state to PX_WPP_IDLE\n", current_state);
+                if (verbose) qDebug() << "Last operation (state=%u) timed out, changing state to PX_WPP_IDLE" << current_state;
                 current_state = PX_WPP_IDLE;
                 protocol_current_count = 0;
                 protocol_current_partner_systemid = 0;
@@ -506,7 +517,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
                                 {
                                         mavlink_local_position_t pos;
                                         mavlink_msg_local_position_decode(msg, &pos);
-                                        if (debug) printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z);
+                                        qDebug() << "Received new position: x:" << pos.x << "| y:" << pos.y << "| z:" << pos.z;
 
                                         posReached = false;
 
@@ -538,7 +549,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
                         mavlink_msg_action_decode(msg, &action);
                         if(action.target == systemid)
                         {
-                                if (verbose) printf("Waypoint: received message with action %d\n", action.action);
+                                if (verbose) qDebug("Waypoint: received message with action %d\n", action.action);
                                 switch (action.action)
                                 {
 //				case MAV_ACTION_LAUNCH:
@@ -585,7 +596,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
                                 {
                                         if (protocol_current_wp_id == waypoints->size()-1)
                                         {
-                                                if (verbose) printf("Received Ack after having sent last waypoint, going to state PX_WPP_IDLE\n");
+                                                if (verbose) qDebug("Received Ack after having sent last waypoint, going to state PX_WPP_IDLE\n");
                                                 current_state = PX_WPP_IDLE;
                                                 protocol_current_wp_id = 0;
                                         }
@@ -607,7 +618,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
                                 {
                                         if (wpc.seq < waypoints->size())
                                         {
-                                                if (verbose) printf("Received MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT\n");
+                                                if (verbose) qDebug("Received MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT\n");
                                                 current_active_wp_id = wpc.seq;
                                                 uint32_t i;
                                                 for(i = 0; i < waypoints->size(); i++)
@@ -621,7 +632,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
                                                                 waypoints->at(i)->current = false;
                                                         }
                                                 }
-                                                if (verbose) printf("New current waypoint %u\n", current_active_wp_id);
+                                                if (verbose) qDebug("New current waypoint %u\n", current_active_wp_id);
                                                 yawReached = false;
                                                 posReached = false;
                                                 send_waypoint_current(current_active_wp_id);
@@ -630,7 +641,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
                                         }
                                         else
                                         {
-                                                if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: Index out of bounds\n");
+                                                if (verbose) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: Index out of bounds\n");
                                         }
                                 }
                         }
@@ -649,8 +660,8 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
                                 {
                                         if (waypoints->size() > 0)
                                         {
-                                                if (verbose && current_state == PX_WPP_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u changing state to PX_WPP_SENDLIST\n", msg->sysid);
-                                                if (verbose && current_state == PX_WPP_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST again from %u staying in state PX_WPP_SENDLIST\n", msg->sysid);
+                                                if (verbose && current_state == PX_WPP_IDLE) qDebug("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u changing state to PX_WPP_SENDLIST\n", msg->sysid);
+                                                if (verbose && current_state == PX_WPP_SENDLIST) qDebug("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST again from %u staying in state PX_WPP_SENDLIST\n", msg->sysid);
                                                 current_state = PX_WPP_SENDLIST;
                                                 protocol_current_wp_id = 0;
                                                 protocol_current_partner_systemid = msg->sysid;
@@ -658,19 +669,19 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
                                         }
                                         else
                                         {
-                                                if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid);
+                                                if (verbose) qDebug("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid);
                                         }
                                         protocol_current_count = waypoints->size();
                                         send_waypoint_count(msg->sysid,msg->compid, protocol_current_count);
                                 }
                                 else
                                 {
-                                        if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST because i'm doing something else already (state=%i).\n", current_state);
+                                        if (verbose) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST because i'm doing something else already (state=%i).\n", current_state);
                                 }
                         }
                         else
                         {
-                                if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST because not my systemid or compid.\n");
+                                if (verbose) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST because not my systemid or compid.\n");
                         }
                         break;
                 }
@@ -686,9 +697,9 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
                                 //ensure that we are in the correct state and that the first request has id 0 and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint)
                                 if ((current_state == PX_WPP_SENDLIST && wpr.seq == 0) || (current_state == PX_WPP_SENDLIST_SENDWPS && (wpr.seq == protocol_current_wp_id || wpr.seq == protocol_current_wp_id + 1) && wpr.seq < waypoints->size()))
                                 {
-                                        if (verbose && current_state == PX_WPP_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u changing state to PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
-                                        if (verbose && current_state == PX_WPP_SENDLIST_SENDWPS && wpr.seq == protocol_current_wp_id + 1) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u staying in state PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
-                                        if (verbose && current_state == PX_WPP_SENDLIST_SENDWPS && wpr.seq == protocol_current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u (again) from %u staying in state PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
+                                        if (verbose && current_state == PX_WPP_SENDLIST) qDebug("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u changing state to PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
+                                        if (verbose && current_state == PX_WPP_SENDLIST_SENDWPS && wpr.seq == protocol_current_wp_id + 1) qDebug("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u staying in state PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
+                                        if (verbose && current_state == PX_WPP_SENDLIST_SENDWPS && wpr.seq == protocol_current_wp_id) qDebug("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u (again) from %u staying in state PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
 
                                         current_state = PX_WPP_SENDLIST_SENDWPS;
                                         protocol_current_wp_id = wpr.seq;
@@ -698,17 +709,17 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
                                 {
                                         if (verbose)
                                         {
-                                                if (!(current_state == PX_WPP_SENDLIST || current_state == PX_WPP_SENDLIST_SENDWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because i'm doing something else already (state=%i).\n", current_state); break; }
+                                                if (!(current_state == PX_WPP_SENDLIST || current_state == PX_WPP_SENDLIST_SENDWPS)) { qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because i'm doing something else already (state=%i).\n", current_state); break; }
                                                 else if (current_state == PX_WPP_SENDLIST)
                                                 {
-                                                        if (wpr.seq != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq);
+                                                        if (wpr.seq != 0) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq);
                                                 }
                                                 else if (current_state == PX_WPP_SENDLIST_SENDWPS)
                                                 {
-                                                        if (wpr.seq != protocol_current_wp_id && wpr.seq != protocol_current_wp_id + 1) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, protocol_current_wp_id, protocol_current_wp_id+1);
-                                                        else if (wpr.seq >= waypoints->size()) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq);
+                                                        if (wpr.seq != protocol_current_wp_id && wpr.seq != protocol_current_wp_id + 1) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, protocol_current_wp_id, protocol_current_wp_id+1);
+                                                        else if (wpr.seq >= waypoints->size()) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq);
                                                 }
-                                                else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST - FIXME: missed error description\n");
+                                                else qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST - FIXME: missed error description\n");
                                         }
                                 }
                         }
@@ -717,7 +728,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
                                 //we we're target but already communicating with someone else
                                 if((wpr.target_system == systemid && wpr.target_component == compid) && !(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid))
                                 {
-                                        if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, protocol_current_partner_systemid);
+                                        if (verbose) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, protocol_current_partner_systemid);
                                 }
                         }
                         break;
@@ -735,8 +746,8 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
                                 {
                                         if (wpc.count > 0)
                                         {
-                                                if (verbose && current_state == PX_WPP_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) from %u changing state to PX_WPP_GETLIST\n", wpc.count, msg->sysid);
-                                                if (verbose && current_state == PX_WPP_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) again from %u\n", wpc.count, msg->sysid);
+                                                if (verbose && current_state == PX_WPP_IDLE) qDebug("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) from %u changing state to PX_WPP_GETLIST\n", wpc.count, msg->sysid);
+                                                if (verbose && current_state == PX_WPP_GETLIST) qDebug("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) again from %u\n", wpc.count, msg->sysid);
 
                                                 current_state = PX_WPP_GETLIST;
                                                 protocol_current_wp_id = 0;
@@ -744,7 +755,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
                                                 protocol_current_partner_compid = msg->compid;
                                                 protocol_current_count = wpc.count;
 
-                                                printf("clearing receive buffer and readying for receiving waypoints\n");
+                                                qDebug("clearing receive buffer and readying for receiving waypoints\n");
                                                 while(waypoints_receive_buffer->size() > 0)
                                                 {
                                                         delete waypoints_receive_buffer->back();
@@ -755,14 +766,14 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
                                         }
                                         else
                                         {
-                                                if (verbose) printf("Ignoring MAVLINK_MSG_ID_WAYPOINT_COUNT from %u with count of %u\n", msg->sysid, wpc.count);
+                                                if (verbose) qDebug("Ignoring MAVLINK_MSG_ID_WAYPOINT_COUNT from %u with count of %u\n", msg->sysid, wpc.count);
                                         }
                                 }
                                 else
                                 {
-                                        if (verbose && !(current_state == PX_WPP_IDLE || current_state == PX_WPP_GETLIST)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm doing something else already (state=%i).\n", current_state);
-                                        else if (verbose && current_state == PX_WPP_GETLIST && protocol_current_wp_id != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm already receiving waypoint %u.\n", protocol_current_wp_id);
-                                        else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT - FIXME: missed error description\n");
+                                        if (verbose && !(current_state == PX_WPP_IDLE || current_state == PX_WPP_GETLIST)) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm doing something else already (state=%i).\n", current_state);
+                                        else if (verbose && current_state == PX_WPP_GETLIST && protocol_current_wp_id != 0) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm already receiving waypoint %u.\n", protocol_current_wp_id);
+                                        else qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT - FIXME: missed error description\n");
                                 }
                         }
                         break;
@@ -780,9 +791,9 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
                                 //ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids
                                 if ((current_state == PX_WPP_GETLIST && wp.seq == 0) || (current_state == PX_WPP_GETLIST_GETWPS && wp.seq == protocol_current_wp_id && wp.seq < protocol_current_count))
                                 {
-                                        if (verbose && current_state == PX_WPP_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u changing state to PX_WPP_GETLIST_GETWPS\n", wp.seq, msg->sysid);
-                                        if (verbose && current_state == PX_WPP_GETLIST_GETWPS && wp.seq == protocol_current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u\n", wp.seq, msg->sysid);
-                                        if (verbose && current_state == PX_WPP_GETLIST_GETWPS && wp.seq-1 == protocol_current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u (again) from %u\n", wp.seq, msg->sysid);
+                                        if (verbose && current_state == PX_WPP_GETLIST) qDebug("Got MAVLINK_MSG_ID_WAYPOINT %u from %u changing state to PX_WPP_GETLIST_GETWPS\n", wp.seq, msg->sysid);
+                                        if (verbose && current_state == PX_WPP_GETLIST_GETWPS && wp.seq == protocol_current_wp_id) qDebug("Got MAVLINK_MSG_ID_WAYPOINT %u from %u\n", wp.seq, msg->sysid);
+                                        if (verbose && current_state == PX_WPP_GETLIST_GETWPS && wp.seq-1 == protocol_current_wp_id) qDebug("Got MAVLINK_MSG_ID_WAYPOINT %u (again) from %u\n", wp.seq, msg->sysid);
 
                                         current_state = PX_WPP_GETLIST_GETWPS;
                                         protocol_current_wp_id = wp.seq + 1;
@@ -792,7 +803,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
 
                                         if(protocol_current_wp_id == protocol_current_count && current_state == PX_WPP_GETLIST_GETWPS)
                                         {
-                                                if (verbose) printf("Got all %u waypoints, changing state to PX_WPP_IDLE\n", protocol_current_count);
+                                                if (verbose) qDebug("Got all %u waypoints, changing state to PX_WPP_IDLE\n", protocol_current_count);
 
                                                 send_waypoint_ack(protocol_current_partner_systemid, protocol_current_partner_compid, 0);
 
@@ -813,7 +824,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
                                                         if (waypoints->at(i)->current == 1)
                                                         {
                                                                 current_active_wp_id = i;
-                                                                //if (verbose) printf("New current waypoint %u\n", current_active_wp_id);
+                                                                //if (verbose) qDebug("New current waypoint %u\n", current_active_wp_id);
                                                                 yawReached = false;
                                                                 posReached = false;
                                                                 send_waypoint_current(current_active_wp_id);
@@ -844,36 +855,36 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
                                         {
                                                 //we're done receiving waypoints, answer with ack.
                                                 send_waypoint_ack(protocol_current_partner_systemid, protocol_current_partner_compid, 0);
-                                                printf("Received MAVLINK_MSG_ID_WAYPOINT while state=PX_WPP_IDLE, answered with WAYPOINT_ACK.\n");
+                                                qDebug("Received MAVLINK_MSG_ID_WAYPOINT while state=PX_WPP_IDLE, answered with WAYPOINT_ACK.\n");
                                         }
                                         if (verbose)
                                         {
-                                                if (!(current_state == PX_WPP_GETLIST || current_state == PX_WPP_GETLIST_GETWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u because i'm doing something else already (state=%i).\n", wp.seq, current_state); break; }
+                                                if (!(current_state == PX_WPP_GETLIST || current_state == PX_WPP_GETLIST_GETWPS)) { qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT %u because i'm doing something else already (state=%i).\n", wp.seq, current_state); break; }
                                                 else if (current_state == PX_WPP_GETLIST)
                                                 {
-                                                        if(!(wp.seq == 0)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the first waypoint ID (%u) was not 0.\n", wp.seq);
-                                                        else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq);
+                                                        if(!(wp.seq == 0)) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT because the first waypoint ID (%u) was not 0.\n", wp.seq);
+                                                        else qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq);
                                                 }
                                                 else if (current_state == PX_WPP_GETLIST_GETWPS)
                                                 {
-                                                        if (!(wp.seq == protocol_current_wp_id)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was not the expected %u.\n", wp.seq, protocol_current_wp_id);
-                                                        else if (!(wp.seq < protocol_current_count)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was out of bounds.\n", wp.seq);
-                                                        else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq);
+                                                        if (!(wp.seq == protocol_current_wp_id)) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was not the expected %u.\n", wp.seq, protocol_current_wp_id);
+                                                        else if (!(wp.seq < protocol_current_count)) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was out of bounds.\n", wp.seq);
+                                                        else qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq);
                                                 }
-                                                else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq);
+                                                else qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq);
                                         }
                                 }
                         }
                         else
                         {
-                                //we we're target but already communicating with someone else
+                                // We're target but already communicating with someone else
                                 if((wp.target_system == systemid && wp.target_component == compid) && !(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && current_state != PX_WPP_IDLE)
                                 {
-                                        if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, protocol_current_partner_systemid);
+                                        if (verbose) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, protocol_current_partner_systemid);
                                 }
                                 else if(wp.target_system == systemid && wp.target_component == compid)
                                 {
-                                        if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid);
+                                        if (verbose) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid);
                                 }
                         }
                         break;
@@ -888,7 +899,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
                         {
                                 protocol_timestamp_lastaction = now;
 
-                                if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid);
+                                if (verbose) qDebug("Got MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid);
                                 while(waypoints->size() > 0)
                                 {
                                         delete waypoints->back();
@@ -898,14 +909,14 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
                         }
                         else if (wpca.target_system == systemid && wpca.target_component == compid && current_state != PX_WPP_IDLE)
                         {
-                                if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, current_state);
+                                if (verbose) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, current_state);
                         }
                         break;
                 }
 
                 default:
                 {
-                        if (debug) printf("Waypoint: received message of unknown type\n");
+                        if (debug) qDebug("Waypoint: received message of unknown type\n");
                         break;
                 }
         }
@@ -920,7 +931,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
                         if (timestamp_firstinside_orbit == 0)
                         {
                                 // Announce that last waypoint was reached
-                                if (verbose) printf("*** Reached waypoint %u ***\n", cur_wp->seq);
+                                if (verbose) qDebug("*** Reached waypoint %u ***\n", cur_wp->seq);
                                 send_waypoint_reached(cur_wp->seq);
                                 timestamp_firstinside_orbit = now;
                         }
@@ -950,7 +961,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
                                         waypoints->at(current_active_wp_id)->current = true;
                                         posReached = false;
                                         //yawReached = false;
-                                        if (verbose) printf("Set new waypoint (%u)\n", current_active_wp_id);
+                                        if (verbose) qDebug("Set new waypoint (%u)\n", current_active_wp_id);
                                 }
                         }
                 }

src/comm/MAVLinkSimulationWaypointPlanner.h

@@ -24,6 +24,7 @@ public:
     explicit MAVLinkSimulationWaypointPlanner(MAVLinkSimulationLink *parent, int systemid);
 
 signals:
+    void messageSent(const mavlink_message_t& msg);
 
 public slots:
     void handleMessage(const mavlink_message_t& msg);

src/uas/QGCMAVLinkUASFactory.cc

@@ -0,0 +1,99 @@
+#include "QGCMAVLinkUASFactory.h"
+#include "UASManager.h"
+
+QGCMAVLinkUASFactory::QGCMAVLinkUASFactory(QObject *parent) :
+    QObject(parent)
+{
+}
+
+UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInterface* link, int sysid, mavlink_heartbeat_t* heartbeat, QObject* parent)
+{
+    QPointer<QObject> p;
+
+    if (parent != NULL)
+    {
+        p = parent;
+    }
+    else
+    {
+        p = mavlink;
+    }
+
+    UASInterface* uas;
+
+    switch (heartbeat->autopilot)
+    {
+    case MAV_AUTOPILOT_GENERIC:
+        {
+        UAS* mav = new UAS(mavlink, sysid);
+        // Set the system type
+        mav->setSystemType((int)heartbeat->type);
+        // Connect this robot to the UAS object
+        connect(mavlink, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
+        uas = mav;
+        }
+        break;
+    case MAV_AUTOPILOT_PIXHAWK:
+        {
+            PxQuadMAV* mav = new PxQuadMAV(mavlink, sysid);
+            // Set the system type
+            mav->setSystemType((int)heartbeat->type);
+            // Connect this robot to the UAS object
+            // it is IMPORTANT here to use the right object type,
+            // else the slot of the parent object is called (and thus the special
+            // packets never reach their goal)
+            connect(mavlink, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
+            uas = mav;
+        }
+        break;
+    case MAV_AUTOPILOT_SLUGS:
+        {
+            SlugsMAV* mav = new SlugsMAV(mavlink, sysid);
+            // Set the system type
+            mav->setSystemType((int)heartbeat->type);
+            // Connect this robot to the UAS object
+            // it is IMPORTANT here to use the right object type,
+            // else the slot of the parent object is called (and thus the special
+            // packets never reach their goal)
+            connect(mavlink, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
+            uas = mav;
+        }
+        break;
+    case MAV_AUTOPILOT_ARDUPILOTMEGA:
+        {
+            ArduPilotMegaMAV* mav = new ArduPilotMegaMAV(mavlink, sysid);
+            // Set the system type
+            mav->setSystemType((int)heartbeat->type);
+            // Connect this robot to the UAS object
+            // it is IMPORTANT here to use the right object type,
+            // else the slot of the parent object is called (and thus the special
+            // packets never reach their goal)
+            connect(mavlink, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
+            uas = mav;
+        }
+        break;
+    default:
+        {
+            UAS* mav = new UAS(mavlink, sysid);
+            mav->setSystemType((int)heartbeat->type);
+            // Connect this robot to the UAS object
+            // it is IMPORTANT here to use the right object type,
+            // else the slot of the parent object is called (and thus the special
+            // packets never reach their goal)
+            connect(mavlink, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
+            uas = mav;
+        }
+        break;
+    }
+
+    // Set the autopilot type
+    uas->setAutopilotType((int)heartbeat->autopilot);
+
+    // Make UAS aware that this link can be used to communicate with the actual robot
+    uas->addLink(link);
+
+    // Now add UAS to "official" list, which makes the whole application aware of it
+    UASManager::instance()->addUAS(uas);
+
+    return uas;
+}

src/uas/QGCMAVLinkUASFactory.h

@@ -0,0 +1,32 @@
+#ifndef QGCMAVLINKUASFACTORY_H
+#define QGCMAVLINKUASFACTORY_H
+
+#include <QObject>
+
+#include "QGCMAVLink.h"
+#include "MAVLinkProtocol.h"
+#include "UASInterface.h"
+#include "LinkInterface.h"
+
+// INCLUDE ALL MAV/UAS CLASSES USING MAVLINK
+#include "UAS.h"
+#include "SlugsMAV.h"
+#include "PxQuadMAV.h"
+#include "ArduPilotMegaMAV.h"
+
+class QGCMAVLinkUASFactory : public QObject
+{
+    Q_OBJECT
+public:
+    explicit QGCMAVLinkUASFactory(QObject *parent = 0);
+
+    /** @brief Create a new UAS object using MAVLink as protocol */
+    static UASInterface* createUAS(MAVLinkProtocol* mavlink, LinkInterface* link, int sysid, mavlink_heartbeat_t* heartbeat, QObject* parent=NULL);
+
+signals:
+
+public slots:
+
+};
+
+#endif // QGCMAVLINKUASFACTORY_H

src/uas/UAS.cc

@@ -149,6 +149,18 @@ void UAS::setSelected()
     UASManager::instance()->setActiveUAS(this);
 }
 
+void UAS::receiveMessageNamedValue(const mavlink_message_t& message)
+{
+    if (message.msgid == MAVLINK_MSG_ID_NAMED_VALUE_FLOAT)
+    {
+
+    }
+    else if (message.msgid == MAVLINK_MSG_ID_NAMED_VALUE_INT)
+    {
+
+    }
+}
+
 void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
 {
     if (!link) return;

src/uas/UAS.h

@@ -172,10 +172,12 @@ public:
     UASWaypointManager &getWaypointManager(void) { return waypointManager; }
     int getSystemType();
     int getAutopilotType() {return autopilot;}
-    void setAutopilotType(int apType) { autopilot = apType;}
 
 public slots:
-
+    /** @brief Set the autopilot type */
+    void setAutopilotType(int apType) { autopilot = apType; }
+    /** @brief Set the type of airframe */
+    void setSystemType(int systemType) { type = systemType; }
     /** @brief Set a new name **/
     void setUASName(const QString& name);
     /** @brief Executes an action **/
@@ -307,6 +309,10 @@ signals:
     void writeSettings();
     /** @brief Read settings from disk */
     void readSettings();
+
+    // MESSAGE RECEPTION
+    /** @brief Receive a named value message */
+    void receiveMessageNamedValue(const mavlink_message_t& message);
 };
 
 

src/uas/UASWaypointManager.cc

@@ -123,6 +123,10 @@ void UASWaypointManager::handleWaypointCount(quint8 systemId, quint8 compId, qui
             qDebug() << "No waypoints on UAS " << systemId;
         }
     }
+    else
+    {
+            qDebug("Rejecting message, check mismatch: current_state: %d == %d, system id %d == %d, comp id %d == %d", current_state, WP_GETLIST, current_partner_systemid, systemId, current_partner_compid, compId);
+    }
 }
 
 void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_waypoint_t *wp)
@@ -165,9 +169,13 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_
         }
         else
         {
-            //MainWindow::instance()->showStatusMessage(tr("Waypoint ID mismatch, rejecting waypoint"));
+            emit updateStatusString(tr("Waypoint ID mismatch, rejecting waypoint"));
         }
     }
+    else
+    {
+            qDebug("Rejecting message, check mismatch: current_state: %d == %d, system id %d == %d, comp id %d == %d", current_state, WP_GETLIST, current_partner_systemid, systemId, current_partner_compid, compId);
+    }
 }
 
 void UASWaypointManager::handleWaypointAck(quint8 systemId, quint8 compId, mavlink_waypoint_ack_t *wpa)
@@ -210,6 +218,10 @@ void UASWaypointManager::handleWaypointRequest(quint8 systemId, quint8 compId, m
             //TODO: Error message or something
         }
     }
+    else
+    {
+            qDebug("Rejecting message, check mismatch: current_state: %d == %d, system id %d == %d, comp id %d == %d", current_state, WP_GETLIST, current_partner_systemid, systemId, current_partner_compid, compId);
+    }
 }
 
 void UASWaypointManager::handleWaypointReached(quint8 systemId, quint8 compId, mavlink_waypoint_reached_t *wpr)

src/ui/MainWindow.cc

@@ -1155,6 +1155,10 @@ void MainWindow::connectCommonActions()
 
     // Custom widget actions
     connect(ui.actionNewCustomWidget, SIGNAL(triggered()), this, SLOT(createCustomWidget()));
+
+    // Audio output
+    ui.actionMuteAudioOutput->setChecked(GAudioOutput::instance()->isMuted());
+    connect(ui.actionMuteAudioOutput, SIGNAL(triggered(bool)), GAudioOutput::instance(), SLOT(mute(bool)));
 }
 
 void MainWindow::connectPxActions()
@@ -1388,14 +1392,14 @@ void MainWindow::UASCreated(UASInterface* uas)
             }
             break;
 
-        loadOperatorView();
+            loadOperatorView();
         }
 
         // Change the view only if this is the first UAS
 
         // If this is the first connected UAS, it is both created as well as
         // the currently active UAS
-        if (UASManager::instance()->getActiveUAS() == uas)
+        if (UASManager::instance()->getUASList().size() == 1)
         {
             //qDebug() << "UPDATING THE VIEW SINCE THIS IS THE FIRST CONNECTED SYSTEM";
 
@@ -1410,8 +1414,7 @@ void MainWindow::UASCreated(UASInterface* uas)
                 if (settings.contains(getWindowStateKey()))
                 {
                     restoreState(settings.value(getWindowStateKey()).toByteArray(), QGC::applicationVersion());
-            }
-
+                }
             }
             else
             {

src/ui/SlugsHilSim.cc

@@ -252,7 +252,8 @@ uint16_t SlugsHilSim::getUint16FromDatagram (const QByteArray* datagram, unsigne
 
 }
 
-void SlugsHilSim::linkSelected(int cbIndex){
+void SlugsHilSim::linkSelected(int cbIndex)
+{
   #ifdef MAVLINK_ENABLED_SLUGS
     // HIL code to go here...
   //hilLink = linksAvailable
@@ -260,6 +261,8 @@ void SlugsHilSim::linkSelected(int cbIndex){
 
     hilLink =linksAvailable.value(cbIndex);
 
+#else
+    Q_UNUSED(cbIndex)
   #endif
 }
 

src/ui/uas/UASView.cc

@@ -137,6 +137,8 @@ UASView::UASView(UASInterface* uas, QWidget *parent) :
     {
         m_ui->positionLabel->hide();
     }
+
+    setSystemType(uas, uas->getSystemType());
 }
 
 UASView::~UASView()