From d51e6d466be77c14ca347306ef625754be3c2bac Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Mon, 20 Jul 2015 17:57:03 -0700 Subject: [PATCH] AP_HAL_SITL: add support for airspeed failures --- libraries/AP_HAL_SITL/sitl_ins.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/libraries/AP_HAL_SITL/sitl_ins.cpp b/libraries/AP_HAL_SITL/sitl_ins.cpp index fc869b90a3..aba22664a3 100644 --- a/libraries/AP_HAL_SITL/sitl_ins.cpp +++ b/libraries/AP_HAL_SITL/sitl_ins.cpp @@ -210,7 +210,8 @@ void SITL_State::_update_ins(float roll, float pitch, float yaw, // Relative sonar_pin_value = _ground_sonar(); - airspeed_pin_value = _airspeed_sensor(airspeed + (_sitl->aspd_noise * _rand_float())); + float airspeed_simulated = (fabsf(_sitl->aspd_fail) > 1.0e-6f) ? _sitl->aspd_fail : airspeed; + airspeed_pin_value = _airspeed_sensor(airspeed_simulated + (_sitl->aspd_noise * _rand_float())); } #endif