From 7dc5da12470e4ea5fab08a7c1e2dcd75c0970d0d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 16 May 2022 15:27:01 +1000 Subject: [PATCH] HAL_SITL: cleanup SITL airspeed handling fixed handling of EAS2TAS, and fixed ratio per sensor. Removed the wind delay code (which was never being used). We should add a generic delay filter if we need this again --- libraries/AP_HAL/SIMState.h | 16 --- libraries/AP_HAL_SITL/AnalogIn.cpp | 4 +- libraries/AP_HAL_SITL/SITL_Periph_State.h | 3 +- libraries/AP_HAL_SITL/SITL_State.h | 4 +- libraries/AP_HAL_SITL/sitl_airspeed.cpp | 136 ++++++++-------------- 5 files changed, 55 insertions(+), 108 deletions(-) diff --git a/libraries/AP_HAL/SIMState.h b/libraries/AP_HAL/SIMState.h index 118d27baf0..44d159a41c 100644 --- a/libraries/AP_HAL/SIMState.h +++ b/libraries/AP_HAL/SIMState.h @@ -109,22 +109,6 @@ private: const char *_fg_address; - // delay buffer variables - static const uint8_t wind_buffer_length = 50; - - // airspeed sensor delay buffer variables - struct readings_wind { - uint32_t time; - float data; - }; - uint8_t store_index_wind; - uint32_t last_store_time_wind; - VectorN buffer_wind; - VectorN buffer_wind_2; - uint32_t time_delta_wind; - uint32_t delayed_time_wind; - uint32_t wind_start_delay_micros; - // internal SITL model SITL::Aircraft *sitl_model; diff --git a/libraries/AP_HAL_SITL/AnalogIn.cpp b/libraries/AP_HAL_SITL/AnalogIn.cpp index a31025db73..50ed481ee1 100644 --- a/libraries/AP_HAL_SITL/AnalogIn.cpp +++ b/libraries/AP_HAL_SITL/AnalogIn.cpp @@ -35,10 +35,10 @@ float ADCSource::read_latest() { return _sitlState->sonar_pin_value; case 1: - return _sitlState->airspeed_pin_value; + return _sitlState->airspeed_pin_value[0]; case 2: - return _sitlState->airspeed_2_pin_value; + return _sitlState->airspeed_pin_value[1]; case 12: return _sitlState->current_pin_value; diff --git a/libraries/AP_HAL_SITL/SITL_Periph_State.h b/libraries/AP_HAL_SITL/SITL_Periph_State.h index 6f89a13456..0c9a790053 100644 --- a/libraries/AP_HAL_SITL/SITL_Periph_State.h +++ b/libraries/AP_HAL_SITL/SITL_Periph_State.h @@ -41,8 +41,7 @@ public: // simulated airspeed, sonar and battery monitor uint16_t sonar_pin_value; // pin 0 - uint16_t airspeed_pin_value; // pin 1 - uint16_t airspeed_2_pin_value; // pin 2 + uint16_t airspeed_pin_value[2]; // pin 1 uint16_t voltage_pin_value; // pin 13 uint16_t current_pin_value; // pin 12 uint16_t voltage2_pin_value; // pin 15 diff --git a/libraries/AP_HAL_SITL/SITL_State.h b/libraries/AP_HAL_SITL/SITL_State.h index 337266f891..73e09dce5f 100644 --- a/libraries/AP_HAL_SITL/SITL_State.h +++ b/libraries/AP_HAL_SITL/SITL_State.h @@ -96,8 +96,7 @@ public: // simulated airspeed, sonar and battery monitor uint16_t sonar_pin_value; // pin 0 - uint16_t airspeed_pin_value; // pin 1 - uint16_t airspeed_2_pin_value; // pin 2 + uint16_t airspeed_pin_value[AIRSPEED_MAX_SENSORS]; // pin 1 uint16_t voltage_pin_value; // pin 13 uint16_t current_pin_value; // pin 12 uint16_t voltage2_pin_value; // pin 15 @@ -186,7 +185,6 @@ private: uint8_t store_index_wind; uint32_t last_store_time_wind; VectorN buffer_wind; - VectorN buffer_wind_2; uint32_t time_delta_wind; uint32_t delayed_time_wind; uint32_t wind_start_delay_micros; diff --git a/libraries/AP_HAL_SITL/sitl_airspeed.cpp b/libraries/AP_HAL_SITL/sitl_airspeed.cpp index c5508b9b0b..0bddc20d4b 100644 --- a/libraries/AP_HAL_SITL/sitl_airspeed.cpp +++ b/libraries/AP_HAL_SITL/sitl_airspeed.cpp @@ -20,101 +20,67 @@ extern const AP_HAL::HAL& hal; using namespace HALSITL; +// return current scale factor that converts from equivalent to true airspeed +// valid for altitudes up to 10km AMSL +// assumes standard atmosphere lapse rate +static float get_EAS2TAS(float altitude) +{ + float pressure = AP::baro().get_pressure(); + if (is_zero(pressure)) { + return 1.0f; + } + + float sigma, delta, theta; + AP_Baro::SimpleAtmosphere(altitude * 0.001, sigma, delta, theta); + + float tempK = C_TO_KELVIN(25) - ISA_LAPSE_RATE * altitude; + const float eas2tas_squared = SSL_AIR_DENSITY / (pressure / (ISA_GAS_CONSTANT * tempK)); + if (!is_positive(eas2tas_squared)) { + return 1.0; + } + return sqrtf(eas2tas_squared); +} + /* convert airspeed in m/s to an airspeed sensor value */ -void SITL_State::_update_airspeed(float airspeed) +void SITL_State::_update_airspeed(float true_airspeed) { - float airspeed2 = airspeed; - const float airspeed_ratio = 1.9936f; - const float diff_pressure = sq(airspeed) * 0.5; + for (uint8_t i=0; iairspeed[i]; + float airspeed = true_airspeed / get_EAS2TAS(_sitl->state.altitude); + const float diff_pressure = sq(airspeed) / arspd.ratio; + float airspeed_raw; - // apply noise to the differential pressure. This emulates the way - // airspeed noise reduces with speed - airspeed = sqrtf(fabsf(2*(diff_pressure + _sitl->arspd_noise[0] * rand_float()))); - airspeed2 = sqrtf(fabsf(2*(diff_pressure + _sitl->arspd_noise[1] * rand_float()))); + // apply noise to the differential pressure. This emulates the way + // airspeed noise reduces with speed + airspeed = sqrtf(fabsf(arspd.ratio*(diff_pressure + arspd.noise * rand_float()))); - // 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 - // algorithm taken from https://en.wikipedia.org/wiki/Calibrated_airspeed#Calculation_from_impact_pressure - float tube_pressure = fabsf(_sitl->arspd_fail_pressure[0] - AP::baro().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)); - } - 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 - // algorithm taken from https://en.wikipedia.org/wiki/Calibrated_airspeed#Calculation_from_impact_pressure - float tube_pressure = fabsf(_sitl->arspd_fail_pressure[1] - AP::baro().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)); - } - - float airspeed_pressure = (airspeed * airspeed) / airspeed_ratio; - float airspeed2_pressure = (airspeed2 * airspeed2) / airspeed_ratio; - - // flip sign here for simulating reversed pitot/static connections - if (_sitl->arspd_signflip) airspeed_pressure *= -1; - if (_sitl->arspd_signflip) airspeed2_pressure *= -1; - - // apply airspeed sensor offset in m/s - float airspeed_raw = airspeed_pressure + _sitl->arspd_offset[0]; - float airspeed2_raw = airspeed2_pressure + _sitl->arspd_offset[1]; - - _sitl->state.airspeed_raw_pressure[0] = airspeed_pressure; - _sitl->state.airspeed_raw_pressure[1] = airspeed2_pressure; - - if (airspeed_raw / 4 > 0xFFFF) { - airspeed_pin_value = 0xFFFF; - return; - } - if (airspeed2_raw / 4 > 0xFFFF) { - airspeed_2_pin_value = 0xFFFF; - return; - } - // add delay - const uint32_t now = AP_HAL::millis(); - uint32_t best_time_delta_wind = 200; // initialise large time representing buffer entry closest to current time - delay. - uint8_t best_index_wind = 0; // initialise number representing the index of the entry in buffer closest to delay. - - // storing data from sensor to buffer - if (now - last_store_time_wind >= 10) { // store data every 10 ms. - last_store_time_wind = now; - if (store_index_wind > wind_buffer_length - 1) { // reset buffer index if index greater than size of buffer - store_index_wind = 0; + // check sensor failure + if (is_positive(arspd.fail)) { + airspeed = arspd.fail; } - buffer_wind[store_index_wind].data = airspeed_raw; // add data to current index - buffer_wind[store_index_wind].time = last_store_time_wind; // add time to current index - buffer_wind_2[store_index_wind].data = airspeed2_raw; // add data to current index - buffer_wind_2[store_index_wind].time = last_store_time_wind; // add time to current index - store_index_wind = store_index_wind + 1; // increment index - } - // return delayed measurement - delayed_time_wind = now - _sitl->wind_delay; // get time corresponding to delay - // find data corresponding to delayed time in buffer - for (uint8_t i = 0; i <= wind_buffer_length - 1; i++) { - // find difference between delayed time and time stamp in buffer - time_delta_wind = abs( - (int32_t)(delayed_time_wind - buffer_wind[i].time)); - // if this difference is smaller than last delta, store this time - if (time_delta_wind < best_time_delta_wind) { - best_index_wind = i; - best_time_delta_wind = time_delta_wind; + if (!is_zero(arspd.fail_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 = fabsf(arspd.fail_pressure - AP::baro().get_pressure() + arspd.fail_pitot_pressure); + airspeed = 340.29409348 * sqrt(5 * (pow((tube_pressure / SSL_AIR_PRESSURE + 1), 2.0/7.0) - 1.0)); } - } - if (best_time_delta_wind < 200) { // only output stored state if < 200 msec retrieval error - airspeed_raw = buffer_wind[best_index_wind].data; - airspeed2_raw = buffer_wind_2[best_index_wind].data; - } + float airspeed_pressure = (airspeed * airspeed) / arspd.ratio; - airspeed_pin_value = airspeed_raw / 4; - airspeed_2_pin_value = airspeed2_raw / 4; + // flip sign here for simulating reversed pitot/static connections + if (arspd.signflip) { + airspeed_pressure *= -1; + } + + // apply airspeed sensor offset in m/s + airspeed_raw = airspeed_pressure + arspd.offset; + + _sitl->state.airspeed_raw_pressure[i] = airspeed_pressure; + + airspeed_pin_value[i] = MIN(0xFFFF, airspeed_raw / 4); + } } #endif