diff --git a/airdata/WindEstimator.cpp b/airdata/WindEstimator.cpp index 07792b915b..85aca2539d 100644 --- a/airdata/WindEstimator.cpp +++ b/airdata/WindEstimator.cpp @@ -326,7 +326,13 @@ WindEstimator::run_sanity_checks() } // constrain airspeed scale factor, negative values physically do not make sense - _state(tas) = math::max(0.0f, _state(tas)); + if (_scale_estimation_on) { + _state(tas) = math::max(0.0f, _state(tas)); + } else { + _state(tas) = 1.0f; + } + + // attain symmetry for (unsigned row = 0; row < 3; row++) { diff --git a/airdata/WindEstimator.hpp b/airdata/WindEstimator.hpp index dee8dc4924..3d458210ca 100644 --- a/airdata/WindEstimator.hpp +++ b/airdata/WindEstimator.hpp @@ -88,6 +88,7 @@ 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; } private: enum { @@ -120,6 +121,7 @@ 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 // initialise state and state covariance matrix bool initialise(const matrix::Vector3f &velI, const matrix::Vector2f &velIvar, const float tas_meas);