Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: add parameter to simulate reversed pitot tube order

Browse Source
This commit is contained in:
Mark Whitehorn 2017-09-19 21:13:49 -06:00 committed by Andrew Tridgell
parent f32f51f843
commit dedb40ce0b
3 changed files with 11 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_HAL_SITL/sitl_airspeed.cpp
View File

@ -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;

1
libraries/SITL/SITL.cpp
View File

@ -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
};

3
libraries/SITL/SITL.h
View File

@ -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);