diff --git a/libraries/AP_HAL_SITL/sitl_airspeed.cpp b/libraries/AP_HAL_SITL/sitl_airspeed.cpp index eb197e6ab8..6d02307dba 100644 --- a/libraries/AP_HAL_SITL/sitl_airspeed.cpp +++ b/libraries/AP_HAL_SITL/sitl_airspeed.cpp @@ -34,13 +34,17 @@ void SITL_State::_update_airspeed(float airspeed) 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 + // 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 = 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_pressure = (airspeed * airspeed) / airspeed_ratio; + + // flip sign here for simulating reversed pitot/static connections + if (_sitl->arspd_signflip) airspeed_pressure *= -1; + float airspeed_raw = airspeed_pressure + airspeed_offset; if (airspeed_raw / 4 > 0xFFFF) { airspeed_pin_value = 0xFFFF; diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index e35c7d3751..18b025a355 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -107,6 +107,7 @@ const AP_Param::GroupInfo SITL::var_info2[] = { AP_GROUPINFO("ARSPD_FAIL_P", 6, SITL, arspd_fail_pressure, 0), AP_GROUPINFO("ARSPD_PITOT", 7, SITL, arspd_fail_pitot_pressure, 0), AP_GROUPINFO("GPS_ALT_OFS", 8, SITL, gps_alt_offset, 0), + AP_GROUPINFO("ARSPD_SIGN", 9, SITL, arspd_signflip, 0), AP_GROUPEND }; diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index fecabc5cba..cd954ab450 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -159,6 +159,9 @@ public: AP_Float temp_tconst; AP_Float temp_baro_factor; + // differential pressure sensor tube order + AP_Int8 arspd_signflip; + uint16_t irlock_port; void simstate_send(mavlink_channel_t chan);