WindEstimator: added support for pre-set airspeed scale factor

Signed-off-by: RomanBapst <[email protected]>

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++) {

airdata/WindEstimator.hpp

@@ -89,7 +89,10 @@ class WindEstimator
 	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 @@ class WindEstimator
 	uint64_t _time_rejected_beta = 0;		///< timestamp of when sideslip measurements have consistently started to be rejected
 	uint64_t _time_rejected_tas =
 		0;		///<timestamp of when true airspeed measurements have consistently started to be rejected
-	bool _scale_estimation_on = false; ///< online scale estimation (IAS-->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);