From 73fe300c00f290091c203f368ac4a933a410d8d7 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 21 Oct 2021 11:50:56 +0200 Subject: [PATCH] WindEstimator: remove option to disable scale estimation The situation where this would be desired is unclear, plus it's basically the same as setting ASPD_SC_P_NOISE to a very small value. Signed-off-by: Silvan Fuhrer --- src/lib/wind_estimator/WindEstimator.cpp | 10 +--------- src/lib/wind_estimator/WindEstimator.hpp | 3 --- src/modules/airspeed_selector/AirspeedValidator.cpp | 5 +---- src/modules/airspeed_selector/AirspeedValidator.hpp | 1 - .../airspeed_selector/airspeed_selector_main.cpp | 3 +-- .../airspeed_selector/airspeed_selector_params.c | 11 +++++------ 6 files changed, 8 insertions(+), 25 deletions(-) diff --git a/src/lib/wind_estimator/WindEstimator.cpp b/src/lib/wind_estimator/WindEstimator.cpp index f275f83ceb..9ddf45dfea 100644 --- a/src/lib/wind_estimator/WindEstimator.cpp +++ b/src/lib/wind_estimator/WindEstimator.cpp @@ -116,7 +116,7 @@ WindEstimator::update(uint64_t time_now) _time_last_update = time_now; float q_w = _wind_p_var; - float q_k_tas = _disable_tas_scale_estimate ? 0.f : _tas_scale_p_var; + float q_k_tas = _tas_scale_p_var; float SPP0 = dt * dt; float SPP1 = SPP0 * q_w; @@ -180,10 +180,6 @@ WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const matrix::Matrix K = _P * H_tas.transpose(); K /= S(0, 0); - if (_disable_tas_scale_estimate) { - K(2, 0) = 0.f; - } - // compute innovation const float airspeed_pred = _state(INDEX_TAS_SCALE) * airspeed_predicted_raw; _tas_innov = true_airspeed - airspeed_pred; @@ -270,10 +266,6 @@ WindEstimator::fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const matrix::Matrix K = _P * H_beta.transpose(); K /= S(0, 0); - if (_disable_tas_scale_estimate) { - K(2, 0) = 0.f; - } - // compute predicted side slip angle matrix::Vector3f rel_wind(velI(0) - _state(INDEX_W_N), velI(1) - _state(INDEX_W_E), velI(2)); matrix::Dcmf R_body_to_earth(q_att); diff --git a/src/lib/wind_estimator/WindEstimator.hpp b/src/lib/wind_estimator/WindEstimator.hpp index acdc007c1b..f62925c882 100644 --- a/src/lib/wind_estimator/WindEstimator.hpp +++ b/src/lib/wind_estimator/WindEstimator.hpp @@ -85,7 +85,6 @@ public: void set_tas_gate(uint8_t gate_size) {_tas_gate = gate_size; } 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 set_disable_tas_scale_estimate(bool disable_tas_scale_estimate) {_disable_tas_scale_estimate = disable_tas_scale_estimate; } private: enum { @@ -123,8 +122,6 @@ private: bool _wind_estimator_reset = false; ///< wind estimator was reset in this cycle - bool _disable_tas_scale_estimate{false}; - // initialise state and state covariance matrix bool initialise(const matrix::Vector3f &velI, const matrix::Vector2f &velIvar, const float tas_meas); diff --git a/src/modules/airspeed_selector/AirspeedValidator.cpp b/src/modules/airspeed_selector/AirspeedValidator.cpp index 38ec733f05..b435bdc0fd 100644 --- a/src/modules/airspeed_selector/AirspeedValidator.cpp +++ b/src/modules/airspeed_selector/AirspeedValidator.cpp @@ -177,13 +177,10 @@ AirspeedValidator::update_CAS_scale_applied() /* fallthrough */ case 1: - - /* fallthrough */ - case 2: _CAS_scale_applied = _tas_scale_init; break; - case 3: + case 2: _CAS_scale_applied = _CAS_scale_validated; break; } diff --git a/src/modules/airspeed_selector/AirspeedValidator.hpp b/src/modules/airspeed_selector/AirspeedValidator.hpp index 1a0a055263..a07561817e 100644 --- a/src/modules/airspeed_selector/AirspeedValidator.hpp +++ b/src/modules/airspeed_selector/AirspeedValidator.hpp @@ -115,7 +115,6 @@ 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 set_disable_tas_scale_estimate(bool disable_scale_est) {_wind_estimator.set_disable_tas_scale_estimate(disable_scale_est); } private: diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index ca33b20395..f9eecc8505 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -387,7 +387,7 @@ AirspeedModule::Run() // save estimated airspeed scale after disarm if (!armed && _armed_prev) { - if (_param_aspd_scale_apply.get() > 1) { + 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, @@ -460,7 +460,6 @@ void AirspeedModule::update_params() _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); } } diff --git a/src/modules/airspeed_selector/airspeed_selector_params.c b/src/modules/airspeed_selector/airspeed_selector_params.c index 9dc172cb42..14fadf07b3 100644 --- a/src/modules/airspeed_selector/airspeed_selector_params.c +++ b/src/modules/airspeed_selector/airspeed_selector_params.c @@ -76,15 +76,14 @@ PARAM_DEFINE_INT32(ASPD_TAS_GATE, 3); PARAM_DEFINE_INT32(ASPD_BETA_GATE, 1); /** - * Controls when to apply the new estimated airspeed scale + * Controls when to apply the new estimated airspeed scale(s) * - * @value 0 Disable airspeed scale estimation completely - * @value 1 Do not apply the new gains (logging and inside wind estimator) - * @value 2 Apply the new scale after disarm - * @value 3 Apply the new gains in air + * @value 0 Do not automatically apply the estimated scale + * @value 1 Apply the estimated scale after disarm + * @value 2 Apply the estimated scale in air * @group Airspeed Validator */ -PARAM_DEFINE_INT32(ASPD_SCALE_APPLY, 2); +PARAM_DEFINE_INT32(ASPD_SCALE_APPLY, 1); /** * Scale of airspeed sensor 1