Fix HIL link reconnect issues due to missing threading flag usage, warn user...

Fix HIL link reconnect issues due to missing threading flag usage, warn user about missing config and take him to config view if necessary

qgroundcontrol.pro

@@ -562,7 +562,8 @@ HEADERS += \
     src/uas/UASManagerInterface.h \
     src/uas/QGCUASParamManagerInterface.h \
     src/uas/QGCUASWorker.h \
-    src/CmdLineOptParser.h
+    src/CmdLineOptParser.h \
+    src/uas/QGXPX4UAS.h
 
 SOURCES += \
     src/main.cc \
@@ -747,4 +748,5 @@ SOURCES += \
     src/ui/designer/QGCXYPlot.cc \
     src/ui/menuactionhelper.cpp \
     src/uas/QGCUASWorker.cc \
-    src/CmdLineOptParser.cc
+    src/CmdLineOptParser.cc \
+    src/uas/QGXPX4UAS.cc

src/comm/QGCXPlaneLink.cc

@@ -64,9 +64,12 @@ QGCXPlaneLink::QGCXPlaneLink(UASInterface* mav, QString remoteHost, QHostAddress
     // http://blog.qt.digia.com/blog/2010/06/17/youre-doing-it-wrong/
     moveToThread(this);
 
+    setTerminationEnabled(false);
+
     this->localHost = localHost;
     this->localPort = localPort/*+mav->getUASID()*/;
-    this->connectState = false;
+    connectState = false;
+
     this->name = tr("X-Plane Link (localPort:%1)").arg(localPort);
     setRemoteHost(remoteHost);
     loadSettings();
@@ -151,13 +154,27 @@ void QGCXPlaneLink::setVersion(unsigned int version)
  **/
 void QGCXPlaneLink::run()
 {
-    if (!mav) return;
-    if (connectState) return;
+    if (!mav) {
+        emit statusMessage("No MAV present");
+        return;
+    }
+
+    if (connectState) {
+        emit statusMessage("Already connected");
+        return;
+    }
 
     socket = new QUdpSocket(this);
     socket->moveToThread(this);
     connectState = socket->bind(localHost, localPort);
-    if (!connectState) return;
+    if (!connectState) {
+
+        emit statusMessage("Binding socket failed!");
+
+        delete socket;
+        socket = NULL;
+        return;
+    }
 
     QObject::connect(socket, SIGNAL(readyRead()), this, SLOT(readBytes()));
 
@@ -208,8 +225,6 @@ void QGCXPlaneLink::run()
         }
     }
 
-    //qDebug() << "REQ SEND TO:" << localAddrStr << localPortStr;
-
     ip.index = 0;
     strncpy(ip.str_ipad_them, localAddrStr.toAscii(), qMin((int)sizeof(ip.str_ipad_them), 16));
     strncpy(ip.str_port_them, localPortStr.toAscii(), qMin((int)sizeof(ip.str_port_them), 6));
@@ -217,10 +232,36 @@ void QGCXPlaneLink::run()
 
     writeBytes((const char*)&ip, sizeof(ip));
 
+    _should_exit = false;
+
     while(!_should_exit) {
         QCoreApplication::processEvents();
         QGC::SLEEP::msleep(5);
     }
+
+    if (mav)
+    {
+        disconnect(mav, SIGNAL(hilControlsChanged(quint64, float, float, float, float, quint8, quint8)), this, SLOT(updateControls(quint64,float,float,float,float,quint8,quint8)));
+        disconnect(mav, SIGNAL(hilActuatorsChanged(quint64, float, float, float, float, float, float, float, float)), this, SLOT(updateActuators(quint64,float,float,float,float,float,float,float,float)));
+
+        disconnect(this, SIGNAL(hilGroundTruthChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), mav, SLOT(sendHilGroundTruth(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)));
+        disconnect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), mav, SLOT(sendHilState(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)));
+        disconnect(this, SIGNAL(sensorHilGpsChanged(quint64,double,double,double,int,float,float,float,float,float,float,float,int)), mav, SLOT(sendHilGps(quint64,double,double,double,int,float,float,float,float,float,float,float,int)));
+        disconnect(this, SIGNAL(sensorHilRawImuChanged(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32)), mav, SLOT(sendHilSensors(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32)));
+
+        // Do not toggle HIL state on the UAS - this is not the job of this link, but of the
+        // UAS object
+    }
+
+    connectState = false;
+
+    socket->close();
+    delete socket;
+    socket = NULL;
+
+    emit simulationDisconnected();
+    emit simulationConnected(false);
+    emit finished();
 }
 
 void QGCXPlaneLink::setPort(int localPort)
@@ -839,50 +880,15 @@ qint64 QGCXPlaneLink::bytesAvailable()
  **/
 bool QGCXPlaneLink::disconnectSimulation()
 {
-    if (!connectState) return true;
-
-    connectState = false;
-
-    if (process) disconnect(process, SIGNAL(error(QProcess::ProcessError)),
-               this, SLOT(processError(QProcess::ProcessError)));
-    if (mav)
+    if (connectState)
     {
-        disconnect(mav, SIGNAL(hilControlsChanged(quint64, float, float, float, float, quint8, quint8)), this, SLOT(updateControls(quint64,float,float,float,float,quint8,quint8)));
-        disconnect(mav, SIGNAL(hilActuatorsChanged(quint64, float, float, float, float, float, float, float, float)), this, SLOT(updateActuators(quint64,float,float,float,float,float,float,float,float)));
-
-        disconnect(this, SIGNAL(hilGroundTruthChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), mav, SLOT(sendHilGroundTruth(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)));
-        disconnect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), mav, SLOT(sendHilState(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)));
-        disconnect(this, SIGNAL(sensorHilGpsChanged(quint64,double,double,double,int,float,float,float,float,float,float,float,int)), mav, SLOT(sendHilGps(quint64,double,double,double,int,float,float,float,float,float,float,float,int)));
-        disconnect(this, SIGNAL(sensorHilRawImuChanged(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32)), mav, SLOT(sendHilSensors(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32)));
-
-        UAS* uas = dynamic_cast<UAS*>(mav);
-        if (uas)
-        {
-            uas->stopHil();
-        }
+        _should_exit = true;
+        wait();
+    } else {
+        emit simulationDisconnected();
+        emit simulationConnected(false);
     }
 
-    if (process)
-    {
-        process->close();
-        delete process;
-        process = NULL;
-    }
-    if (terraSync)
-    {
-        terraSync->close();
-        delete terraSync;
-        terraSync = NULL;
-    }
-    if (socket)
-    {
-        socket->close();
-        delete socket;
-        socket = NULL;
-    }
-
-    emit simulationDisconnected();
-    emit simulationConnected(false);
     return !connectState;
 }
 
@@ -1017,11 +1023,15 @@ void QGCXPlaneLink::setRandomAttitude()
  **/
 bool QGCXPlaneLink::connectSimulation()
 {
-    qDebug() << "STARTING X-PLANE LINK, CONNECTING TO" << remoteHost << ":" << remotePort;
-    // XXX Hack
-    storeSettings();
-
-    start(HighPriority);
+    if (connectState) {
+        qDebug() << "Simulation already active";
+    } else {
+        qDebug() << "STARTING X-PLANE LINK, CONNECTING TO" << remoteHost << ":" << remotePort;
+        // XXX Hack
+        storeSettings();
+
+        start(HighPriority);
+    }
 
     return true;
 }

src/uas/QGCMAVLinkUASFactory.cc

 #include "QGCMAVLinkUASFactory.h"
 #include "UASManager.h"
 #include "QGCUASWorker.h"
+#include "QGXPX4UAS.h"
 
 QGCMAVLinkUASFactory::QGCMAVLinkUASFactory(QObject *parent) :
     QObject(parent)
@@ -57,6 +58,20 @@ UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInte
         uas = mav;
     }
     break;
+    case MAV_AUTOPILOT_PX4:
+    {
+        QGXPX4UAS* px4 = new QGXPX4UAS(mavlink, worker, sysid);
+        // Set the system type
+        px4->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)), px4, SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
+        uas = px4;
+    }
+    break;
     case MAV_AUTOPILOT_SLUGS:
     {
         SlugsMAV* mav = new SlugsMAV(mavlink, worker, sysid);

src/uas/QGXPX4UAS.cc

+#include "QGXPX4UAS.h"
+
+QGXPX4UAS::QGXPX4UAS(MAVLinkProtocol* mavlink, QThread* thread, int id) :
+    UAS(mavlink, thread, id)
+{
+}
+
+/**
+ * This function is called by MAVLink once a complete, uncorrupted (CRC check valid)
+ * mavlink packet is received.
+ *
+ * @param link Hardware link the message came from (e.g. /dev/ttyUSB0 or UDP port).
+ *             messages can be sent back to the system via this link
+ * @param message MAVLink message, as received from the MAVLink protocol stack
+ */
+void QGXPX4UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
+{
+    UAS::receiveMessage(link, message);
+}
+
+void QGXPX4UAS::processParamValueMsgHook(mavlink_message_t& msg, const QString& paramName,const mavlink_param_value_t& rawValue, mavlink_param_union_t& paramValue)
+{
+    int compId = msg.compid;
+    if (paramName == "SYS_AUTOSTART") {
+
+        bool ok;
+
+        int val = parameters.value(compId)->value(paramName).toInt(&ok);
+
+        if (ok && val == 0) {
+            emit misconfigurationDetected(this);
+            qDebug() << "HINTING MISCONFIGURATION";
+        }
+
+        qDebug() << "SYS_AUTOSTART FOUND WITH VAL: " << val;
+    }
+}

src/uas/QGXPX4UAS.h

+#ifndef QGXPX4UAS_H
+#define QGXPX4UAS_H
+
+#include "UAS.h"
+
+class QGXPX4UAS : public UAS
+{
+    Q_OBJECT
+    Q_INTERFACES(UASInterface)
+public:
+    QGXPX4UAS(MAVLinkProtocol* mavlink, QThread* thread, int id);
+
+public slots:
+    /** @brief Receive a MAVLink message from this MAV */
+    void receiveMessage(LinkInterface* link, mavlink_message_t message);
+
+    virtual void processParamValueMsgHook(mavlink_message_t& msg, const QString& paramName,const mavlink_param_value_t& rawValue, mavlink_param_union_t& paramValue);
+
+};
+
+#endif // QGXPX4UAS_H

src/uas/UAS.cc

@@ -1044,6 +1044,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
             paramVal.type = rawValue.param_type;
 
             processParamValueMsg(message, parameterName,rawValue,paramVal);
+            processParamValueMsgHook(message, parameterName,rawValue,paramVal);
 
          }
             break;

src/uas/UAS.h

@@ -974,6 +974,7 @@ protected:
     quint64 getUnixReferenceTime(quint64 time);
 
     virtual void processParamValueMsg(mavlink_message_t& msg, const QString& paramName,const mavlink_param_value_t& rawValue, mavlink_param_union_t& paramValue);
+    virtual void processParamValueMsgHook(mavlink_message_t& msg, const QString& paramName,const mavlink_param_value_t& rawValue, mavlink_param_union_t& paramValue) {};
 
     int componentID[256];
     bool componentMulti[256];

src/uas/UASInterface.h

@@ -430,6 +430,11 @@ signals:
     void poiFound(UASInterface* uas, int type, int colorIndex, QString message, float x, float y, float z);
     void poiConnectionFound(UASInterface* uas, int type, int colorIndex, QString message, float x1, float y1, float z1, float x2, float y2, float z2);
 
+    /**
+     * @brief A misconfiguration has been detected by the UAS
+     */
+    void misconfigurationDetected(UASInterface* uas);
+
     /** @brief A text message from the system has been received */
     void textMessageReceived(int uasid, int componentid, int severity, QString text);
 

src/ui/MainWindow.cc

@@ -1700,6 +1700,7 @@ void MainWindow::UASCreated(UASInterface* uas)
 
     connect(uas, SIGNAL(systemSpecsChanged(int)), this, SLOT(UASSpecsChanged(int)));
     connect(uas, SIGNAL(valueChanged(int,QString,QString,QVariant,quint64)), this, SIGNAL(valueChanged(int,QString,QString,QVariant,quint64)));
+    connect(uas, SIGNAL(misconfigurationDetected(UASInterface*)), this, SLOT(handleMisconfiguration(UASInterface*)));
 
     // HIL
     showHILConfigurationWidget(uas);
@@ -1945,6 +1946,26 @@ void MainWindow::setAdvancedMode(bool isAdvancedMode)
     settings.setValue("ADVANCED_MODE",isAdvancedMode);
 }
 
+void MainWindow::handleMisconfiguration(UASInterface* uas) {
+
+    // Ask user if he wants to handle this now
+    QMessageBox msgBox(this);
+    msgBox.setIcon(QMessageBox::Information);
+    msgBox.setText(tr("Missing or Invalid Onboard Configuration"));
+    msgBox.setInformativeText(tr("The onboard system configuration is missing or incomplete. Do you want to resolve this now?"));
+    msgBox.setStandardButtons(QMessageBox::Ok | QMessageBox::Cancel);
+    msgBox.setDefaultButton(QMessageBox::Ok);
+    int val = msgBox.exec();
+
+    if (val == QMessageBox::Ok) {
+        // He wants to handle it, make sure this system is selected
+        UASManager::instance()->setActiveUAS(uas);
+
+        // Flick to config view
+        loadHardwareConfigView();
+    }
+}
+
 void MainWindow::loadEngineerView()
 {
     if (currentView != VIEW_ENGINEER)

src/ui/MainWindow.h

@@ -222,6 +222,7 @@ public slots:
 
     /** @brief Sets advanced mode, allowing for editing of tool widget locations */
     void setAdvancedMode(bool isAdvancedMode);
+    void handleMisconfiguration(UASInterface* uas);
     /** @brief Load configuration views */
     void loadHardwareConfigView();
     void loadSoftwareConfigView();