Fully hooked up HIL and vehicle config, time to flesh it out

src/comm/QGCXPlaneLink.cc

@@ -36,6 +36,7 @@ This file is part of the QGROUNDCONTROL project
 #include "QGCXPlaneLink.h"
 #include "QGC.h"
 #include <QHostInfo>
+#include "UAS.h"
 #include "MainWindow.h"
 
 QGCXPlaneLink::QGCXPlaneLink(UASInterface* mav, QString remoteHost, QHostAddress localHost, quint16 localPort) :
@@ -199,9 +200,9 @@ void QGCXPlaneLink::writeBytes(const char* data, qint64 size)
             ascii.append(219);
         }
     }
-    qDebug() << "Sent" << size << "bytes to" << remoteHost.toString() << ":" << remotePort << "data:";
-    qDebug() << bytes;
-    qDebug() << "ASCII:" << ascii;
+    //qDebug() << "Sent" << size << "bytes to" << remoteHost.toString() << ":" << remotePort << "data:";
+    //qDebug() << bytes;
+    //qDebug() << "ASCII:" << ascii;
 #endif
     if (connectState && socket) socket->writeDatagram(data, size, remoteHost, remotePort);
 }
@@ -232,7 +233,7 @@ void QGCXPlaneLink::readBytes()
     // XPlane always has 5 bytes header: 'DATA@'
     unsigned nsegs = (s-5)/36;
 
-    qDebug() << "XPLANE:" << "LEN:" << s << "segs:" << nsegs;
+    //qDebug() << "XPLANE:" << "LEN:" << s << "segs:" << nsegs;
 
     #pragma pack(push, 1)
     struct payload {
@@ -248,6 +249,8 @@ void QGCXPlaneLink::readBytes()
     float airspeed;
     float groundspeed;
 
+    float man_roll, man_pitch, man_yaw;
+
     if (data[0] == 'D' &&
             data[1] == 'A' &&
             data[2] == 'T' &&
@@ -262,24 +265,30 @@ void QGCXPlaneLink::readBytes()
 
             if (p.index == 3)
             {
-                //qDebug() << "SPEEDS:" << p.f[0] << p.f[1] << p.f[2];
                 airspeed = p.f[6] * 0.44704f;
                 groundspeed = p.f[7] * 0.44704;
 
-
-
-                qDebug() << "SPEEDS:" << "airspeed" << airspeed << "m/s, groundspeed" << groundspeed << "m/s";
+                //qDebug() << "SPEEDS:" << "airspeed" << airspeed << "m/s, groundspeed" << groundspeed << "m/s";
+            }
+            else if (p.index == 8)
+            {
+                //qDebug() << "MAN:" << p.f[0] << p.f[3] << p.f[7];
+                man_roll = p.f[0];
+                man_pitch = p.f[1];
+                man_yaw = p.f[2];
+                UAS* uas = dynamic_cast<UAS*>(mav);
+                if (uas) uas->setManualControlCommands(man_roll, man_pitch, man_yaw, 0.6);
             }
             else if (p.index == 16)
             {
-                qDebug() << "ANG VEL:" << p.f[0] << p.f[3] << p.f[7];
+                //qDebug() << "ANG VEL:" << p.f[0] << p.f[3] << p.f[7];
                 rollspeed = p.f[2];
                 pitchspeed = p.f[1];
                 yawspeed = p.f[0];
             }
             else if (p.index == 17)
             {
-                qDebug() << "HDNG" << "pitch" << p.f[0] << "roll" << p.f[1] << "hding true" << p.f[2] << "hding mag" << p.f[3];
+                //qDebug() << "HDNG" << "pitch" << p.f[0] << "roll" << p.f[1] << "hding true" << p.f[2] << "hding mag" << p.f[3];
                 // XXX Feeding true heading - might need fix
                 pitch = (p.f[0] - 180.0f) / 180.0f * M_PI;
                 roll = (p.f[1] - 180.0f) / 180.0f * M_PI;
@@ -292,30 +301,30 @@ void QGCXPlaneLink::readBytes()
 //            }
             else if (p.index == 20)
             {
-                qDebug() << "LAT/LON/ALT:" << p.f[0] << p.f[1] << p.f[2];
+                //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] * 0.3048f; // convert feet (MSL) to meters
             }
             else if (p.index == 12)
             {
-                qDebug() << "AIL/ELEV/RUD" << p.f[0] << p.f[1] << p.f[2];
+                //qDebug() << "AIL/ELEV/RUD" << p.f[0] << p.f[1] << p.f[2];
             }
             else if (p.index == 25)
             {
-                qDebug() << "THROTTLE" << p.f[0] << p.f[1] << p.f[2] << p.f[3];
+                //qDebug() << "THROTTLE" << p.f[0] << p.f[1] << p.f[2] << p.f[3];
             }
             else if (p.index == 0)
             {
-                qDebug() << "STATS" << "fgraphics/s" << p.f[0] << "fsim/s" << p.f[2] << "t frame" << p.f[3] << "cpu load" << p.f[4] << "grnd ratio" << p.f[5] << "filt ratio" << p.f[6];
+                //qDebug() << "STATS" << "fgraphics/s" << p.f[0] << "fsim/s" << p.f[2] << "t frame" << p.f[3] << "cpu load" << p.f[4] << "grnd ratio" << p.f[5] << "filt ratio" << p.f[6];
             }
             else if (p.index == 11)
             {
-                qDebug() << "CONTROLS" << "ail" << p.f[0] << "elev" << p.f[1] << "rudder" << p.f[2] << "nwheel" << p.f[3];
+                //qDebug() << "CONTROLS" << "ail" << p.f[0] << "elev" << p.f[1] << "rudder" << p.f[2] << "nwheel" << p.f[3];
             }
             else
             {
-                qDebug() << "UNKNOWN #" << p.index << p.f[0] << p.f[1] << p.f[2] << p.f[3];
+                //qDebug() << "UNKNOWN #" << p.index << p.f[0] << p.f[1] << p.f[2] << p.f[3];
             }
         }
     }
@@ -360,12 +369,19 @@ bool QGCXPlaneLink::disconnectSimulation()
 {
     if (!connectState) return true;
 
+    connectState = false;
+
     if (process) disconnect(process, SIGNAL(error(QProcess::ProcessError)),
                this, SLOT(processError(QProcess::ProcessError)));
     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)));
+        UAS* uas = dynamic_cast<UAS*>(mav);
+        if (uas)
+        {
+            uas->stopHil();
+        }
     }
 
     if (process)
@@ -387,8 +403,6 @@ bool QGCXPlaneLink::disconnectSimulation()
         socket = NULL;
     }
 
-    connectState = false;
-
     emit simulationDisconnected();
     emit simulationConnected(false);
     return !connectState;
@@ -402,6 +416,8 @@ bool QGCXPlaneLink::disconnectSimulation()
 bool QGCXPlaneLink::connectSimulation()
 {
     if (!mav) return false;
+    if (connectState) return false;
+
     socket = new QUdpSocket(this);
     connectState = socket->bind(localHost, localPort);
     if (!connectState) return false;
@@ -414,9 +430,11 @@ bool QGCXPlaneLink::connectSimulation()
     connect(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)));
     connect(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)));
 
-    // XXX ugly hack to set MAVLINK_MODE_HIL_FLAG_ENABLED
-    // without pulling MAVLINK dependencies in here
-    mav->setMode(32);
+    UAS* uas = dynamic_cast<UAS*>(mav);
+    if (uas)
+    {
+        uas->startHil();
+    }
 
     // XXX This will later be enabled to start X-Plane from within QGroundControl with the right arguments
 

src/comm/QGCXPlaneLink.h

@@ -112,6 +112,9 @@ protected:
     QProcess* process;
     QProcess* terraSync;
 
+    bool gpsReceived;
+    bool attitudeReceived;
+
     void setName(QString name);
 
 };

src/uas/UAS.cc

@@ -2405,12 +2405,12 @@ void UAS::setManualControlCommands(double roll, double pitch, double yaw, double
     manualThrust = thrust * thrustScaling;
 
     // If system has manual inputs enabled and is armed
-    if((mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) && (mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY))
+    if(((mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) && (mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY)) || (mode & MAV_MODE_FLAG_HIL_ENABLED))
     {
         mavlink_message_t message;
         mavlink_msg_manual_control_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, this->uasId, (float)manualRollAngle, (float)manualPitchAngle, (float)manualYawAngle, (float)manualThrust, controlRollManual, controlPitchManual, controlYawManual, controlThrustManual);
         sendMessage(message);
-        qDebug() << __FILE__ << __LINE__ << ": SENT MANUAL CONTROL MESSAGE: roll" << manualRollAngle << " pitch: " << manualPitchAngle << " yaw: " << manualYawAngle << " thrust: " << manualThrust;
+        //qDebug() << __FILE__ << __LINE__ << ": SENT MANUAL CONTROL MESSAGE: roll" << manualRollAngle << " pitch: " << manualPitchAngle << " yaw: " << manualYawAngle << " thrust: " << manualThrust;
 
         emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, QGC::groundTimeMilliseconds());
     }

src/ui/QGCVehicleConfig.cc

@@ -3,6 +3,7 @@
 #include <QTimer>
 
 #include "QGCVehicleConfig.h"
+#include "UASManager.h"
 #include "QGC.h"
 #include "ui_QGCVehicleConfig.h"
 
@@ -20,6 +21,19 @@ QGCVehicleConfig::QGCVehicleConfig(QWidget *parent) :
 
     connect(ui->rcCalibrationButton, SIGNAL(clicked(bool)), this, SLOT(toggleCalibrationRC(bool)));
     connect(ui->storeButton, SIGNAL(clicked()), this, SLOT(writeParameters()));
+
+    connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)));
+
+    setActiveUAS(UASManager::instance()->getActiveUAS());
+
+    for (int i = 0; i < chanMax; i++)
+    {
+        rcValue[i] = 1500;
+    }
+
+    updateTimer.setInterval(150);
+    connect(&updateTimer, SIGNAL(timeout()), this, SLOT(updateView()));
+    updateTimer.start();
 }
 
 QGCVehicleConfig::~QGCVehicleConfig()
@@ -68,7 +82,10 @@ void QGCVehicleConfig::setActiveUAS(UASInterface* active)
 
     // Connect new system
     mav = active;
-    connect(active, SIGNAL(statusChanged(UASInterface*,QString,QString)), this, SLOT(updateState(UASInterface*, QString,QString)));
+    connect(active, SIGNAL(remoteControlChannelRawChanged(int,float)), this,
+               SLOT(remoteControlChannelRawChanged(int,float)));
+    connect(active, SIGNAL(parameterChanged(int,int,QString,QVariant)), this,
+               SLOT(parameterChanged(int,int,QString,QVariant)));
 }
 
 void QGCVehicleConfig::resetCalibrationRC()
@@ -153,7 +170,7 @@ void QGCVehicleConfig::remoteControlChannelRawChanged(int chan, float val)
     if (chan == rcMapping[0])
     {
         // ROLL
-        if (rcRoll > rcTrim[chan])
+        if (rcRoll >= rcTrim[chan])
         {
             rcRoll = (val - rcTrim[chan])/(rcMax[chan] - rcTrim[chan]);
         }
@@ -162,12 +179,13 @@ void QGCVehicleConfig::remoteControlChannelRawChanged(int chan, float val)
             rcRoll = (val - rcMin[chan])/(rcTrim[chan] - rcMin[chan]);
         }
 
+        rcValue[0] = val;
         rcRoll = qBound(-1.0f, rcRoll, 1.0f);
     }
     else if (chan == rcMapping[1])
     {
         // PITCH
-        if (rcPitch > rcTrim[chan])
+        if (rcPitch >= rcTrim[chan])
         {
             rcPitch = (val - rcTrim[chan])/(rcMax[chan] - rcTrim[chan]);
         }
@@ -175,13 +193,13 @@ void QGCVehicleConfig::remoteControlChannelRawChanged(int chan, float val)
         {
             rcPitch = (val - rcMin[chan])/(rcTrim[chan] - rcMin[chan]);
         }
-
+        rcValue[1] = val;
         rcPitch = qBound(-1.0f, rcPitch, 1.0f);
     }
     else if (chan == rcMapping[2])
     {
         // YAW
-        if (rcYaw > rcTrim[chan])
+        if (rcYaw >= rcTrim[chan])
         {
             rcYaw = (val - rcTrim[chan])/(rcMax[chan] - rcTrim[chan]);
         }
@@ -189,13 +207,13 @@ void QGCVehicleConfig::remoteControlChannelRawChanged(int chan, float val)
         {
             rcYaw = (val - rcMin[chan])/(rcTrim[chan] - rcMin[chan]);
         }
-
+        rcValue[2] = val;
         rcYaw = qBound(-1.0f, rcYaw, 1.0f);
     }
     else if (chan == rcMapping[3])
     {
         // THROTTLE
-        if (rcThrottle > rcTrim[chan])
+        if (rcThrottle >= rcTrim[chan])
         {
             rcThrottle = (val - rcTrim[chan])/(rcMax[chan] - rcTrim[chan]);
         }
@@ -203,13 +221,13 @@ void QGCVehicleConfig::remoteControlChannelRawChanged(int chan, float val)
         {
             rcThrottle = (val - rcMin[chan])/(rcTrim[chan] - rcMin[chan]);
         }
-
+        rcValue[3] = val;
         rcThrottle = qBound(-1.0f, rcThrottle, 1.0f);
     }
     else if (chan == rcMapping[4])
     {
         // MODE SWITCH
-        if (rcMode > rcTrim[chan])
+        if (rcMode >= rcTrim[chan])
         {
             rcMode = (val - rcTrim[chan])/(rcMax[chan] - rcTrim[chan]);
         }
@@ -217,7 +235,7 @@ void QGCVehicleConfig::remoteControlChannelRawChanged(int chan, float val)
         {
             rcMode = (val - rcMin[chan])/(rcTrim[chan] - rcMin[chan]);
         }
-
+        rcValue[4] = val;
         rcMode = qBound(-1.0f, rcMode, 1.0f);
     }
     else if (chan == rcMapping[5])
@@ -238,7 +256,7 @@ void QGCVehicleConfig::remoteControlChannelRawChanged(int chan, float val)
 
     changed = true;
 
-    qDebug() << "RC CHAN:" << chan << "PPM:" << val;
+    //qDebug() << "RC CHAN:" << chan << "PPM:" << val;
 }
 
 void QGCVehicleConfig::parameterChanged(int uas, int component, QString parameterName, QVariant value)
@@ -293,10 +311,10 @@ void QGCVehicleConfig::updateView()
 {
     if (changed)
     {
-        ui->rollSlider->setValue(rcRoll);
-        ui->pitchSlider->setValue(rcPitch);
-        ui->yawSlider->setValue(rcYaw);
-        ui->throttleSlider->setValue(rcThrottle);
+        ui->rollSlider->setValue(rcValue[0]);
+        ui->pitchSlider->setValue(rcValue[1]);
+        ui->yawSlider->setValue(rcValue[2]);
+        ui->throttleSlider->setValue(rcValue[3]);
         changed = false;
     }
 }

src/ui/QGCVehicleConfig.h

@@ -2,6 +2,7 @@
 #define QGCVEHICLECONFIG_H
 
 #include <QWidget>
+#include <QTimer>
 
 #include "UASInterface.h"
 
@@ -60,6 +61,7 @@ protected:
     int rcTrim[chanMax];                ///< Zero-position (center for roll/pitch/yaw, 0 throttle for throttle)
     int rcMapping[chanMax];             ///< PWM to function mappings
     bool rcRev[chanMax];                ///< Channel reverse
+    int rcValue[chanMax];               ///< Last values
     float rcRoll;                       ///< PPM input channel used as roll control input
     float rcPitch;                      ///< PPM input channel used as pitch control input
     float rcYaw;                        ///< PPM input channel used as yaw control input
@@ -70,6 +72,7 @@ protected:
     float rcAux3;                       ///< PPM input channel used as aux 1 input
     bool rcCalChanged;                  ///< Set if the calibration changes (and needs to be written)
     bool changed;                       ///< Set if any of the input data changed
+    QTimer updateTimer;                 ///< Controls update intervals
     
 private:
     Ui::QGCVehicleConfig *ui;