From 09620ee1e75a78bbfc32d14f6202bc3f380b1982 Mon Sep 17 00:00:00 2001
From: Harshit Kumar Sankhla <harshitsankhla@hotmail.com>
Date: Thu, 16 Jul 2020 15:47:54 +0530
Subject: [PATCH] AP_HAL_SITL: use airspeed noise as pressure noise

this emulates real sensors that have higher airspeed noise at
lower speeds, also change airspeed offset to a sitl parameter
---
 libraries/AP_HAL_SITL/sitl_airspeed.cpp | 37 +++++++++++++++----------
 1 file changed, 22 insertions(+), 15 deletions(-)

diff --git a/libraries/AP_HAL_SITL/sitl_airspeed.cpp b/libraries/AP_HAL_SITL/sitl_airspeed.cpp
index 2b9b7b20fd..41ddc1809f 100644
--- a/libraries/AP_HAL_SITL/sitl_airspeed.cpp
+++ b/libraries/AP_HAL_SITL/sitl_airspeed.cpp
@@ -25,28 +25,33 @@ using namespace HALSITL;
  */
 void SITL_State::_update_airspeed(float airspeed)
 {
-    const float airspeed_ratio = 1.9936f;
-    const float airspeed_offset = 2013.0f;
-    
     float airspeed2 = airspeed;
+    const float airspeed_ratio = 1.9936f;
+    const float diff_pressure = sq(airspeed) * 0.5;
+    
+    // apply noise to the differential pressure. This emulates the way
+    // airspeed noise reduces with speed
+    airspeed = sqrtf(fabsf(2*(diff_pressure + _sitl->arspd_noise[0] * rand_float())));
+    airspeed2 = sqrtf(fabsf(2*(diff_pressure + _sitl->arspd_noise[1] * rand_float())));
 
-    // Check sensor failure
-    airspeed = is_zero(_sitl->arspd_fail) ? airspeed : _sitl->arspd_fail;
-    airspeed2 = is_zero(_sitl->arspd2_fail) ? airspeed2 : _sitl->arspd2_fail;
-    // Add noise
-    airspeed = airspeed + (_sitl->arspd_noise * rand_float());
-    airspeed2 = airspeed2 + (_sitl->arspd_noise * rand_float());
+    // check sensor failure
+    if (is_positive(_sitl->arspd_fail[0])) {
+        airspeed = _sitl->arspd_fail[0];
+    }
+    if (is_positive(_sitl->arspd_fail[1])) {
+        airspeed2 = _sitl->arspd_fail[1];
+    }
 
-    if (!is_zero(_sitl->arspd_fail_pressure)) {
+    if (!is_zero(_sitl->arspd_fail_pressure[0])) {
         // compute a realistic pressure report given some level of trapper air pressure in the tube and our current altitude
         // algorithm taken from https://en.wikipedia.org/wiki/Calibrated_airspeed#Calculation_from_impact_pressure
-        float tube_pressure = fabsf(_sitl->arspd_fail_pressure - _barometer->get_pressure() + _sitl->arspd_fail_pitot_pressure);
+        float tube_pressure = fabsf(_sitl->arspd_fail_pressure[0] - _barometer->get_pressure() + _sitl->arspd_fail_pitot_pressure[0]);
         airspeed = 340.29409348 * sqrt(5 * (pow((tube_pressure / SSL_AIR_PRESSURE + 1), 2.0/7.0) - 1.0));
     }
-    if (!is_zero(_sitl->arspd2_fail_pressure)) {
+    if (!is_zero(_sitl->arspd_fail_pressure[1])) {
         // compute a realistic pressure report given some level of trapper air pressure in the tube and our current altitude
         // algorithm taken from https://en.wikipedia.org/wiki/Calibrated_airspeed#Calculation_from_impact_pressure
-        float tube_pressure = fabsf(_sitl->arspd2_fail_pressure - _barometer->get_pressure() + _sitl->arspd2_fail_pitot_pressure);
+        float tube_pressure = fabsf(_sitl->arspd_fail_pressure[1] - _barometer->get_pressure() + _sitl->arspd_fail_pitot_pressure[1]);
         airspeed2 = 340.29409348 * sqrt(5 * (pow((tube_pressure / SSL_AIR_PRESSURE + 1), 2.0/7.0) - 1.0));
     }
 
@@ -57,8 +62,10 @@ void SITL_State::_update_airspeed(float airspeed)
     if (_sitl->arspd_signflip) airspeed_pressure *= -1;
     if (_sitl->arspd_signflip) airspeed2_pressure *= -1;
 
-    float airspeed_raw = airspeed_pressure + airspeed_offset;
-    float airspeed2_raw = airspeed2_pressure + airspeed_offset;
+    // apply airspeed sensor offset in m/s
+    float airspeed_raw = airspeed_pressure + _sitl->arspd_offset[0];
+    float airspeed2_raw = airspeed2_pressure + _sitl->arspd_offset[1];
+
     if (airspeed_raw / 4 > 0xFFFF) {
         airspeed_pin_value = 0xFFFF;
         return;