fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Airspeed Selector: use better density source and only save scale parameter if valid (#20764)

Browse Source 
* 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 <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2022-12-20 15:52:54 +01:00 committed by GitHub
parent efbb2cf2e3
commit 20342216e2
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 29 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/lib/airspeed/airspeed.cpp
View File

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

13
src/lib/airspeed/airspeed.h
View File

@ -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); __EXPORT float get_air_density(float static_pressure, float temperature_celsius);
/** /**
* Calculate calibrated airspeed (CAS) from true airspeed (TAS). * @brief Calculates calibrated airspeed from true airspeed and air density
* It is the inverse function to calc_TAS_from_CAS()
* *
* @param speed_true current true airspeed * @param speed_true true airspeed [m/s]
* @param pressure_ambient pressure at the side of the tube/airplane * @param air_density air density [kg/m3]
* @param temperature_celsius air temperature in degrees Celsius * @return calibrated airspeed [m/s]
* @return CAS in m/s
*/ */
__EXPORT float calc_CAS_from_TAS(float speed_true, float pressure_ambient, __EXPORT float calc_calibrated_from_true_airspeed(float speed_true, float air_density);
float temperature_celsius);
__END_DECLS __END_DECLS

45
src/modules/airspeed_selector/airspeed_selector_main.cpp
View File

@ -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 (!armed && _armed_prev) {
if (_param_aspd_scale_apply.get() > 0) { if (_param_aspd_scale_apply.get() > 0 && _airspeed_validator[i].get_airspeed_valid()
if (fabsf(_airspeed_validator[i].get_CAS_scale_validated() - _param_airspeed_scale[i]) > 0.01f) { && fabsf(_airspeed_validator[i].get_CAS_scale_validated() - _param_airspeed_scale[i]) > FLT_EPSILON) {
// 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());
switch (i) { mavlink_log_info(&_mavlink_log_pub, "Airspeed sensor Nr. %d ASPD_SCALE updated: %.4f --> %.4f", i + 1,
case 0: (double)_param_airspeed_scale[i],
_param_airspeed_scale_1.set(_airspeed_validator[i].get_CAS_scale_validated()); (double)_airspeed_validator[i].get_CAS_scale_validated());
_param_airspeed_scale_1.commit_no_notification();
break;
case 1: switch (i) {
_param_airspeed_scale_2.set(_airspeed_validator[i].get_CAS_scale_validated()); case 0:
_param_airspeed_scale_2.commit_no_notification(); _param_airspeed_scale_1.set(_airspeed_validator[i].get_CAS_scale_validated());
break; _param_airspeed_scale_1.commit_no_notification();
break;
case 2: case 1:
_param_airspeed_scale_3.set(_airspeed_validator[i].get_CAS_scale_validated()); _param_airspeed_scale_2.set(_airspeed_validator[i].get_CAS_scale_validated());
_param_airspeed_scale_3.commit_no_notification(); _param_airspeed_scale_2.commit_no_notification();
break; 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]); _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_east = _vehicle_local_position.vy - _wind_estimate_sideslip.windspeed_east;
const float TAS_down = _vehicle_local_position.vz; // no wind estimate in z 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_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, _ground_minus_wind_CAS = calc_calibrated_from_true_airspeed(_ground_minus_wind_TAS, _vehicle_air_data.rho);
_vehicle_air_data.baro_temp_celcius);
} else { } else {
_ground_minus_wind_TAS = _ground_minus_wind_CAS = NAN; _ground_minus_wind_TAS = _ground_minus_wind_CAS = NAN;