From 58e784024f0b5f79a68573dadb9b63f12bf5236c Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Tue, 1 Aug 2017 13:44:32 -0700 Subject: [PATCH] AP_HAL_SITL: Add support for blocked pitot tube simulations --- libraries/AP_HAL_SITL/sitl_airspeed.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/libraries/AP_HAL_SITL/sitl_airspeed.cpp b/libraries/AP_HAL_SITL/sitl_airspeed.cpp index 379ad2645f..eb197e6ab8 100644 --- a/libraries/AP_HAL_SITL/sitl_airspeed.cpp +++ b/libraries/AP_HAL_SITL/sitl_airspeed.cpp @@ -33,6 +33,13 @@ void SITL_State::_update_airspeed(float airspeed) // Add noise airspeed = airspeed + (_sitl->arspd_noise * rand_float()); + if (!is_zero(_sitl->arspd_fail_pressure)) { + // compute a realistic pressure report given some level of trapper air pressure in the tube abd our current altitude + // algorithim taken from https://en.wikipedia.org/wiki/Calibrated_airspeed#Calculation_from_impact_pressure + float tube_pressure = abs(_sitl->arspd_fail_pressure - _barometer->get_pressure() + _sitl->arspd_fail_pitot_pressure); + airspeed = 340.29409348 * sqrt(5 * (pow((tube_pressure / 101325.01576 + 1), 2.0/7.0) - 1.0)); + } + const float airspeed_pressure = (airspeed * airspeed) / airspeed_ratio; float airspeed_raw = airspeed_pressure + airspeed_offset; if (airspeed_raw / 4 > 0xFFFF) {