Working on sensor HIL

src/comm/QGCHilLink.h

@@ -80,10 +80,19 @@ signals:
      **/
     void simulationConnected(bool connected);
 
-    /** @brief State update from FlightGear */
+    /** @brief State update from simulation */
     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);
+
+    void sensorHilGpsChanged(quint64 time_us, double lat, double lon, double alt, int fix_type, float eph, float epv, float vel, float cog, int satellites);
+
+    void sensorHilRawImuChanged(quint64 time_us, float xacc, float yacc, float zacc,
+                                                  float xgyro, float ygyro, float zgyro,
+                                                  float xmag, float ymag, float zmag,
+                                                  float abs_pressure, float diff_pressure,
+                                                  float pressure_alt, float temperature,
+                                                  quint16 fields_updated);
     
     /** @brief Remote host and port changed */
     void remoteChanged(const QString& hostPort);

src/comm/QGCXPlaneLink.cc

@@ -53,7 +53,8 @@ QGCXPlaneLink::QGCXPlaneLink(UASInterface* mav, QString remoteHost, QHostAddress
     xPlaneVersion(0),
     simUpdateLast(QGC::groundTimeMilliseconds()),
     simUpdateLastText(QGC::groundTimeMilliseconds()),
-    simUpdateHz(0)
+    simUpdateHz(0),
+    sensorHilEnabled(true)
 {
     this->localHost = localHost;
     this->localPort = localPort/*+mav->getUASID()*/;
@@ -390,6 +391,7 @@ void QGCXPlaneLink::readBytes()
 {
     // Only emit updates on attitude message
     bool emitUpdate = false;
+    quint16 fields_changed = 0;
 
     const qint64 maxLength = 1000;
     char data[maxLength];
@@ -440,6 +442,12 @@ void QGCXPlaneLink::readBytes()
 
                 //qDebug() << "SPEEDS:" << "airspeed" << airspeed << "m/s, groundspeed" << groundspeed << "m/s";
             }
+            else if (p.index == 6 && xPlaneVersion == 10)
+            {
+                // inHg to kPa
+                abs_pressure = p.f[0] * 3.3863886666718317f;
+                temperature = p.f[1];
+            }
             // Forward controls from X-Plane to MAV, not very useful
             // better: Connect Joystick to QGroundControl
 //            else if (p.index == 8)
@@ -457,6 +465,7 @@ void QGCXPlaneLink::readBytes()
                 rollspeed = p.f[2];
                 pitchspeed = p.f[1];
                 yawspeed = p.f[0];
+                fields_changed |= (1 << 0) | (1 << 1) | (1 << 2);
             }
             else if ((xPlaneVersion == 10 && p.index == 17) || (xPlaneVersion == 9 && p.index == 18))
             {
@@ -471,6 +480,20 @@ void QGCXPlaneLink::readBytes()
                 if (yaw < -M_PI) {
                     yaw += 2.0 * M_PI;
                 }
+
+                float yawmag = p.f[3];
+
+                if (yawmag > M_PI) {
+                    yawmag -= 2.0 * M_PI;
+                }
+                if (yawmag < -M_PI) {
+                    yawmag += 2.0 * M_PI;
+                }
+
+                xmag = cos(yawmag) * 0.4f - sin(yawmag) * 0.0f + 0.0f;
+                ymag = sin(yawmag) * 0.4f - sin(yawmag) * 0.0f + 0.0f;
+                zmag = 0.0f            + 0.0f            + 1.0f * 0.4f;
+
                 emitUpdate = true;
             }
             else if ((xPlaneVersion == 9 && p.index == 17))
@@ -478,6 +501,7 @@ void QGCXPlaneLink::readBytes()
                 rollspeed = p.f[2];
                 pitchspeed = p.f[1];
                 yawspeed = p.f[0];
+                fields_changed |= (1 << 0) | (1 << 1) | (1 << 2);
             }
 
 //            else if (p.index == 19)
@@ -491,6 +515,12 @@ void QGCXPlaneLink::readBytes()
                 lon = p.f[1];
                 alt = p.f[2] * 0.3048f; // convert feet (MSL) to meters
             }
+            else if (p.index == 21 && xPlaneVersion == 10)
+            {
+                vx = p.f[3];
+                vy = p.f[4];
+                vz = p.f[5];
+            }
             else if (p.index == 12)
             {
                 //qDebug() << "AIL/ELEV/RUD" << p.f[0] << p.f[1] << p.f[2];
@@ -545,9 +575,27 @@ void QGCXPlaneLink::readBytes()
         }
         simUpdateLast = QGC::groundTimeMilliseconds();
 
-        emit hilStateChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed,
+        if (sensorHilEnabled)
+        {
+            diff_pressure = 0.0f;
+            pressure_alt = alt;
+
+            emit sensorHilRawImuChanged(QGC::groundTimeUsecs(), xacc, yacc, zacc, rollspeed, pitchspeed, yawspeed,
+                                        xmag, ymag, zmag, abs_pressure, diff_pressure, pressure_alt, temperature, fields_changed);
+
+            int gps_fix_type = 3;
+            float eph = 0.3;
+            float epv = 0.6;
+            float vel = sqrt(vx*vx + vy*vy + vz*vz);
+            float cog = yaw;
+            int satellites = 8;
+
+            emit sensorHilGpsChanged(QGC::groundTimeUsecs(), lat, lon, alt, gps_fix_type, eph, epv, vel, cog, satellites);
+        } else {
+            emit hilStateChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed,
                          pitchspeed, yawspeed, lat*1E7, lon*1E7, alt*1E3,
                          vx, vy, vz, xacc*1000, yacc*1000, zacc*1000);
+        }
     }
 
     if (!oldConnectionState && xPlaneConnected)
@@ -594,7 +642,11 @@ bool QGCXPlaneLink::disconnectSimulation()
     {
         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(mav, SIGNAL(hilActuatorsChanged(uint64_t, float, float, float, float, float, float, float, float)), this, SLOT(updateActuators(uint64_t,float,float,float,float,float,float,float,float)));
-        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)));
+
+        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(quint64,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)));
+        disconnect(this, SIGNAL(sensorHilGpsChanged(quint64,double,double,double,int,float,float,float,float,int)), mav, SLOT(sendHilGps(uint64_t, double, double, double, int, float, float, float, float, int)));
+        disconnect(this, SIGNAL(sensorHilRawImuChanged(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint16)), mav, SLOT(sendHilSensors(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint16)));
+
         UAS* uas = dynamic_cast<UAS*>(mav);
         if (uas)
         {
@@ -774,7 +826,10 @@ 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(mav, SIGNAL(hilActuatorsChanged(uint64_t, float, float, float, float, float, float, float, float)), this, SLOT(updateActuators(uint64_t,float,float,float,float,float,float,float,float)));
-    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)));
+
+    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(quint64,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)));
+    connect(this, SIGNAL(sensorHilGpsChanged(quint64,double,double,double,int,float,float,float,float,int)), mav, SLOT(sendHilGps(uint64_t, double, double, double, int, float, float, float, float, int)));
+    connect(this, SIGNAL(sensorHilRawImuChanged(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint16)), mav, SLOT(sendHilSensors(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint16)));
 
     UAS* uas = dynamic_cast<UAS*>(mav);
     if (uas)

src/comm/QGCXPlaneLink.h

@@ -184,6 +184,7 @@ protected:
     float vx, vy, vz, xacc, yacc, zacc;
     float airspeed;
     float groundspeed;
+    float xmag, ymag, zmag, abs_pressure, diff_pressure, pressure_alt, temperature;
 
     float man_roll, man_pitch, man_yaw;
     QString airframeName;
@@ -193,6 +194,7 @@ protected:
     quint64 simUpdateLast;
     quint64 simUpdateLastText;
     float simUpdateHz;
+    bool sensorHilEnabled;
 
     void setName(QString name);
 

src/uas/UAS.cc

@@ -2706,6 +2706,48 @@ void UAS::sendHilState(uint64_t time_us, float roll, float pitch, float yaw, flo
     }
 }
 
+void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, float rollspeed, float pitchspeed, float yawspeed,
+                                    float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, quint16 fields_changed)
+{
+    if (this->mode & MAV_MODE_FLAG_HIL_ENABLED)
+    {
+        mavlink_message_t msg;
+        mavlink_msg_highres_imu_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg,
+                                   time_us, xacc, yacc, zacc, rollspeed, pitchspeed, yawspeed,
+                                     xmag, ymag, zmag, abs_pressure, diff_pressure, pressure_alt, temperature,
+                                     fields_changed);
+        sendMessage(msg);
+    }
+    else
+    {
+        // Attempt to set HIL mode
+        mavlink_message_t msg;
+        mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode | MAV_MODE_FLAG_HIL_ENABLED, navMode);
+        sendMessage(msg);
+        qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable.";
+    }
+}
+
+void UAS::sendHilGps(quint64 time_us, double lat, double lon, double alt, int fix_type, float eph, float epv, float vel, float cog, int satellites)
+{
+    if (this->mode & MAV_MODE_FLAG_HIL_ENABLED)
+    {
+        mavlink_message_t msg;
+        mavlink_msg_gps_raw_int_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg,
+                                   time_us, fix_type, lat*1e7, lon*1e7, alt*1e3, eph*1e2, epv*1e2, vel*1e2, cog*1e2, satellites);
+        sendMessage(msg);
+    }
+    else
+    {
+        // Attempt to set HIL mode
+        mavlink_message_t msg;
+        mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode | MAV_MODE_FLAG_HIL_ENABLED, navMode);
+        sendMessage(msg);
+        qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable.";
+    }
+}
+
+
 /**
 * Connect flight gear link.
 **/

src/uas/UAS.h

@@ -534,13 +534,19 @@ public slots:
     void enableHilFlightGear(bool enable, QString options);
     void enableHilXPlane(bool enable);
 
-
     /** @brief Send the full HIL state to the MAV */
-
-    void sendHilState(	uint64_t time_us, float roll, float pitch, float yaw, float rollspeed,
+    void sendHilState(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);
 
+    /** @brief RAW sensors for sensor HIL */
+    void sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, float rollspeed, float pitchspeed, float yawspeed,
+                                        float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, quint16 fields_changed);
+
+    /** @brief Send raw GPS for sensor HIL */
+    void sendHilGps(quint64 time_us, double lat, double lon, double alt, int fix_type, float eph, float epv, float vel, float cog, int satellites);
+
+
     /** @brief Places the UAV in Hardware-in-the-Loop simulation status **/
     void startHil();
 

src/uas/UASInterface.h

@@ -328,6 +328,19 @@ public slots:
     /** @brief Get the current battery type and specs */
     virtual QString getBatterySpecs() = 0;
 
+    /** @brief Send the full HIL state to the MAV */
+    virtual void sendHilState(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) = 0;
+
+    /** @brief RAW sensors for sensor HIL */
+    virtual void sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, float rollspeed, float pitchspeed, float yawspeed,
+                                        float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, quint16 fields_changed) = 0;
+
+    /** @brief Send raw GPS for sensor HIL */
+    virtual void sendHilGps(quint64 time_us, double lat, double lon, double alt, int fix_type, float eph, float epv, float vel, float cog, int satellites) = 0;
+
+
 protected:
     QColor color;
 

src/ui/QGCVehicleConfig.cc

@@ -475,7 +475,7 @@ void QGCVehicleConfig::parameterChanged(int uas, int component, QString paramete
 
     if (minTpl.exactMatch(parameterName)) {
         bool ok;
-        unsigned int index = parameterName.mid(2, 1).toInt(&ok) - 1;
+        int index = parameterName.mid(2, 1).toInt(&ok) - 1;
         //qDebug() << "PARAM:" << parameterName << "index:" << index;
         if (ok && (index >= 0) && (index < chanMax))
         {
@@ -485,7 +485,7 @@ void QGCVehicleConfig::parameterChanged(int uas, int component, QString paramete
 
     if (maxTpl.exactMatch(parameterName)) {
         bool ok;
-        unsigned int index = parameterName.mid(2, 1).toInt(&ok) - 1;
+        int index = parameterName.mid(2, 1).toInt(&ok) - 1;
         if (ok && (index >= 0) && (index < chanMax))
         {
             rcMax[index] = value.toInt();
@@ -494,7 +494,7 @@ void QGCVehicleConfig::parameterChanged(int uas, int component, QString paramete
 
     if (trimTpl.exactMatch(parameterName)) {
         bool ok;
-        unsigned int index = parameterName.mid(2, 1).toInt(&ok) - 1;
+        int index = parameterName.mid(2, 1).toInt(&ok) - 1;
         if (ok && (index >= 0) && (index < chanMax))
         {
             rcTrim[index] = value.toInt();
@@ -503,7 +503,7 @@ void QGCVehicleConfig::parameterChanged(int uas, int component, QString paramete
 
     if (revTpl.exactMatch(parameterName)) {
         bool ok;
-        unsigned int index = parameterName.mid(2, 1).toInt(&ok) - 1;
+        int index = parameterName.mid(2, 1).toInt(&ok) - 1;
         if (ok && (index >= 0) && (index < chanMax))
         {
             rcRev[index] = (value.toInt() == -1) ? true : false;