diff --git a/airdata/WindEstimator.cpp b/airdata/WindEstimator.cpp index e78e355746..df4d3bb971 100644 --- a/airdata/WindEstimator.cpp +++ b/airdata/WindEstimator.cpp @@ -330,15 +330,15 @@ WindEstimator::run_sanity_checks() return; } - // constrain airspeed scale factor, negative values physically do not make sense - if (_scale_estimation_on) { + // check if we should inhibit learning of airspeed scale factor and rather use a pre-set value. + // airspeed scale factor errors arise from sensor installation which does not change and only needs + // to be computed once for a perticular installation. + if (_enforced_airspeed_scale < 0) { _state(tas) = math::max(0.0f, _state(tas)); } else { - _state(tas) = 1.0f; + _state(tas) = _enforced_airspeed_scale; } - - // attain symmetry for (unsigned row = 0; row < 3; row++) { for (unsigned column = 0; column < row; column++) { diff --git a/airdata/WindEstimator.hpp b/airdata/WindEstimator.hpp index a18d84194d..571a57315f 100644 --- a/airdata/WindEstimator.hpp +++ b/airdata/WindEstimator.hpp @@ -89,7 +89,10 @@ public: void set_beta_noise(float beta_var) { _beta_var = beta_var * beta_var; } 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_estimation_on(bool scale_estimation_on) {_scale_estimation_on = scale_estimation_on; } + + // Inhibit learning of the airspeed scale factor and force the estimator to use _enforced_airspeed_scale as scale factor. + // Negative input values enable learning of the airspeed scale factor. + void enforce_airspeed_scale(float scale) {_enforced_airspeed_scale = scale; } private: enum { @@ -122,9 +125,9 @@ private: uint64_t _time_rejected_beta = 0; ///< timestamp of when sideslip measurements have consistently started to be rejected uint64_t _time_rejected_tas = 0; ///CAS/EAS) is on bool _wind_estimator_reset = false; ///< wind estimator was reset in this cycle + float _enforced_airspeed_scale{-1.0f}; ///< by default we want to estimate the true airspeed scale factor (see enforce_airspeed_scale(...) ) // initialise state and state covariance matrix bool initialise(const matrix::Vector3f &velI, const matrix::Vector2f &velIvar, const float tas_meas);