From a6fae6b5392c74caf58174d118df287f78b532b4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 20 Jun 2014 14:38:20 +0200 Subject: [PATCH] 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 | 6 +- src/comm/QGCXPlaneLink.cc | 114 +++++++++++++++++--------------- src/uas/QGCMAVLinkUASFactory.cc | 15 +++++ src/uas/QGXPX4UAS.cc | 37 +++++++++++ src/uas/QGXPX4UAS.h | 21 ++++++ src/uas/UAS.cc | 1 + src/uas/UAS.h | 1 + src/uas/UASInterface.h | 5 ++ src/ui/MainWindow.cc | 21 ++++++ src/ui/MainWindow.h | 1 + 10 files changed, 168 insertions(+), 54 deletions(-) create mode 100644 src/uas/QGXPX4UAS.cc create mode 100644 src/uas/QGXPX4UAS.h diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 070402c50..74868c451 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -567,7 +567,8 @@ HEADERS += \ src/uas/QGCUASFileManager.h \ src/ui/QGCUASFileView.h \ src/uas/QGCUASWorker.h \ - src/CmdLineOptParser.h + src/CmdLineOptParser.h \ + src/uas/QGXPX4UAS.h SOURCES += \ src/main.cc \ @@ -755,4 +756,5 @@ SOURCES += \ src/uas/QGCUASFileManager.cc \ src/ui/QGCUASFileView.cc \ src/uas/QGCUASWorker.cc \ - src/CmdLineOptParser.cc + src/CmdLineOptParser.cc \ + src/uas/QGXPX4UAS.cc diff --git a/src/comm/QGCXPlaneLink.cc b/src/comm/QGCXPlaneLink.cc index b30b5216e..f829b7e12 100644 --- a/src/comm/QGCXPlaneLink.cc +++ b/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(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; } diff --git a/src/uas/QGCMAVLinkUASFactory.cc b/src/uas/QGCMAVLinkUASFactory.cc index 4f1b7daa5..ccf4c0673 100644 --- a/src/uas/QGCMAVLinkUASFactory.cc +++ b/src/uas/QGCMAVLinkUASFactory.cc @@ -1,6 +1,7 @@ #include "QGCMAVLinkUASFactory.h" #include "UASManager.h" #include "QGCUASWorker.h" +#include "QGXPX4UAS.h" QGCMAVLinkUASFactory::QGCMAVLinkUASFactory(QObject *parent) : QObject(parent) @@ -58,6 +59,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); diff --git a/src/uas/QGXPX4UAS.cc b/src/uas/QGXPX4UAS.cc new file mode 100644 index 000000000..723c7d483 --- /dev/null +++ b/src/uas/QGXPX4UAS.cc @@ -0,0 +1,37 @@ +#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; + } +} diff --git a/src/uas/QGXPX4UAS.h b/src/uas/QGXPX4UAS.h new file mode 100644 index 000000000..9876ee49b --- /dev/null +++ b/src/uas/QGXPX4UAS.h @@ -0,0 +1,21 @@ +#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 diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 03f0837c2..210bbeee1 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -1047,6 +1047,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; diff --git a/src/uas/UAS.h b/src/uas/UAS.h index 880063655..dcaf7cc65 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -981,6 +981,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]; diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index d0ea85fb7..cf697ea9b 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -439,6 +439,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); diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index 2883dadc3..68ce49bdf 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -1736,6 +1736,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); @@ -1981,6 +1982,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) diff --git a/src/ui/MainWindow.h b/src/ui/MainWindow.h index 887407bef..5cc2545fa 100644 --- a/src/ui/MainWindow.h +++ b/src/ui/MainWindow.h @@ -224,6 +224,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(); -- 2.22.0