Pushed HIL with X-plane to being close to operational. Receiving from X-Plane ...

Pushed HIL with X-plane to being close to operational. Receiving from X-Plane  implemented, sending to it is prepared. Needs an iteration of onboard SW first

qgroundcontrol.pro

@@ -223,7 +223,8 @@ FORMS += src/ui/MainWindow.ui \
     src/ui/mission/QGCMissionNavSweep.ui \
     src/ui/mission/QGCMissionDoStartSearch.ui \
     src/ui/mission/QGCMissionDoFinishSearch.ui \
-    src/ui/QGCVehicleConfig.ui
+    src/ui/QGCVehicleConfig.ui \
+    src/ui/QGCHilConfiguration.ui
 INCLUDEPATH += src \
     src/ui \
     src/ui/linechart \
@@ -360,7 +361,8 @@ HEADERS += src/MG.h \
     src/ui/mission/QGCMissionDoStartSearch.h \
     src/ui/mission/QGCMissionDoFinishSearch.h \
     src/ui/QGCVehicleConfig.h \
-    src/comm/QGCHilLink.h
+    src/comm/QGCHilLink.h \
+    src/ui/QGCHilConfiguration.h
 
 # Google Earth is only supported on Mac OS and Windows with Visual Studio Compiler
 macx|macx-g++|macx-g++42|win32-msvc2008|win32-msvc2010::HEADERS += src/ui/map3D/QGCGoogleEarthView.h
@@ -517,7 +519,8 @@ SOURCES += src/main.cc \
     src/ui/mission/QGCMissionDoStartSearch.cc \
     src/ui/mission/QGCMissionDoFinishSearch.cc \
     src/ui/QGCVehicleConfig.cc \
-    src/comm/QGCHilLink.cc
+    src/comm/QGCHilLink.cc \
+    src/ui/QGCHilConfiguration.cc
 
 # Enable Google Earth only on Mac OS and Windows with Visual Studio compiler
 macx|macx-g++|macx-g++42|win32-msvc2008|win32-msvc2010::SOURCES += src/ui/map3D/QGCGoogleEarthView.cc

src/comm/QGCXPlaneLink.cc

@@ -39,13 +39,14 @@ This file is part of the QGROUNDCONTROL project
 #include "MainWindow.h"
 
 QGCXPlaneLink::QGCXPlaneLink(UASInterface* mav, QString remoteHost, QHostAddress localHost, quint16 localPort) :
+    mav(mav),
+    socket(NULL),
     process(NULL),
     terraSync(NULL)
 {
     this->localHost = localHost;
     this->localPort = localPort/*+mav->getUASID()*/;
     this->connectState = false;
-    this->mav = mav;
     this->name = tr("X-Plane Link (localPort:%1)").arg(localPort);
     setRemoteHost(remoteHost);
 }
@@ -209,55 +210,62 @@ void QGCXPlaneLink::readBytes()
 
     qDebug() << "XPLANE:" << "LEN:" << s << "segs:" << nsegs;
 
-    union dataconv {
-        char c[8];
-        float f;
-        double d;
-        int i;
-    } u;
+    #pragma pack(push, 1)
+    struct payload {
+        int index;
+        float f[8];
+    } p;
+    #pragma pack(pop)
 
-    for (unsigned i = 0; i < nsegs; i++)
-    {
-        // Get index
-        unsigned ioff = (5+i*36);
-        memcpy(&(u.i), data+ioff, sizeof(u.i));
-        qDebug() << "ind:" << u.i;
-        unsigned doff = ioff+sizeof(u.i);
-        unsigned dsize = sizeof(u.d);
-        memcpy(&(u.d), data+doff, dsize);
-        doff += dsize;
-        qDebug() << "val1:" << u.d;
-    }
-
-    return;
-
-    // Parse string
-    double time;
+    quint64 time;
     float roll, pitch, yaw, rollspeed, pitchspeed, yawspeed;
     double lat, lon, alt;
     double vx, vy, vz, xacc, yacc, zacc;
     double airspeed;
 
-//    time = values.at(0).toDouble();
-//    lat = values.at(1).toDouble();
-//    lon = values.at(2).toDouble();
-//    alt = values.at(3).toDouble();
-//    roll = values.at(4).toDouble();
-//    pitch = values.at(5).toDouble();
-//    yaw = values.at(6).toDouble();
-//    rollspeed = values.at(7).toDouble();
-//    pitchspeed = values.at(8).toDouble();
-//    yawspeed = values.at(9).toDouble();
+    if (data[0] == 'D' &&
+            data[1] == 'A' &&
+            data[2] == 'T' &&
+            data[3] == 'A')
+    {
+
+        for (unsigned i = 0; i < nsegs; i++)
+        {
+            // Get index
+            unsigned ioff = (5+i*36);;
+            memcpy(&(p), data+ioff, sizeof(p));
+
+            if (p.index == 3)
+            {
+                qDebug() << "SPEEDS:" << p.f[0] << p.f[1] << p.f[2];
+            }
 
-//    xacc = values.at(10).toDouble();
-//    yacc = values.at(11).toDouble();
-//    zacc = values.at(12).toDouble();
+            if (p.index == 18)
+            {
+                qDebug() << "ANG VEL:" << p.f[0] << p.f[1] << p.f[2];
+                roll = p.f[0] / 180.0 * M_PI;
+                pitch = p.f[1] / 180.0 * M_PI;
+                yaw = p.f[1] / 180.0 * M_PI;
+            }
 
-//    vx = values.at(13).toDouble();
-//    vy = values.at(14).toDouble();
-//    vz = values.at(15).toDouble();
+            if (p.index == 19)
+            {
+                qDebug() << "ATT:" << p.f[0] << p.f[1] << p.f[2];
+            }
 
-//    airspeed = values.at(16).toDouble();
+            if (p.index == 20)
+            {
+                qDebug() << "LAT/LON/ALT:" << p.f[0] << p.f[1] << p.f[2];
+                lat = p.f[0];
+                lon = p.f[1];
+                alt = p.f[2];
+            }
+        }
+    }
+    else
+    {
+        qDebug() << "UNKNOWN PACKET:" << data;
+    }
 
     // Send updated state
     emit hilStateChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed,
@@ -293,10 +301,15 @@ qint64 QGCXPlaneLink::bytesAvailable()
  **/
 bool QGCXPlaneLink::disconnectSimulation()
 {
-    disconnect(process, SIGNAL(error(QProcess::ProcessError)),
+    if (!connectState) return true;
+
+    if (process) disconnect(process, SIGNAL(error(QProcess::ProcessError)),
                this, SLOT(processError(QProcess::ProcessError)));
-    disconnect(mav, SIGNAL(hilControlsChanged(uint64_t, float, float, float, float, uint8_t, uint8_t)), this, SLOT(updateControls(uint64_t,float,float,float,float,uint8_t,uint8_t)));
-    disconnect(this, SIGNAL(hilStateChanged(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)), mav, SLOT(sendHilState(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)));
+    if (mav)
+    {
+        disconnect(mav, SIGNAL(hilControlsChanged(uint64_t, float, float, float, float, uint8_t, uint8_t)), this, SLOT(updateControls(uint64_t,float,float,float,float,uint8_t,uint8_t)));
+        disconnect(this, SIGNAL(hilStateChanged(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)), mav, SLOT(sendHilState(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)));
+    }
 
     if (process)
     {
@@ -334,6 +347,7 @@ bool QGCXPlaneLink::connectSimulation()
     if (!mav) return false;
     socket = new QUdpSocket(this);
     connectState = socket->bind(localHost, localPort);
+    if (!connectState) return false;
 
     QObject::connect(socket, SIGNAL(readyRead()), this, SLOT(readBytes()));
 
@@ -505,7 +519,7 @@ bool QGCXPlaneLink::connectSimulation()
 
 //        qDebug() << "STARTING: " << processFgfs << processCall;
 
-    qDebug() << "STARTING X-PLANE LINK";
+    qDebug() << "STARTING X-PLANE LINK, CONNECTING TO" << remoteHost << ":" << remotePort;
 
     start(LowPriority);
     return connectState;

src/comm/QGCXPlaneLink.h

@@ -89,6 +89,7 @@ public slots:
     bool disconnectSimulation();
 
 protected:
+    UASInterface* mav;
     QString name;
     QHostAddress localHost;
     quint16 localPort;
@@ -108,34 +109,11 @@ protected:
     QMutex statisticsMutex;
     QMutex dataMutex;
     QTimer refreshTimer;
-    UASInterface* mav;
     QProcess* process;
     QProcess* terraSync;
 
     void setName(QString name);
 
-signals:
-    /**
-     * @brief This signal is emitted instantly when the link is connected
-     **/
-    void flightGearConnected();
-
-    /**
-     * @brief This signal is emitted instantly when the link is disconnected
-     **/
-    void flightGearDisconnected();
-
-    /**
-     * @brief This signal is emitted instantly when the link status changes
-     **/
-    void flightGearConnected(bool connected);
-
-    /** @brief State update from FlightGear */
-    void hilStateChanged(uint64_t time_us, float roll, float pitch, float yaw, float rollspeed,
-                        float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt,
-                        int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc);
-
-
 };
 
 #endif // QGCXPLANESIMULATIONLINK_H

src/uas/UAS.cc

@@ -2603,7 +2603,7 @@ void UAS::sendHilState(uint64_t time_us, float roll, float pitch, float yaw, flo
 **/
 void UAS::startHil()
 {
-    // Connect Flight Gear Link
+    // Connect HIL simulation link
     simulation->connectSimulation();
     mavlink_message_t msg;
     mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode | MAV_MODE_FLAG_HIL_ENABLED, navMode);

src/uas/UAS.h

@@ -362,6 +362,10 @@ public:
     QGCUASParamManager* getParamManager() const {
         return paramManager;
     }
+    /** @brief Get the HIL simulation */
+    QGCHilLink* getHILSimulation() const {
+        return simulation;
+    }
     // TODO Will be removed
     /** @brief Set reference to the param manager **/
     void setParamManager(QGCUASParamManager* manager) {

src/ui/MainWindow.cc

@@ -35,6 +35,8 @@ This file is part of the QGROUNDCONTROL project
 #include <QTimer>
 #include <QHostInfo>
 #include <QSplashScreen>
+#include <QGCHilLink.h>
+#include <QGCHilConfiguration.h>
 
 #include "QGC.h"
 #include "MAVLinkSimulationLink.h"
@@ -1382,6 +1384,19 @@ void MainWindow::UASCreated(UASInterface* uas)
     if (!ui.menuConnected_Systems->isEnabled()) ui.menuConnected_Systems->setEnabled(true);
     if (!ui.menuUnmanned_System->isEnabled()) ui.menuUnmanned_System->setEnabled(true);
 
+    // Add simulation configuration widget
+    UAS* mav = dynamic_cast<UAS*>(uas);
+
+    if (mav)
+    {
+        QGCHilConfiguration* hconf = new QGCHilConfiguration(mav->getHILSimulation(), this);
+        QString hilDockName = tr("HIL Config (%1)").arg(uas->getUASName());
+        QDockWidget* hilDock = new QDockWidget(hilDockName, this);
+        hilDock->setWidget(hconf);
+        hilDock->setObjectName(QString("HIL_CONFIG_%1").arg(uas->getUASID()));
+        addTool(hilDock, hilDockName, Qt::RightDockWidgetArea);
+    }
+
     // Reload view state in case new widgets were added
     loadViewState();
 }

src/ui/QGCHilConfiguration.cc

+#include "QGCHilConfiguration.h"
+#include "ui_QGCHilConfiguration.h"
+
+QGCHilConfiguration::QGCHilConfiguration(QGCHilLink* link, QWidget *parent) :
+    QWidget(parent),
+    link(link),
+    ui(new Ui::QGCHilConfiguration)
+{
+    ui->setupUi(this);
+
+    connect(ui->startButton, SIGNAL(clicked(bool)), this, SLOT(toggleSimulation(bool)));
+    connect(ui->hostComboBox, SIGNAL(activated(QString)), link, SLOT(setRemoteHost(QString)));
+
+    ui->startButton->setText(tr("Connect"));
+}
+
+void QGCHilConfiguration::toggleSimulation(bool connect)
+{
+    Q_UNUSED(connect);
+    if (!link->isConnected())
+    {
+        link->setRemoteHost(ui->hostComboBox->currentText());
+        link->connectSimulation();
+        ui->startButton->setText(tr("Disconnect"));
+    }
+    else
+    {
+        link->disconnectSimulation();
+        ui->startButton->setText(tr("Connect"));
+    }
+}
+
+QGCHilConfiguration::~QGCHilConfiguration()
+{
+    delete ui;
+}

src/ui/QGCHilConfiguration.h

+#ifndef QGCHILCONFIGURATION_H
+#define QGCHILCONFIGURATION_H
+
+#include <QWidget>
+
+#include "QGCHilLink.h"
+
+namespace Ui {
+class QGCHilConfiguration;
+}
+
+class QGCHilConfiguration : public QWidget
+{
+    Q_OBJECT
+    
+public:
+    QGCHilConfiguration(QGCHilLink* link, QWidget *parent = 0);
+    ~QGCHilConfiguration();
+
+public slots:
+    /** Start / stop simulation */
+    void toggleSimulation(bool connect);
+
+protected:
+    QGCHilLink* link;
+    
+private:
+    Ui::QGCHilConfiguration *ui;
+};
+
+#endif // QGCHILCONFIGURATION_H

src/ui/QGCHilConfiguration.ui

+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>QGCHilConfiguration</class>
+ <widget class="QWidget" name="QGCHilConfiguration">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>305</width>
+    <height>172</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>Form</string>
+  </property>
+  <layout class="QGridLayout" name="gridLayout" columnstretch="40,60,50,50">
+   <item row="4" column="0" colspan="3">
+    <widget class="QLabel" name="statusLabel">
+     <property name="text">
+      <string>Status</string>
+     </property>
+    </widget>
+   </item>
+   <item row="5" column="0" colspan="2">
+    <widget class="QPushButton" name="randomAttitudeButton">
+     <property name="text">
+      <string>Random ATT</string>
+     </property>
+    </widget>
+   </item>
+   <item row="3" column="0" colspan="2">
+    <widget class="QPushButton" name="startButton">
+     <property name="text">
+      <string>Start</string>
+     </property>
+    </widget>
+   </item>
+   <item row="0" column="0">
+    <widget class="QLabel" name="simLabel">
+     <property name="text">
+      <string>Simulator</string>
+     </property>
+    </widget>
+   </item>
+   <item row="1" column="0">
+    <widget class="QLabel" name="hostLabel">
+     <property name="text">
+      <string>Host</string>
+     </property>
+    </widget>
+   </item>
+   <item row="3" column="2" colspan="2">
+    <widget class="QPushButton" name="setHomeButton">
+     <property name="text">
+      <string>Set HOME</string>
+     </property>
+    </widget>
+   </item>
+   <item row="1" column="1" colspan="3">
+    <widget class="QComboBox" name="hostComboBox">
+     <property name="editable">
+      <bool>true</bool>
+     </property>
+     <item>
+      <property name="text">
+       <string>127.0.0.1:49000</string>
+      </property>
+     </item>
+    </widget>
+   </item>
+   <item row="0" column="1" colspan="3">
+    <widget class="QComboBox" name="simComboBox">
+     <item>
+      <property name="text">
+       <string>X-Plane</string>
+      </property>
+     </item>
+    </widget>
+   </item>
+   <item row="5" column="2" colspan="2">
+    <widget class="QPushButton" name="randomPositionButton">
+     <property name="text">
+      <string>Random POS</string>
+     </property>
+    </widget>
+   </item>
+  </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>