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

AP_HAL_SITL: use airspeed noise as pressure noise

Browse Source 
this emulates real sensors that have higher airspeed noise at
lower speeds, also change airspeed offset to a sitl parameter
This commit is contained in:
Harshit Kumar Sankhla 2020-07-16 15:47:54 +05:30 committed by Andrew Tridgell
parent 505953bdb2
commit 09620ee1e7
1 changed files with 22 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
libraries/AP_HAL_SITL/sitl_airspeed.cpp
View File

@ -25,28 +25,33 @@ using namespace HALSITL;
*/ */
void SITL_State::_update_airspeed(float airspeed) void SITL_State::_update_airspeed(float airspeed)
{ {
const float airspeed_ratio = 1.9936f;
const float airspeed_offset = 2013.0f;
float airspeed2 = airspeed; float airspeed2 = airspeed;
const float airspeed_ratio = 1.9936f;
const float diff_pressure = sq(airspeed) * 0.5;
// Check sensor failure // apply noise to the differential pressure. This emulates the way
airspeed = is_zero(_sitl->arspd_fail) ? airspeed : _sitl->arspd_fail; // airspeed noise reduces with speed
airspeed2 = is_zero(_sitl->arspd2_fail) ? airspeed2 : _sitl->arspd2_fail; airspeed = sqrtf(fabsf(2*(diff_pressure + _sitl->arspd_noise[0] * rand_float())));
// Add noise airspeed2 = sqrtf(fabsf(2*(diff_pressure + _sitl->arspd_noise[1] * rand_float())));
airspeed = airspeed + (_sitl->arspd_noise * rand_float());
airspeed2 = airspeed2 + (_sitl->arspd_noise * rand_float());
if (!is_zero(_sitl->arspd_fail_pressure)) { // 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[0])) {
// compute a realistic pressure report given some level of trapper air pressure in the tube and our current altitude // 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 // 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)); 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 // 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 // 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)); 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) airspeed_pressure *= -1;
if (_sitl->arspd_signflip) airspeed2_pressure *= -1; if (_sitl->arspd_signflip) airspeed2_pressure *= -1;
float airspeed_raw = airspeed_pressure + airspeed_offset; // apply airspeed sensor offset in m/s
float airspeed2_raw = airspeed2_pressure + airspeed_offset; float airspeed_raw = airspeed_pressure + _sitl->arspd_offset[0];
float airspeed2_raw = airspeed2_pressure + _sitl->arspd_offset[1];
if (airspeed_raw / 4 > 0xFFFF) { if (airspeed_raw / 4 > 0xFFFF) {
airspeed_pin_value = 0xFFFF; airspeed_pin_value = 0xFFFF;
return; return;