fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-05 17:50:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

AirspeedSelector: airspeed scale estimation improvements and robustification

Browse Source 
- run airspeed scale estimation always, not in dedicated mode
- add option to apply scale automatically, with extra feasibility check
- add airspeed scale for all 3 possible airspeed instances
- clean up parameters
- add check for data stuck (non-changing airspeed data)

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2021-10-11 15:05:27 +02:00
parent 8e8c6efd66
commit 625f556b0e
7 changed files with 286 additions and 130 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/airspeed_selector/airspeed_selector_main.cpp
+60 -51
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
* Copyright (c) 2018-2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -149,13 +149,14 @@ private:
bool _in_takeoff_situation{true}; /**< in takeoff situation (defined as not yet stall speed reached) */
float _ground_minus_wind_TAS{0.0f}; /**< true airspeed from groundspeed minus windspeed */
float _ground_minus_wind_CAS{0.0f}; /**< calibrated airspeed from groundspeed minus windspeed */
bool _scale_estimation_previously_on{false}; /**< scale_estimation was on in the last cycle */
bool _armed_prev{false};
hrt_abstime _time_last_airspeed_update[MAX_NUM_AIRSPEED_SENSORS] {};
perf_counter_t _perf_elapsed{};
float _param_airspeed_scale[MAX_NUM_AIRSPEED_SENSORS] {}; /** array to save the airspeed scale params in */
DEFINE_PARAMETERS(
(ParamFloat<px4::params::ASPD_W_P_NOISE>) _param_west_w_p_noise,
(ParamFloat<px4::params::ASPD_SC_P_NOISE>) _param_west_sc_p_noise,
@@ -163,8 +164,10 @@ private:
(ParamFloat<px4::params::ASPD_BETA_NOISE>) _param_west_beta_noise,
(ParamInt<px4::params::ASPD_TAS_GATE>) _param_west_tas_gate,
(ParamInt<px4::params::ASPD_BETA_GATE>) _param_west_beta_gate,
(ParamInt<px4::params::ASPD_SCALE_EST>) _param_west_scale_estimation_on,
(ParamFloat<px4::params::ASPD_SCALE>) _param_west_airspeed_scale,
(ParamInt<px4::params::ASPD_SCALE_APPLY>) _param_aspd_scale_apply,
(ParamFloat<px4::params::ASPD_SCALE_1>) _param_airspeed_scale_1,
(ParamFloat<px4::params::ASPD_SCALE_2>) _param_airspeed_scale_2,
(ParamFloat<px4::params::ASPD_SCALE_3>) _param_airspeed_scale_3,
(ParamInt<px4::params::ASPD_PRIMARY>) _param_airspeed_primary_index,
(ParamInt<px4::params::ASPD_DO_CHECKS>) _param_airspeed_checks_on,
(ParamInt<px4::params::ASPD_FALLBACK_GW>) _param_airspeed_fallback_gw,
@@ -291,6 +294,12 @@ AirspeedModule::Run()
if (!_initialized) {
init(); // initialize airspeed validator instances
for (int i = 0; i < MAX_NUM_AIRSPEED_SENSORS; i++) {
_airspeed_validator[i].set_CAS_scale_estimated(_param_airspeed_scale[i]);
_airspeed_validator[i].set_scale_init(_param_airspeed_scale[i]);
}
_initialized = true;
}
@@ -300,7 +309,7 @@ AirspeedModule::Run()
update_params();
}
bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
const bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
// check for new connected airspeed sensors as long as we're disarmed
if (!armed) {
@@ -375,11 +384,44 @@ AirspeedModule::Run()
_airspeed_validator[i].reset_airspeed_to_invalid(_time_now_usec);
}
// save estimated airspeed scale after disarm
if (!armed && _armed_prev) {
if (_param_aspd_scale_apply.get() > 1) {
if (fabsf(_airspeed_validator[i].get_CAS_scale_estimated() - _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_estimated());
switch (i) {
case 0:
_param_airspeed_scale_1.set(_airspeed_validator[i].get_CAS_scale_estimated());
_param_airspeed_scale_1.commit_no_notification();
break;
case 1:
_param_airspeed_scale_2.set(_airspeed_validator[i].get_CAS_scale_estimated());
_param_airspeed_scale_2.commit_no_notification();
break;
case 2:
_param_airspeed_scale_3.set(_airspeed_validator[i].get_CAS_scale_estimated());
_param_airspeed_scale_3.commit_no_notification();
break;
}
}
}
_airspeed_validator[i].set_scale_init(_param_airspeed_scale[i]);
}
}
}
select_airspeed_and_publish();
_armed_prev = armed;
perf_end(_perf_elapsed);
if (should_exit()) {
@@ -391,6 +433,10 @@ void AirspeedModule::update_params()
{
updateParams();
_param_airspeed_scale[0] = _param_airspeed_scale_1.get();
_param_airspeed_scale[1] = _param_airspeed_scale_2.get();
_param_airspeed_scale[2] = _param_airspeed_scale_3.get();
_wind_estimator_sideslip.set_wind_p_noise(_param_west_w_p_noise.get());
_wind_estimator_sideslip.set_tas_scale_p_noise(_param_west_sc_p_noise.get());
_wind_estimator_sideslip.set_tas_noise(_param_west_tas_noise.get());
@@ -398,62 +444,24 @@ void AirspeedModule::update_params()
_wind_estimator_sideslip.set_tas_gate(_param_west_tas_gate.get());
_wind_estimator_sideslip.set_beta_gate(_param_west_beta_gate.get());
for (int i = 0; i < _number_of_airspeed_sensors; i++) {
for (int i = 0; i < MAX_NUM_AIRSPEED_SENSORS; i++) {
_airspeed_validator[i].set_wind_estimator_wind_p_noise(_param_west_w_p_noise.get());
_airspeed_validator[i].set_wind_estimator_tas_scale_p_noise(_param_west_sc_p_noise.get());
_airspeed_validator[i].set_wind_estimator_tas_noise(_param_west_tas_noise.get());
_airspeed_validator[i].set_wind_estimator_beta_noise(_param_west_beta_noise.get());
_airspeed_validator[i].set_wind_estimator_tas_gate(_param_west_tas_gate.get());
_airspeed_validator[i].set_wind_estimator_beta_gate(_param_west_beta_gate.get());
_airspeed_validator[i].set_wind_estimator_scale_estimation_on(_param_west_scale_estimation_on.get());
// only apply manual entered airspeed scale to first airspeed measurement
// TODO: enable multiple airspeed sensors
_airspeed_validator[0].set_airspeed_scale_manual(_param_west_airspeed_scale.get());
_airspeed_validator[i].set_tas_scale_apply(_param_aspd_scale_apply.get());
_airspeed_validator[i].set_wind_estimator_tas_scale_init(_param_airspeed_scale[i]);
_airspeed_validator[i].set_tas_innov_threshold(_tas_innov_threshold.get());
_airspeed_validator[i].set_tas_innov_integ_threshold(_tas_innov_integ_threshold.get());
_airspeed_validator[i].set_checks_fail_delay(_checks_fail_delay.get());
_airspeed_validator[i].set_checks_clear_delay(_checks_clear_delay.get());
_airspeed_validator[i].set_airspeed_stall(_param_fw_airspd_stall.get());
_airspeed_validator[i].set_disable_tas_scale_estimate(_param_aspd_scale_apply.get() == 0);
}
// if the airspeed scale estimation is enabled and the airspeed is valid,
// then set the scale inside the wind estimator to -1 such that it starts to estimate it
if (!_scale_estimation_previously_on && _param_west_scale_estimation_on.get()) {
if (_valid_airspeed_index > 0) {
// set it to a negative value to start estimation inside wind estimator
_airspeed_validator[0].set_airspeed_scale_manual(-1.0f);
} else {
mavlink_log_info(&_mavlink_log_pub, "Airspeed: can't estimate scale as no valid sensor.\t");
events::send(events::ID("airspeed_selector_cannot_est_scale"), events::Log::Error,
"Airspeed: cannot estimate scale as there is no valid sensor");
_param_west_scale_estimation_on.set(0); // reset this param to 0 as estimation was not turned on
_param_west_scale_estimation_on.commit_no_notification();
}
} else if (_scale_estimation_previously_on && !_param_west_scale_estimation_on.get()) {
if (_valid_airspeed_index > 0) {
_param_west_airspeed_scale.set(_airspeed_validator[_valid_airspeed_index - 1].get_CAS_scale());
_param_west_airspeed_scale.commit_no_notification();
_airspeed_validator[_valid_airspeed_index - 1].set_airspeed_scale_manual(_param_west_airspeed_scale.get());
mavlink_log_info(&_mavlink_log_pub, "Airspeed: estimated scale (ASPD_SCALE): %0.2f\t",
(double)_airspeed_validator[_valid_airspeed_index - 1].get_CAS_scale());
events::send<float>(events::ID("airspeed_selector_scale"), events::Log::Info,
"Airspeed: estimated scale (ASPD_SCALE): {1:.2}", _airspeed_validator[_valid_airspeed_index - 1].get_CAS_scale());
} else {
mavlink_log_info(&_mavlink_log_pub, "Airspeed: can't estimate scale as no valid sensor.\t");
events::send(events::ID("airspeed_selector_cannot_est_scale2"), events::Log::Error,
"Airspeed: cannot estimate scale as there is no valid sensor");
}
}
_scale_estimation_previously_on = _param_west_scale_estimation_on.get();
}
void AirspeedModule::poll_topics()
@@ -505,11 +513,12 @@ void AirspeedModule::update_wind_estimator_sideslip()
_wind_estimator_sideslip.get_wind_var(wind_cov);
_wind_estimate_sideslip.variance_north = wind_cov[0];
_wind_estimate_sideslip.variance_east = wind_cov[1];
_wind_estimate_sideslip.tas_innov = _wind_estimator_sideslip.get_tas_innov();
_wind_estimate_sideslip.tas_innov_var = _wind_estimator_sideslip.get_tas_innov_var();
_wind_estimate_sideslip.tas_innov = NAN;
_wind_estimate_sideslip.tas_innov_var = NAN;
_wind_estimate_sideslip.beta_innov = _wind_estimator_sideslip.get_beta_innov();
_wind_estimate_sideslip.beta_innov_var = _wind_estimator_sideslip.get_beta_innov_var();
_wind_estimate_sideslip.tas_scale = _wind_estimator_sideslip.get_tas_scale();
_wind_estimate_sideslip.tas_scale = NAN;
_wind_estimate_sideslip.tas_scale_var = NAN;
_wind_estimate_sideslip.source = airspeed_wind_s::SOURCE_AS_BETA_ONLY;
}