Add support for HIL_ACTUATOR_CONTROLS mavlink message

libs/mavlink/include/mavlink/v1.0

@@ -1 +1 @@
-Subproject commit 28ea42ccb6ab1d764cacf3f0f55358dc3c66faa9
+Subproject commit 621bb1f70d17548b3f159fb30514023960be41f1

src/comm/QGCXPlaneLink.cc

@@ -49,6 +49,7 @@ QGCXPlaneLink::QGCXPlaneLink(Vehicle* vehicle, QString remoteHost, QHostAddress
     simUpdateLastGroundTruth(QGC::groundTimeMilliseconds()),
     simUpdateHz(0),
     _sensorHilEnabled(true),
+    _useHilActuatorControls(true),
     _should_exit(false)
 {
     // We're doing it wrong - because the Qt folks got the API wrong:
@@ -88,6 +89,7 @@ void QGCXPlaneLink::loadSettings()
     setVersion(settings.value("XPLANE_VERSION", 10).toInt());
     selectAirframe(settings.value("AIRFRAME", "default").toString());
     _sensorHilEnabled = settings.value("SENSOR_HIL", _sensorHilEnabled).toBool();
+    _useHilActuatorControls = settings.value("ACTUATOR_HIL", _useHilActuatorControls).toBool();
     settings.endGroup();
 }
 
@@ -100,6 +102,7 @@ void QGCXPlaneLink::storeSettings()
     settings.setValue("XPLANE_VERSION", xPlaneVersion);
     settings.setValue("AIRFRAME", airframeName);
     settings.setValue("SENSOR_HIL", _sensorHilEnabled);
+    settings.setValue("ACTUATOR_HIL", _useHilActuatorControls);
     settings.endGroup();
 }
 
@@ -136,6 +139,23 @@ void QGCXPlaneLink::setVersion(unsigned int version)
     if (changed) emit versionChanged(QString("X-Plane %1").arg(xPlaneVersion));
 }
 
+void QGCXPlaneLink::enableHilActuatorControls(bool enable)
+{
+    if (enable != _useHilActuatorControls) {
+        _useHilActuatorControls = enable;
+    }
+
+    /* Only use override for new message and specific airframes */
+    MAV_TYPE type = _vehicle->vehicleType();
+    float value = 0.0f;
+    if (type == MAV_TYPE_VTOL_RESERVED2) {
+        value = (enable ? 1.0f : 0.0f);
+    }
+
+    sendDataRef("sim/operation/override/override_control_surfaces", value);
+    emit useHilActuatorControlsChanged(enable);
+}
+
 
 /**
  * @brief Runs the thread
@@ -170,6 +190,7 @@ void QGCXPlaneLink::run()
     QObject::connect(socket, &QUdpSocket::readyRead, this, &QGCXPlaneLink::readBytes);
 
     connect(_vehicle->uas(), &UAS::hilControlsChanged, this, &QGCXPlaneLink::updateControls, Qt::QueuedConnection);
+    connect(_vehicle->uas(), &UAS::hilActuatorControlsChanged, this, &QGCXPlaneLink::updateActuatorControls, Qt::QueuedConnection);
 
     connect(this, &QGCXPlaneLink::hilGroundTruthChanged, _vehicle->uas(), &UAS::sendHilGroundTruth, Qt::QueuedConnection);
     connect(this, &QGCXPlaneLink::hilStateChanged, _vehicle->uas(), &UAS::sendHilState, Qt::QueuedConnection);
@@ -218,6 +239,9 @@ void QGCXPlaneLink::run()
 
     writeBytesSafe((const char*)&ip, sizeof(ip));
 
+    /* Call function which makes sure individual control override is enabled/disabled */
+    enableHilActuatorControls(_useHilActuatorControls);
+
     _should_exit = false;
 
     while(!_should_exit) {
@@ -338,6 +362,11 @@ void QGCXPlaneLink::setRemoteHost(const QString& newHost)
 
 void QGCXPlaneLink::updateControls(quint64 time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, quint8 systemMode, quint8 navMode)
 {
+    /* Only use HIL_CONTROL when the checkbox is unchecked */
+    if (_useHilActuatorControls) {
+        //qDebug() << "received HIL_CONTROL but not using it";
+        return;
+    }
     #pragma pack(push, 1)
     struct payload {
         char b[5];
@@ -400,6 +429,113 @@ void QGCXPlaneLink::updateControls(quint64 time, float rollAilerons, float pitch
     }
 }
 
+void QGCXPlaneLink::updateActuatorControls(quint64 time, quint64 flags, float ctl_0, float ctl_1, float ctl_2, float ctl_3, float ctl_4, float ctl_5, float ctl_6, float ctl_7, float ctl_8, float ctl_9, float ctl_10, float ctl_11, float ctl_12, float ctl_13, float ctl_14, float ctl_15, quint8 mode)
+{
+    if (!_useHilActuatorControls) {
+        //qDebug() << "received HIL_ACTUATOR_CONTROLS but not using it";
+        return;
+    }
+
+    Q_UNUSED(time);
+    Q_UNUSED(flags);
+    Q_UNUSED(mode);
+    Q_UNUSED(ctl_12);
+    Q_UNUSED(ctl_13);
+    Q_UNUSED(ctl_14);
+    Q_UNUSED(ctl_15);
+
+    #pragma pack(push, 1)
+    struct payload {
+        char b[5];
+        int index;
+        float f[8];
+    } p;
+    #pragma pack(pop)
+
+    p.b[0] = 'D';
+    p.b[1] = 'A';
+    p.b[2] = 'T';
+    p.b[3] = 'A';
+    p.b[4] = '\0';
+
+    /* Initialize with zeroes */
+    memset(p.f, 0, sizeof(p.f));
+
+    switch (_vehicle->vehicleType()) {
+        case MAV_TYPE_QUADROTOR:
+        case MAV_TYPE_HEXAROTOR:
+        case MAV_TYPE_OCTOROTOR:
+        {
+            p.f[0] = ctl_0;         ///< X-Plane Engine 1
+            p.f[1] = ctl_1;         ///< X-Plane Engine 2
+            p.f[2] = ctl_2;         ///< X-Plane Engine 3
+            p.f[3] = ctl_3;         ///< X-Plane Engine 4
+            p.f[4] = ctl_4;         ///< X-Plane Engine 5
+            p.f[5] = ctl_5;         ///< X-Plane Engine 6
+            p.f[6] = ctl_6;         ///< X-Plane Engine 7
+            p.f[7] = ctl_7;         ///< X-Plane Engine 8
+
+            /* Direct throttle control */
+            p.index = 25;
+            writeBytesSafe((const char*)&p, sizeof(p));
+            break;
+        }
+        case MAV_TYPE_VTOL_RESERVED2:
+        {
+            /**
+             * Tailsitter with four control flaps and eight motors.
+             */
+
+            /* Throttle channels */
+            p.f[0] = ctl_0;
+            p.f[1] = ctl_1;
+            p.f[2] = ctl_2;
+            p.f[3] = ctl_3;
+            p.f[4] = ctl_4;
+            p.f[5] = ctl_5;
+            p.f[6] = ctl_6;
+            p.f[7] = ctl_7;
+            p.index = 25;
+            writeBytesSafe((const char*)&p, sizeof(p));
+
+            /* Control individual actuators */
+            float max_surface_deflection = 30.0f; // Degrees
+            sendDataRef("sim/flightmodel/controls/wing1l_ail1def", ctl_8 * max_surface_deflection);
+            sendDataRef("sim/flightmodel/controls/wing1r_ail1def", ctl_9 * max_surface_deflection);
+            sendDataRef("sim/flightmodel/controls/wing2l_ail1def", ctl_10 * max_surface_deflection);
+            sendDataRef("sim/flightmodel/controls/wing2r_ail1def", ctl_11 * max_surface_deflection);
+
+            break;
+        }
+        default:
+        {
+            /* direct pass-through, normal fixed-wing. */
+            p.f[0] = -ctl_1;        ///< X-Plane Elevator
+            p.f[1] = ctl_0;         ///< X-Plane Aileron
+            p.f[2] = ctl_2;         ///< X-Plane Rudder
+
+            /* Send to group 8, which equals manual controls */
+            p.index = 8;
+            writeBytesSafe((const char*)&p, sizeof(p));
+
+            /* Send throttle to all eight motors */
+            p.index = 25;
+            p.f[0] = ctl_3;
+            p.f[1] = ctl_3;
+            p.f[2] = ctl_3;
+            p.f[3] = ctl_3;
+            p.f[4] = ctl_3;
+            p.f[5] = ctl_3;
+            p.f[6] = ctl_3;
+            p.f[7] = ctl_3;
+            writeBytesSafe((const char*)&p, sizeof(p));
+            break;
+        }
+
+    }
+
+}
+
 Eigen::Matrix3f euler_to_wRo(double yaw, double pitch, double roll) {
   double c__ = cos(yaw);
   double _c_ = cos(pitch);
@@ -984,3 +1120,39 @@ void QGCXPlaneLink::setName(QString name)
     this->name = name;
     //    emit nameChanged(this->name);
 }
+
+void QGCXPlaneLink::sendDataRef(QString ref, float value)
+{
+    #pragma pack(push, 1)
+    struct payload {
+        char b[5];
+        float value;
+        char name[500];
+    } dref;
+    #pragma pack(pop)
+
+    dref.b[0] = 'D';
+    dref.b[1] = 'R';
+    dref.b[2] = 'E';
+    dref.b[3] = 'F';
+    dref.b[4] = '0';
+
+    /* Set value */
+    dref.value = value;
+
+    /* Fill name with zeroes */
+    memset(dref.name, 0, sizeof(dref.name));
+
+    /* Set dref name */
+
+    /* Send command */
+    QByteArray ba = ref.toUtf8();
+    if (ba.length() > 500) {
+        return;
+    }
+
+    for (int i = 0; i < ba.length(); i++) {
+        dref.name[i] = ba.at(i);
+    }
+    writeBytesSafe((const char*)&dref, sizeof(dref));
+}

src/comm/QGCXPlaneLink.h

@@ -92,6 +92,14 @@ public:
         return _sensorHilEnabled;
     }
 
+    bool useHilActuatorControls() {
+        return _useHilActuatorControls;
+    }
+
+signals:
+    /** @brief Sensor leve HIL state changed */
+    void useHilActuatorControlsChanged(bool enabled);
+
 public slots:
 //    void setAddress(QString address);
     void setPort(int port);
@@ -99,6 +107,25 @@ public slots:
     void setRemoteHost(const QString& host);
     /** @brief Send new control states to the simulation */
     void updateControls(quint64 time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, quint8 systemMode, quint8 navMode);
+    /** @brief Send new control commands to the simulation */
+    void updateActuatorControls(quint64 time, quint64 flags,
+                                float ctl_0,
+                                float ctl_1,
+                                float ctl_2,
+                                float ctl_3,
+                                float ctl_4,
+                                float ctl_5,
+                                float ctl_6,
+                                float ctl_7,
+                                float ctl_8,
+                                float ctl_9,
+                                float ctl_10,
+                                float ctl_11,
+                                float ctl_12,
+                                float ctl_13,
+                                float ctl_14,
+                                float ctl_15,
+                                quint8 mode);
     /** @brief Set the simulator version as text string */
     void setVersion(const QString& version);
     /** @brief Set the simulator version as integer */
@@ -110,6 +137,8 @@ public slots:
             emit sensorHilChanged(enable);
     }
 
+    void enableHilActuatorControls(bool enable);
+
     void processError(QProcess::ProcessError err);
 
     void readBytes();
@@ -199,9 +228,11 @@ protected:
     quint64 simUpdateLastGroundTruth;
     float simUpdateHz;
     bool _sensorHilEnabled;
+    bool _useHilActuatorControls;
     bool _should_exit;
 
     void setName(QString name);
+    void sendDataRef(QString ref, float value);
 };
 
 #endif // QGCXPLANESIMULATIONLINK_H

src/uas/UAS.cc

@@ -429,6 +429,30 @@ void UAS::receiveMessage(mavlink_message_t message)
             emit hilControlsChanged(hil.time_usec, hil.roll_ailerons, hil.pitch_elevator, hil.yaw_rudder, hil.throttle, hil.mode, hil.nav_mode);
         }
             break;
+        case MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS:
+        {
+            mavlink_hil_actuator_controls_t hil;
+            mavlink_msg_hil_actuator_controls_decode(&message, &hil);
+            emit hilActuatorControlsChanged(hil.time_usec, hil.flags,
+                                            hil.controls[0],
+                                            hil.controls[1],
+                                            hil.controls[2],
+                                            hil.controls[3],
+                                            hil.controls[4],
+                                            hil.controls[5],
+                                            hil.controls[6],
+                                            hil.controls[7],
+                                            hil.controls[8],
+                                            hil.controls[9],
+                                            hil.controls[10],
+                                            hil.controls[11],
+                                            hil.controls[12],
+                                            hil.controls[13],
+                                            hil.controls[14],
+                                            hil.controls[15],
+                                            hil.mode);
+        }
+            break;
         case MAVLINK_MSG_ID_VFR_HUD:
         {
             mavlink_vfr_hud_t hud;

src/uas/UAS.h

@@ -554,6 +554,8 @@ signals:
     void imageReady(UASInterface* uas);
     /** @brief HIL controls have changed */
     void hilControlsChanged(quint64 time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, quint8 systemMode, quint8 navMode);
+    /** @brief HIL actuator controls (replaces HIL controls) */
+    void hilActuatorControlsChanged(quint64 time, quint64 flags, float ctl_0, float ctl_1, float ctl_2, float ctl_3, float ctl_4, float ctl_5, float ctl_6, float ctl_7, float ctl_8, float ctl_9, float ctl_10, float ctl_11, float ctl_12, float ctl_13, float ctl_14, float ctl_15, quint8 mode);
 
     void localXChanged(double val,QString name);
     void localYChanged(double val,QString name);

src/ui/QGCHilXPlaneConfiguration.cc

@@ -35,8 +35,11 @@ QGCHilXPlaneConfiguration::QGCHilXPlaneConfiguration(QGCHilLink* link, QGCHilCon
         // XXX not implemented yet
         //ui->airframeComboBox->hide();
         ui->sensorHilCheckBox->setChecked(xplane->sensorHilEnabled());
+        ui->useHilActuatorControlsCheckBox->setChecked(true);
         connect(xplane, &QGCXPlaneLink::sensorHilChanged, ui->sensorHilCheckBox, &QCheckBox::setChecked);
         connect(ui->sensorHilCheckBox, &QCheckBox::clicked, xplane, &QGCXPlaneLink::enableSensorHIL);
+        connect(xplane, &QGCXPlaneLink::useHilActuatorControlsChanged, ui->useHilActuatorControlsCheckBox, &QCheckBox::setChecked);
+        connect(ui->useHilActuatorControlsCheckBox, &QCheckBox::clicked, xplane, &QGCXPlaneLink::enableHilActuatorControls);
 
         connect(link, static_cast<void (QGCHilLink::*)(int)>(&QGCHilLink::versionChanged),
                 this, &QGCHilXPlaneConfiguration::setVersion);

src/ui/QGCHilXPlaneConfiguration.ui

@@ -6,14 +6,14 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>226</width>
+    <width>269</width>
     <height>150</height>
    </rect>
   </property>
   <property name="windowTitle">
    <string>Form</string>
   </property>
-  <layout class="QGridLayout" name="gridLayout" rowstretch="0,0,0,0,0" columnstretch="20,0,0">
+  <layout class="QGridLayout" name="gridLayout" rowstretch="0,0,0,0,0,0,0,0" columnstretch="20,0,0">
    <property name="leftMargin">
     <number>0</number>
    </property>
@@ -26,16 +26,11 @@
    <property name="bottomMargin">
     <number>0</number>
    </property>
-   <item row="0" column="1" colspan="2">
-    <widget class="QComboBox" name="hostComboBox">
-     <property name="editable">
-      <bool>true</bool>
+   <item row="1" column="1" colspan="2">
+    <widget class="QPushButton" name="startButton">
+     <property name="text">
+      <string>Start</string>
      </property>
-     <item>
-      <property name="text">
-       <string>127.0.0.1:49000</string>
-      </property>
-     </item>
     </widget>
    </item>
    <item row="0" column="0">
@@ -45,14 +40,7 @@
      </property>
     </widget>
    </item>
-   <item row="3" column="0" colspan="3">
-    <widget class="QCheckBox" name="sensorHilCheckBox">
-     <property name="text">
-      <string>Enable sensor level HIL</string>
-     </property>
-    </widget>
-   </item>
-   <item row="4" column="0" colspan="3">
+   <item row="7" column="0" colspan="3">
     <spacer name="verticalSpacer">
      <property name="orientation">
       <enum>Qt::Vertical</enum>
@@ -65,10 +53,35 @@
      </property>
     </spacer>
    </item>
-   <item row="1" column="1" colspan="2">
-    <widget class="QPushButton" name="startButton">
+   <item row="3" column="0" colspan="3">
+    <widget class="QCheckBox" name="sensorHilCheckBox">
      <property name="text">
-      <string>Start</string>
+      <string>Enable sensor level HIL</string>
+     </property>
+    </widget>
+   </item>
+   <item row="0" column="1" colspan="2">
+    <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="4" column="0" colspan="3">
+    <widget class="QCheckBox" name="useHilActuatorControlsCheckBox">
+     <property name="sizePolicy">
+      <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+       <horstretch>0</horstretch>
+       <verstretch>0</verstretch>
+      </sizepolicy>
+     </property>
+     <property name="text">
+      <string>Use newer actuator format</string>
      </property>
     </widget>
    </item>