From 20342216e23b8fda155196e1b27afe05a0a65fea Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 20 Dec 2022 15:52:54 +0100 Subject: [PATCH] Airspeed Selector: use better density source and only save scale parameter if valid (#20764) * AirspeedSelector: use vehicle_air_data.rho for calculating groundspeed-wind CAS Previously the vehicle_air_data.temperature and pressure was used, instead of the density field directly. Only makes a difference if there is an airspeed sensor connected to provide the air temperature. * AirspeedSelector: only safe estimated scale in param if airspeed is valid * AirspeedSelector: remove 0.01 cliff for saving learned scale to param Signed-off-by: Silvan Fuhrer --- src/lib/airspeed/airspeed.cpp | 4 +- src/lib/airspeed/airspeed.h | 13 +++--- .../airspeed_selector_main.cpp | 45 +++++++++---------- 3 files changed, 29 insertions(+), 33 deletions(-) diff --git a/src/lib/airspeed/airspeed.cpp b/src/lib/airspeed/airspeed.cpp index bd47f0930f..24f7f01850 100644 --- a/src/lib/airspeed/airspeed.cpp +++ b/src/lib/airspeed/airspeed.cpp @@ -228,7 +228,7 @@ float get_air_density(float static_pressure, float temperature_celsius) return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS)); } -float calc_CAS_from_TAS(float speed_true, float pressure_ambient, float temperature_celsius) +float calc_calibrated_from_true_airspeed(float speed_true, float air_density) { - return speed_true * sqrtf(get_air_density(pressure_ambient, temperature_celsius) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); + return speed_true * sqrtf(air_density / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); } diff --git a/src/lib/airspeed/airspeed.h b/src/lib/airspeed/airspeed.h index 74d273054e..dba000a4e5 100644 --- a/src/lib/airspeed/airspeed.h +++ b/src/lib/airspeed/airspeed.h @@ -130,16 +130,13 @@ __EXPORT float calc_TAS(float total_pressure, float static_pressure, float tempe __EXPORT float get_air_density(float static_pressure, float temperature_celsius); /** - * Calculate calibrated airspeed (CAS) from true airspeed (TAS). - * It is the inverse function to calc_TAS_from_CAS() + * @brief Calculates calibrated airspeed from true airspeed and air density * - * @param speed_true current true airspeed - * @param pressure_ambient pressure at the side of the tube/airplane - * @param temperature_celsius air temperature in degrees Celsius - * @return CAS in m/s + * @param speed_true true airspeed [m/s] + * @param air_density air density [kg/m3] + * @return calibrated airspeed [m/s] */ -__EXPORT float calc_CAS_from_TAS(float speed_true, float pressure_ambient, - float temperature_celsius); +__EXPORT float calc_calibrated_from_true_airspeed(float speed_true, float air_density); __END_DECLS diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index 9cccb1247e..5098a0bb08 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -397,32 +397,32 @@ AirspeedModule::Run() } - // save estimated airspeed scale after disarm + // save estimated airspeed scale after disarm if airspeed is valid and scale has changed if (!armed && _armed_prev) { - if (_param_aspd_scale_apply.get() > 0) { - if (fabsf(_airspeed_validator[i].get_CAS_scale_validated() - _param_airspeed_scale[i]) > 0.01f) { - // apply the new scale if changed more than 0.01 - mavlink_log_info(&_mavlink_log_pub, "Airspeed sensor Nr. %d ASPD_SCALE updated: %.2f --> %.2f", i + 1, - (double)_param_airspeed_scale[i], - (double)_airspeed_validator[i].get_CAS_scale_validated()); + if (_param_aspd_scale_apply.get() > 0 && _airspeed_validator[i].get_airspeed_valid() + && fabsf(_airspeed_validator[i].get_CAS_scale_validated() - _param_airspeed_scale[i]) > FLT_EPSILON) { - switch (i) { - case 0: - _param_airspeed_scale_1.set(_airspeed_validator[i].get_CAS_scale_validated()); - _param_airspeed_scale_1.commit_no_notification(); - break; + mavlink_log_info(&_mavlink_log_pub, "Airspeed sensor Nr. %d ASPD_SCALE updated: %.4f --> %.4f", i + 1, + (double)_param_airspeed_scale[i], + (double)_airspeed_validator[i].get_CAS_scale_validated()); - case 1: - _param_airspeed_scale_2.set(_airspeed_validator[i].get_CAS_scale_validated()); - _param_airspeed_scale_2.commit_no_notification(); - break; + switch (i) { + case 0: + _param_airspeed_scale_1.set(_airspeed_validator[i].get_CAS_scale_validated()); + _param_airspeed_scale_1.commit_no_notification(); + break; - case 2: - _param_airspeed_scale_3.set(_airspeed_validator[i].get_CAS_scale_validated()); - _param_airspeed_scale_3.commit_no_notification(); - break; - } + case 1: + _param_airspeed_scale_2.set(_airspeed_validator[i].get_CAS_scale_validated()); + _param_airspeed_scale_2.commit_no_notification(); + break; + + case 2: + _param_airspeed_scale_3.set(_airspeed_validator[i].get_CAS_scale_validated()); + _param_airspeed_scale_3.commit_no_notification(); + break; } + } _airspeed_validator[i].set_scale_init(_param_airspeed_scale[i]); @@ -553,8 +553,7 @@ void AirspeedModule::update_ground_minus_wind_airspeed() const float TAS_east = _vehicle_local_position.vy - _wind_estimate_sideslip.windspeed_east; const float TAS_down = _vehicle_local_position.vz; // no wind estimate in z _ground_minus_wind_TAS = sqrtf(TAS_north * TAS_north + TAS_east * TAS_east + TAS_down * TAS_down); - _ground_minus_wind_CAS = calc_CAS_from_TAS(_ground_minus_wind_TAS, _vehicle_air_data.baro_pressure_pa, - _vehicle_air_data.baro_temp_celcius); + _ground_minus_wind_CAS = calc_calibrated_from_true_airspeed(_ground_minus_wind_TAS, _vehicle_air_data.rho); } else { _ground_minus_wind_TAS = _ground_minus_wind_CAS = NAN;