diff --git a/src/lib/wind_estimator/WindEstimator.hpp b/src/lib/wind_estimator/WindEstimator.hpp index 462f88ff26..dbdf854da9 100644 --- a/src/lib/wind_estimator/WindEstimator.hpp +++ b/src/lib/wind_estimator/WindEstimator.hpp @@ -94,6 +94,14 @@ public: void set_beta_gate(uint8_t gate_size) {_beta_gate = gate_size; } void set_scale_init(float scale_init) {_scale_init = 1.f / math::constrain(scale_init, 0.1f, 10.f); } + void reset_scale_to_init() + { + _state(INDEX_TAS_SCALE) = _scale_init; + auto P_wind = _P.slice<2, 2>(0, 0); + _P.setZero(); + _P.slice<2, 2>(0, 0) = P_wind; + } + private: enum { INDEX_W_N = 0, diff --git a/src/modules/airspeed_selector/AirspeedValidator.hpp b/src/modules/airspeed_selector/AirspeedValidator.hpp index ec78b9ef8f..f902e8a9de 100644 --- a/src/modules/airspeed_selector/AirspeedValidator.hpp +++ b/src/modules/airspeed_selector/AirspeedValidator.hpp @@ -121,6 +121,7 @@ public: void set_tas_scale_apply(int tas_scale_apply) { _tas_scale_apply = tas_scale_apply; } void set_CAS_scale_validated(float scale) { _CAS_scale_validated = scale; } void set_scale_init(float scale) { _wind_estimator.set_scale_init(scale); } + void reset_scale_estimator() { _wind_estimator.reset_scale_to_init(); } void set_enable_data_stuck_check(bool enable) { _data_stuck_check_enabled = enable; } void set_enable_innovation_check(bool enable) { _innovation_check_enabled = enable; } diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index b3209e2f33..94a8b9cd8a 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -494,10 +494,24 @@ void AirspeedModule::update_params() param_get(_param_handle_fw_thr_max, &_param_fw_thr_max); } + const float prev_scale[MAX_NUM_AIRSPEED_SENSORS] = { + _param_airspeed_scale[0], + _param_airspeed_scale[1], + _param_airspeed_scale[2] + }; + _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(); + for (int i = 0; i < MAX_NUM_AIRSPEED_SENSORS; i++) { + if (fabsf(_param_airspeed_scale[i] - prev_scale[i]) > FLT_EPSILON) { + _airspeed_validator[i].set_scale_init(_param_airspeed_scale[i]); + _airspeed_validator[i].reset_scale_estimator(); + _airspeed_validator[i].set_CAS_scale_validated(_param_airspeed_scale[i]); + } + } + _wind_estimator_sideslip.set_wind_process_noise_spectral_density(_param_aspd_wind_nsd.get()); _wind_estimator_sideslip.set_tas_scale_process_noise_spectral_density(_param_aspd_scale_nsd.get()); _wind_estimator_sideslip.set_tas_noise(_param_west_tas_noise.get()); diff --git a/src/modules/airspeed_selector/airspeed_selector_params.c b/src/modules/airspeed_selector/airspeed_selector_params.c index a7db4cb30c..34d74cbfc5 100644 --- a/src/modules/airspeed_selector/airspeed_selector_params.c +++ b/src/modules/airspeed_selector/airspeed_selector_params.c @@ -95,7 +95,6 @@ PARAM_DEFINE_INT32(ASPD_SCALE_APPLY, 2); * @min 0.5 * @max 2.0 * @decimal 2 - * @reboot_required true * @group Airspeed Validator * @volatile */ @@ -109,7 +108,6 @@ PARAM_DEFINE_FLOAT(ASPD_SCALE_1, 1.0f); * @min 0.5 * @max 2.0 * @decimal 2 - * @reboot_required true * @group Airspeed Validator * @volatile */ @@ -123,7 +121,6 @@ PARAM_DEFINE_FLOAT(ASPD_SCALE_2, 1.0f); * @min 0.5 * @max 2.0 * @decimal 2 - * @reboot_required true * @group Airspeed Validator * @volatile */