Cleanup on state HIL

src/comm/QGCFlightGearLink.cc

@@ -244,9 +244,9 @@ void QGCFlightGearLink::readBytes()
     double lat, lon, alt;
     double vx, vy, vz, xacc, yacc, zacc;
 
-    lat = values.at(1).toDouble() * 1e7;
-    lon = values.at(2).toDouble() * 1e7;
-    alt = values.at(3).toDouble() * 1e3;
+    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();
@@ -254,13 +254,13 @@ void QGCFlightGearLink::readBytes()
     pitchspeed = values.at(8).toDouble();
     yawspeed = values.at(9).toDouble();
 
-    xacc = values.at(10).toDouble()*1e3/9.8; // convert to mg's
-    yacc = values.at(11).toDouble()*1e3/9.8;
-    zacc = values.at(12).toDouble()*1e3/9.8;
+    xacc = values.at(10).toDouble();
+    yacc = values.at(11).toDouble();
+    zacc = values.at(12).toDouble();
 
-    vx = values.at(13).toDouble() * 1e2;
-    vy = values.at(14).toDouble() * 1e2;
-    vz = values.at(15).toDouble() * 1e2;
+    vx = values.at(13).toDouble();
+    vy = values.at(14).toDouble();
+    vz = values.at(15).toDouble();
 
     // Send updated state
     emit hilStateChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed,
@@ -299,7 +299,7 @@ bool QGCFlightGearLink::disconnectSimulation()
     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)));
+    disconnect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,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)));
 
     if (process)
     {
@@ -346,7 +346,7 @@ bool QGCFlightGearLink::connectSimulation()
     terraSync = new QProcess(this);
 
     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)));
+    connect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,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)));
 
 
     UAS* uas = dynamic_cast<UAS*>(mav);

src/comm/QGCHilLink.h

@@ -89,9 +89,9 @@ signals:
     void simulationConnected(bool connected);
 
     /** @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 hilStateChanged(quint64 time_us, float roll, float pitch, float yaw, float rollspeed,
+                                          float pitchspeed, float yawspeed, double lat, double lon, double alt,
+                                          float vx, float vy, float vz, float xacc, float yacc, float 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);
 

src/comm/QGCJSBSimLink.cc

@@ -271,7 +271,7 @@ bool QGCJSBSimLink::disconnectSimulation()
     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)));
+    disconnect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,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)));
 
     if (process)
     {
@@ -311,7 +311,7 @@ bool QGCJSBSimLink::connectSimulation()
     process = new QProcess(this);
 
     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)));
+    connect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,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)));
 
 
     UAS* uas = dynamic_cast<UAS*>(mav);

src/comm/QGCXPlaneLink.cc

@@ -694,7 +694,7 @@ 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(quint64,float,float,float,float,float,float,double,double,double,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)));
         disconnect(this, SIGNAL(sensorHilGpsChanged(quint64,double,double,double,int,float,float,float,float,int)), mav, SLOT(sendHilGps(quint64, 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)));
 
@@ -878,7 +878,7 @@ 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(quint64,float,float,float,float,float,float,double,double,double,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)));
     connect(this, SIGNAL(sensorHilGpsChanged(quint64,double,double,double,int,float,float,float,float,int)), mav, SLOT(sendHilGps(quint64, 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)));
 

src/uas/UAS.cc

@@ -2728,8 +2728,8 @@ void UAS::enableHilXPlane(bool enable)
 * @param zacc Z acceleration (mg)
 */
 void UAS::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)
+                       float pitchspeed, float yawspeed, double lat, double lon, double alt,
+                       float vx, float vy, float vz, float xacc, float yacc, float zacc)
 {
     if (this->mode & MAV_MODE_FLAG_HIL_ENABLED)
     {
@@ -2743,11 +2743,15 @@ void UAS::sendHilState(uint64_t time_us, float roll, float pitch, float yaw, flo
             emit valueChanged(uasId, "roll rate sim", "rad/s", rollspeed, getUnixTime());
             emit valueChanged(uasId, "pitch rate sim", "rad/s", pitchspeed, getUnixTime());
             emit valueChanged(uasId, "yaw rate sim", "rad/s", yawspeed, getUnixTime());
+
+            emit valueChanged(uasId, "vx sim", "rad", vx*100, getUnixTime());
+            emit valueChanged(uasId, "vy sim", "rad", vy*100, getUnixTime());
+            emit valueChanged(uasId, "vz sim", "rad", vz*100, getUnixTime());
         } else {
             mavlink_message_t msg;
             mavlink_msg_hil_state_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg,
                                        time_us, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed,
-                                       lat*1e7f, lon*1e7f, alt*1000, vx*100, vy*100, vz*100, xacc*1000, yacc*1000, zacc*1000);
+                                       lat*1e7f, lon*1e7f, alt*1000, vx*100, vy*100, vz*100, xacc*1000/9.81, yacc*1000/9.81, zacc*1000/9.81);
             sendMessage(msg);
         }
     }

src/uas/UAS.h

@@ -541,9 +541,9 @@ public slots:
     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,
-                        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 sendHilState(quint64 time_us, float roll, float pitch, float yaw, float rollspeed,
+                        float pitchspeed, float yawspeed, double lat, double lon, double alt,
+                        float vx, float vy, float vz, float xacc, float yacc, float zacc);
 
     /** @brief RAW sensors for sensor HIL */
     void sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, float rollspeed, float pitchspeed, float yawspeed,

src/uas/UASInterface.h

@@ -329,9 +329,9 @@ public slots:
     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;
+    virtual void sendHilState(quint64 time_us, float roll, float pitch, float yaw, float rollspeed,
+                        float pitchspeed, float yawspeed, double lat, double lon, double alt,
+                        float vx, float vy, float vz, float xacc, float yacc, float 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,