Merge pull request #401 from thomasgubler/hil_flightgear

flightgear hil: correct scaling of pressure for mavlink message and remo...

files/flightgear/Protocol/qgroundcontrol-fixed-wing.xml

@@ -142,14 +142,6 @@
      <factor>0.514444444444444</factor>      <!-- knots to mps -->
    </chunk>
 
-   <chunk>
-     <name>airspeed-indicated-mps</name>
-     <type>float</type>
-     <format>%.8f</format>
-     <node>/instrumentation/indicated-speed-kt</node>
-     <factor>0.514444444444444</factor>      <!-- knots to mps -->
-   </chunk>
-
    <!-- Magnetometer -->
    <chunk>
     <name>Magnetic Variation (rad)</name>

src/comm/QGCFlightGearLink.cc

@@ -235,9 +235,10 @@ void QGCFlightGearLink::readBytes()
     QStringList values = state.split("\t");
 
     // Check length
-    if (values.size() != 22)
+    const int nValues = 21;
+    if (values.size() != nValues)
     {
-        qDebug() << "RETURN LENGTH MISMATCHING EXPECTED" << 22 << "BUT GOT" << values.size();
+        qDebug() << "RETURN LENGTH MISMATCHING EXPECTED" << nValues << "BUT GOT" << values.size();
         qDebug() << state;
         return;
     }
@@ -273,13 +274,12 @@ void QGCFlightGearLink::readBytes()
     vz = values.at(15).toFloat();
 
     true_airspeed = values.at(16).toFloat();
-    ind_airspeed = values.at(17).toFloat();
 
-    mag_variation = values.at(18).toFloat();
-    mag_dip = values.at(19).toFloat();
+    mag_variation = values.at(17).toFloat();
+    mag_dip = values.at(18).toFloat();
 
-    temperature = values.at(20).toFloat();
-    abs_pressure = values.at(21).toFloat();
+    temperature = values.at(19).toFloat();
+    abs_pressure = values.at(20).toFloat()*1e2; //convert to Pa from hPa
 
     // Send updated state
     //qDebug()  << "sensorHilEnabled: " << sensorHilEnabled;
@@ -292,6 +292,19 @@ void QGCFlightGearLink::readBytes()
         const float absolute_null_celsius = -273.15f; // °C
         float density = abs_pressure / (air_gas_constant * (temperature - absolute_null_celsius));
         diff_pressure = true_airspeed * true_airspeed * density / 2.0f;
+        //qDebug() << "diff_pressure: " << diff_pressure << "abs_pressure: " << abs_pressure;
+
+        /* Calculate indicated airspeed */
+        const float air_density_sea_level_15C  = 1.225f; //kg/m^3
+        if (diff_pressure > 0)
+        {
+            ind_airspeed =  sqrtf((2.0f*diff_pressure) / air_density_sea_level_15C);
+        } else
+        {
+            ind_airspeed =  -sqrtf((2.0f*fabsf(diff_pressure)) / air_density_sea_level_15C);
+        }
+
+        //qDebug() << "ind_airspeed: " << ind_airspeed << "true_airspeed: " << true_airspeed;
 
         float pressure_alt = alt;
 
@@ -336,7 +349,7 @@ void QGCFlightGearLink::readBytes()
         zmag_body = mag_body(2);
 
         emit sensorHilRawImuChanged(QGC::groundTimeUsecs(), xacc, yacc, zacc, rollspeed, pitchspeed, yawspeed,
-                                    xmag_body, ymag_body, zmag_body, abs_pressure, diff_pressure, pressure_alt, temperature, fields_changed);
+                                    xmag_body, ymag_body, zmag_body, abs_pressure*1e-2f, diff_pressure*1e-2f, pressure_alt, temperature, fields_changed); //Pressure in hPa for mavlink
 
 //        qDebug()  << "sensorHilRawImuChanged " << xacc  << yacc << zacc  << rollspeed << pitchspeed << yawspeed << xmag << ymag << zmag << abs_pressure << diff_pressure << pressure_alt << temperature;
         int gps_fix_type = 3;

src/uas/UAS.cc

@@ -3124,6 +3124,10 @@ void UAS::sendHilState(quint64 time_us, float roll, float pitch, float yaw, floa
     }
 }
 
+/*
+* @param abs_pressure Absolute Pressure (hPa)
+* @param diff_pressure Differential Pressure  (hPa)
+*/
 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, quint32 fields_changed)
 {