Negative airspeeds from negative differential pressures. (#3126)

* Negative airspeeds from negative differential pressures. * Some protections against negative airspeeds
paparazzi · Oct 5, 2023 · 889521b · 889521b
1 parent 5b25426
commit 889521b
Show file tree

Hide file tree

Showing 5 changed files with 22 additions and 8 deletions.
diff --git a/sw/airborne/modules/air_data/air_data.c b/sw/airborne/modules/air_data/air_data.c
@@ -327,7 +327,12 @@ float eas_from_dynamic_pressure(float q)
    * Lower bound of q at zero, no flying backwards guys...
    */
   const float two_div_rho_0 = 2.0 / PPRZ_ISA_AIR_DENSITY;
-  return sqrtf(Max(q * two_div_rho_0, 0));
+  float sign = 1.0f;
+  if (q < 0) {
+    sign = -1.0f;
+    q = -q;
+  }
+  return sqrtf(q * two_div_rho_0) * sign;
 }
 
 /**

diff --git a/sw/airborne/modules/datalink/mavlink.c b/sw/airborne/modules/datalink/mavlink.c
@@ -825,7 +825,7 @@ static void mavlink_send_vfr_hud(struct transport_tx *trans, struct link_device
 #endif
   float hmsl_alt = state.ned_origin_f.hmsl - state.ned_origin_f.lla.alt;
   mavlink_msg_vfr_hud_send(MAVLINK_COMM_0,
-                           stateGetAirspeed_f(),
+                           Max(0,stateGetAirspeed_f()),
                            stateGetHorizontalSpeedNorm_f(), // groundspeed
                            heading,
                            throttle,

diff --git a/sw/airborne/modules/nav/nav_drop.c b/sw/airborne/modules/nav/nav_drop.c
@@ -161,8 +161,8 @@ unit_t nav_drop_compute_approach(uint8_t wp_target, uint8_t wp_start, uint8_t wp
 
   // wind in NED frame
   if (stateIsAirspeedValid()) {
-    nav_drop_vx = x1 * stateGetAirspeed_f() + stateGetHorizontalWindspeed_f()->y;
-    nav_drop_vy = y_1 * stateGetAirspeed_f() + stateGetHorizontalWindspeed_f()->x;
+    nav_drop_vx = x1 * Max(0, stateGetAirspeed_f()) + stateGetHorizontalWindspeed_f()->y;
+    nav_drop_vy = y_1 * Max(0, stateGetAirspeed_f()) + stateGetHorizontalWindspeed_f()->x;
   } else {
     // use approximate airspeed, initially set to AIRSPEED_AT_RELEASE
     nav_drop_vx = x1 * airspeed + stateGetHorizontalWindspeed_f()->y;

diff --git a/sw/airborne/modules/sensors/airspeed_ms45xx_i2c.c b/sw/airborne/modules/sensors/airspeed_ms45xx_i2c.c
@@ -141,10 +141,11 @@ PRINT_CONFIG_VAR(MS45XX_PRESSURE_OFFSET)
  */
 #ifdef MS45XX_AIRSPEED_SCALE
 PRINT_CONFIG_MSG("MS45XX changed air density. PS: Use MS45XX_PRESSURE_SCALE to calibrate the MS45XX.");
-#else
-#define MS45XX_AIRSPEED_SCALE 1.6327
 #endif
 
+#define MS45XX_RHO_DIV_2 1.6327
+
+
 /** Time constant for second order Butterworth low pass filter
  * Default of 0.15 should give cut-off freq of 1/(2*pi*tau) ~= 1Hz
  */
@@ -269,7 +270,11 @@ void ms45xx_i2c_event(void)
       float temp = ms45xx.temperature / 10.0f;
       AbiSendMsgTEMPERATURE(MS45XX_SENDER_ID, temp);
       // Compute airspeed
-      ms45xx.airspeed = sqrtf(Max(ms45xx.pressure, 0)) * MS45XX_AIRSPEED_SCALE;
+      float sign = 1.0f;
+      if (ms45xx.pressure < 0.0f) {
+        sign = -1.0f;
+      }
+      ms45xx.airspeed = sqrtf(ms45xx.pressure * sign * MS45XX_RHO_DIV_2) * sign;
 
     }
 

diff --git a/sw/airborne/modules/sensors/airspeed_uavcan.c b/sw/airborne/modules/sensors/airspeed_uavcan.c
@@ -64,7 +64,11 @@ static void airspeed_uavcan_downlink(struct transport_tx *trans, struct link_dev
   uint8_t dev_id = UAVCAN_SENDER_ID;
   uint16_t raw = 0;
   float offset = 0;
-  float airspeed = 0;
+  float sign = 1.0f;
+  if (airspeed_uavcan.diff_p < 0) {
+    sign = -1.0f;
+  }
+  float airspeed = sqrt(airspeed_uavcan.diff_p * sign * 2.0f / 1.225f) * sign;
   pprz_msg_send_AIRSPEED_RAW(trans,dev,AC_ID,
                                 &dev_id,
                                 &raw,