Fix HIL link reconnect issues due to missing threading flag usage, wa…

…rn user about missing config and take him to config view if necessary
D4U · Jun 20, 2014 · a6fae6b · a6fae6b
1 parent 52be49a
commit a6fae6b
Show file tree

Hide file tree

Showing 10 changed files with 168 additions and 54 deletions.
diff --git 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
@@ -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,19 +225,43 @@ 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));
  ip.use_ip = 1;
 
  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;
 }

diff --git 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
@@ -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
@@ -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
@@ -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
@@ -981,6 +981,7 @@ public slots:
  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
@@ -439,6 +439,11 @@ public slots:
  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
@@ -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
@@ -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();