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 <silvan@auterion.com>

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<float, 3, 1> 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<float, 3, 1> 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);

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);
 

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;
 	}

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:
 

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);
 	}
 }
 

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