AirspeedSelector: airspeed scale estimation improvements and robustification

- run airspeed scale estimation always, not in dedicated mode
- add option to apply scale automatically, with extra feasibility check
- add airspeed scale for all 3 possible airspeed instances
- clean up parameters
- add check for data stuck (non-changing airspeed data)

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

msg/airspeed_wind.msg

@@ -9,7 +9,9 @@ float32 variance_east		# Wind estimate error variance in east / Y direction (m/s
 
 float32 tas_innov 		# True airspeed innovation
 float32 tas_innov_var 		# True airspeed innovation variance
+
 float32 tas_scale 		# Estimated true airspeed scale factor
+float32 tas_scale_var 		# True airspeed scale factor variance
 
 float32 beta_innov 		# Sideslip measurement innovation
 float32 beta_innov_var 		# Sideslip measurement innovation variance

src/lib/wind_estimator/WindEstimator.cpp

@@ -56,7 +56,7 @@ WindEstimator::initialise(const matrix::Vector3f &velI, const matrix::Vector2f &
 	// initilaise wind states assuming zero side slip and horizontal flight
 	_state(INDEX_W_N) = velI(INDEX_W_N) - tas_meas * cosf(heading_est);
 	_state(INDEX_W_E) = velI(INDEX_W_E) - tas_meas * sinf(heading_est);
-	_state(INDEX_TAS_SCALE) = 1.0f;
+	_state(INDEX_TAS_SCALE) = _scale_init;
 
 	// compute jacobian of states wrt north/each earth velocity states and true airspeed measurement
 	float L0 = v_e * v_e;
@@ -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 = _tas_scale_p_var;
+	float q_k_tas = _disable_tas_scale_estimate ? 0.f : _tas_scale_p_var;
 
 	float SPP0 = dt * dt;
 	float SPP1 = SPP0 * q_w;
@@ -160,28 +160,32 @@ WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const
 	const float v_e = velI(1);
 	const float v_d = velI(2);
 
-	const float k_tas = _state(INDEX_TAS_SCALE);
+	// calculate airspeed from ground speed and wind states (without scale)
+	const float airspeed_predicted_raw = sqrtf((v_n - _state(INDEX_W_N)) * (v_n - _state(INDEX_W_N)) +
+					     (v_e - _state(INDEX_W_E)) * (v_e - _state(INDEX_W_E)) + v_d * v_d);
 
-	// compute kalman gain K
+	// compute state observation matrix H
-	const float HH0 = sqrtf(v_d * v_d + (v_e - _state(INDEX_W_E)) * (v_e - _state(INDEX_W_E)) + (v_n - _state(
+	const float HH0 = airspeed_predicted_raw;
-					INDEX_W_N)) * (v_n - _state(INDEX_W_N)));
+	const float HH1 = _state(INDEX_TAS_SCALE) / HH0;
-	const float HH1 = k_tas / HH0;
 
 	matrix::Matrix<float, 1, 3> H_tas;
 	H_tas(0, 0) = HH1 * (-v_n + _state(INDEX_W_N));
 	H_tas(0, 1) = HH1 * (-v_e + _state(INDEX_W_E));
 	H_tas(0, 2) = HH0;
 
-	matrix::Matrix<float, 3, 1> K = _P * H_tas.transpose();
+	// compute innovation covariance S
-
 	const matrix::Matrix<float, 1, 1> S = H_tas * _P * H_tas.transpose() + _tas_var;
 
+	// compute Kalman gain
+	matrix::Matrix<float, 3, 1> K = _P * H_tas.transpose();
 	K /= S(0, 0);
-	// compute innovation
-	const float airspeed_pred = k_tas * sqrtf((v_n - _state(INDEX_W_N)) * (v_n - _state(INDEX_W_N)) + (v_e - _state(
-					    INDEX_W_E)) *
-				    (v_e - _state(INDEX_W_E)) + v_d * v_d);
 
+	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;
 
 	// innovation variance
@@ -259,13 +263,17 @@ WindEstimator::fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const
 	H_beta(0, 1) = HB15 * (HB12 - HB7 + HB8) + HB16 * HB5;
 	H_beta(0, 2) = 0;
 
-	// compute kalman gain
+	// compute innovation covariance S
-	matrix::Matrix<float, 3, 1> K = _P * H_beta.transpose();
-
 	const matrix::Matrix<float, 1, 1> S = H_beta * _P * H_beta.transpose() + _beta_var;
 
+	// compute Kalman gain
+	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);
@@ -333,16 +341,6 @@ WindEstimator::run_sanity_checks()
 		return;
 	}
 
-	// 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(INDEX_TAS_SCALE) = math::max(0.0f, _state(INDEX_TAS_SCALE));
-
-	} else {
-		_state(INDEX_TAS_SCALE) = _enforced_airspeed_scale;
-	}
-
 	// attain symmetry
 	for (unsigned row = 0; row < 3; row++) {
 		for (unsigned column = 0; column < row; column++) {

src/lib/wind_estimator/WindEstimator.hpp

@@ -70,7 +70,9 @@ public:
 	bool check_if_meas_is_rejected(uint64_t time_now, float innov, float innov_var, uint8_t gate_size,
 				       uint64_t &time_meas_rejected, bool &reinit_filter);
 
-	float get_tas_scale() { return _state(INDEX_TAS_SCALE); }
+	// invert scale (CAS = IAS * scale), protect agains division by 0, constrain to [0.1, 10]
+	float get_tas_scale() { return math::constrain(1.f / (_state(INDEX_TAS_SCALE) + 0.0001f), 0.1f, 10.f); }
+	float get_tas_scale_var() { return _P(2, 2); }
 	float get_tas_innov() { return _tas_innov; }
 	float get_tas_innov_var() { return _tas_innov_var; }
 	float get_beta_innov() { return _beta_innov; }
@@ -88,10 +90,8 @@ 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_init(float scale_init) {_scale_init = math::constrain(1.f / (scale_init + 0.0001f), 0.1f, 10.f); }
-	// Inhibit learning of the airspeed scale factor and force the estimator to use _enforced_airspeed_scale as scale factor.
+	void set_disable_tas_scale_estimate(bool disable_tas_scale_estimate) {_disable_tas_scale_estimate = disable_tas_scale_estimate; }
-	// Negative input values enable learning of the airspeed scale factor.
-	void enforce_airspeed_scale(float scale) {_enforced_airspeed_scale = scale; }
 
 private:
 	enum {
@@ -100,7 +100,7 @@ private:
 		INDEX_TAS_SCALE
 	};	///< enum which can be used to access state.
 
-	matrix::Vector3f _state;		///< state vector
+	matrix::Vector3f _state{0.f, 0.f, 1.f};
 	matrix::Matrix3f _P;		///< state covariance matrix
 
 	float _tas_innov{0.0f};	///< true airspeed innovation
@@ -118,6 +118,8 @@ private:
 	uint8_t _tas_gate{3};	///< airspeed fusion gate size
 	uint8_t _beta_gate{1};	///< sideslip fusion gate size
 
+	float _scale_init{1.f};
+
 	uint64_t _time_last_airspeed_fuse = 0;	///< timestamp of last airspeed fusion
 	uint64_t _time_last_beta_fuse = 0;	///< timestamp of last sideslip fusion
 	uint64_t _time_last_update = 0;		///< timestamp of last covariance prediction
@@ -126,7 +128,8 @@ private:
 		0;	///< timestamp of when true airspeed measurements have consistently started to be rejected
 
 	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(...) )
+
+	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

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -39,6 +39,10 @@
 
 #include "AirspeedValidator.hpp"
 
+AirspeedValidator::AirspeedValidator()
+{
+	reset_CAS_scale_check(); //this resets all elements of the Vectors to NAN
+}
 
 void
 AirspeedValidator::update_airspeed_validator(const airspeed_validator_update_data &input_data)
@@ -46,12 +50,14 @@ AirspeedValidator::update_airspeed_validator(const airspeed_validator_update_dat
 	// get indicated airspeed from input data (raw airspeed)
 	_IAS = input_data.airspeed_indicated_raw;
 
-	update_CAS_scale();
+	update_CAS_scale_estimated(input_data.lpos_valid, input_data.lpos_vx, input_data.lpos_vy, input_data.lpos_vz);
+	update_CAS_scale_applied();
 	update_CAS_TAS(input_data.air_pressure_pa, input_data.air_temperature_celsius);
 	update_wind_estimator(input_data.timestamp, input_data.airspeed_true_raw, input_data.lpos_valid, input_data.lpos_vx,
 			      input_data.lpos_vy,
 			      input_data.lpos_vz, input_data.lpos_evh, input_data.lpos_evv, input_data.att_q);
 	update_in_fixed_wing_flight(input_data.in_fixed_wing_flight);
+	check_airspeed_data_stuck(input_data.timestamp);
 	check_airspeed_innovation(input_data.timestamp, input_data.vel_test_ratio, input_data.mag_test_ratio);
 	check_load_factor(input_data.accel_z);
 	update_airspeed_valid_status(input_data.timestamp);
@@ -105,34 +111,112 @@ AirspeedValidator::get_wind_estimator_states(uint64_t timestamp)
 	wind_est.beta_innov = _wind_estimator.get_beta_innov();
 	wind_est.beta_innov_var = _wind_estimator.get_beta_innov_var();
 	wind_est.tas_scale = _wind_estimator.get_tas_scale();
+	wind_est.tas_scale_var = _wind_estimator.get_tas_scale_var();
 	return wind_est;
 }
 
 void
-AirspeedValidator::set_airspeed_scale_manual(float airspeed_scale_manual)
+AirspeedValidator::update_CAS_scale_estimated(bool lpos_valid, float vx, float vy, float vz)
+{
+	if (!_in_fixed_wing_flight) {
+		return;
+	}
+
+	// reset every 100s as we assume that all the samples for current check are in similar wind conditions
+	if (hrt_elapsed_time(&_begin_current_scale_check) > 100_s) {
+		reset_CAS_scale_check();
+	}
+
+	const float course_over_ground_rad = matrix::wrap_2pi(atan2f(vy, vx));
+	const int segment_index = int(SCALE_CHECK_SAMPLES * course_over_ground_rad / (2.f * M_PI_F));
+
+	_scale_check_groundspeed(segment_index) = sqrt(vx * vx + vy * vy + vz * vz);
+	_scale_check_TAS(segment_index) = _TAS;
+
+	// run check if all segments are filled
+	if (PX4_ISFINITE(_scale_check_groundspeed.norm_squared())) {
+
+		float ground_speed_sum = 0.f;
+		float TAS_sum = 0.f;
+
+		for (int i = 0; i < SCALE_CHECK_SAMPLES; i++) {
+			ground_speed_sum += _scale_check_groundspeed(i);
+			TAS_sum += _scale_check_TAS(i);
+		}
+
+		const float TAS_to_grounspeed_error_current = ground_speed_sum - TAS_sum * _CAS_scale_estimated;
+		const float TAS_to_grounspeed_error_new = ground_speed_sum - TAS_sum * _wind_estimator.get_tas_scale();
+
+		// check passes if the average airspeed with the scale applied is closer to groundspeed than without
+		if (fabsf(TAS_to_grounspeed_error_new) < fabsf(TAS_to_grounspeed_error_current)) {
+
+			// constrain the scale update to max 0.01 at a time
+			const float new_scale_constrained = math::constrain(_wind_estimator.get_tas_scale(), _CAS_scale_estimated - 0.01f,
+							    _CAS_scale_estimated + 0.01f);
+
+			// PX4_INFO("_CAS_scale_estimated updated: %.2f --> %.2f", (double)_CAS_scale_estimated,
+			// 	 (double)new_scale_constrained);
+
+			_CAS_scale_estimated = new_scale_constrained;
+		}
+
+		reset_CAS_scale_check();
+	}
+}
+
+void
+AirspeedValidator::reset_CAS_scale_check()
 {
-	_airspeed_scale_manual = airspeed_scale_manual;
+
-	_wind_estimator.enforce_airspeed_scale(1.0f / airspeed_scale_manual); // scale is inverted inside the wind estimator
+	_scale_check_groundspeed.setAll(NAN);
+	_scale_check_TAS.setAll(NAN);
+
+	_begin_current_scale_check = hrt_absolute_time();
 }
 
 void
-AirspeedValidator::update_CAS_scale()
+AirspeedValidator::update_CAS_scale_applied()
 {
-	if (_wind_estimator.is_estimate_valid()) {
+	switch (_tas_scale_apply) {
-		_CAS_scale = 1.0f / math::constrain(_wind_estimator.get_tas_scale(), 0.5f, 2.0f);
+	default:
 
-	} else {
+	/* fallthrough */
-		_CAS_scale = _airspeed_scale_manual;
+	case 0:
+
+	/* fallthrough */
+	case 1:
+
+	/* fallthrough */
+	case 2:
+		_CAS_scale_applied = _tas_scale_init;
+		break;
+
+	case 3:
+		_CAS_scale_applied = _CAS_scale_estimated;
+		break;
 	}
 }
 
 void
 AirspeedValidator::update_CAS_TAS(float air_pressure_pa, float air_temperature_celsius)
 {
-	_CAS = calc_CAS_from_IAS(_IAS, _CAS_scale);
+	_CAS = calc_CAS_from_IAS(_IAS, _CAS_scale_applied);
 	_TAS = calc_TAS_from_CAS(_CAS, air_pressure_pa, air_temperature_celsius);
 }
 
+void
+AirspeedValidator::check_airspeed_data_stuck(uint64_t time_now)
+{
+	// data stuck test: trigger when IAS is not changing for DATA_STUCK_TIMEOUT (2s)
+
+	if (fabsf(_IAS - _IAS_prev) > FLT_EPSILON || _time_last_unequal_data == 0) {
+		_time_last_unequal_data = time_now;
+		_IAS_prev = _IAS;
+	}
+
+	_data_stuck_test_failed = hrt_elapsed_time(&_time_last_unequal_data) > DATA_STUCK_TIMEOUT;
+}
+
 void
 AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_status_vel_test_ratio,
 		float estimator_status_mag_test_ratio)
@@ -205,12 +289,12 @@ AirspeedValidator::check_load_factor(float accel_z)
 void
 AirspeedValidator::update_airspeed_valid_status(const uint64_t timestamp)
 {
-	if (_innovations_check_failed || _load_factor_check_failed) {
+	if (_data_stuck_test_failed || _innovations_check_failed || _load_factor_check_failed) {
-		// either innovation or load factor check failed, so record timestamp
+		// at least one check (data stuck, innovation or load factor) failed, so record timestamp
 		_time_checks_failed = timestamp;
 
-	} else if (!_innovations_check_failed && !_load_factor_check_failed) {
+	} else if (! _data_stuck_test_failed && !_innovations_check_failed && !_load_factor_check_failed) {
-		// both innovation or load factor checks must pass to declare airspeed good
+		// all checks(data stuck, innovation and load factor) must pass to declare airspeed good
 		_time_checks_passed = timestamp;
 	}
 
@@ -223,7 +307,7 @@ AirspeedValidator::update_airspeed_valid_status(const uint64_t timestamp)
 		// a timeout period is applied.
 		const bool single_check_fail_timeout = (timestamp - _time_checks_passed) > _checks_fail_delay * 1_s;
 
-		if (both_checks_failed || single_check_fail_timeout) {
+		if (both_checks_failed || single_check_fail_timeout || _data_stuck_test_failed) {
 
 			_airspeed_valid = false;
 		}

src/modules/airspeed_selector/AirspeedValidator.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -73,7 +73,7 @@ struct airspeed_validator_update_data {
 class AirspeedValidator
 {
 public:
-	AirspeedValidator() = default;
+	AirspeedValidator();
 	~AirspeedValidator() = default;
 
 	void update_airspeed_validator(const airspeed_validator_update_data &input_data);
@@ -84,13 +84,18 @@ public:
 	float get_CAS() { return _CAS; }
 	float get_TAS() { return _TAS; }
 	bool get_airspeed_valid() { return _airspeed_valid; }
-	float get_CAS_scale() {return _CAS_scale;}
+	float get_CAS_scale_estimated() {return _CAS_scale_estimated;}
 
 	airspeed_wind_s get_wind_estimator_states(uint64_t timestamp);
 
 	// setters wind estimator parameters
 	void set_wind_estimator_wind_p_noise(float wind_sigma) { _wind_estimator.set_wind_p_noise(wind_sigma); }
 	void set_wind_estimator_tas_scale_p_noise(float tas_scale_sigma) { _wind_estimator.set_tas_scale_p_noise(tas_scale_sigma); }
+	void set_wind_estimator_tas_scale_init(float tas_scale_init)
+	{
+		_tas_scale_init = tas_scale_init;
+	}
+
 	void set_wind_estimator_tas_noise(float tas_sigma) { _wind_estimator.set_tas_noise(tas_sigma); }
 	void set_wind_estimator_beta_noise(float beta_var) { _wind_estimator.set_beta_noise(beta_var); }
 	void set_wind_estimator_tas_gate(uint8_t gate_size)
@@ -100,9 +105,6 @@ public:
 	}
 
 	void set_wind_estimator_beta_gate(uint8_t gate_size) { _wind_estimator.set_beta_gate(gate_size); }
-	void set_wind_estimator_scale_estimation_on(bool scale_estimation_on) { _wind_estimator_scale_estimation_on = scale_estimation_on;}
-
-	void set_airspeed_scale_manual(float airspeed_scale_manual);
 
 	// setters for failure detection tuning parameters
 	void set_tas_innov_threshold(float tas_innov_threshold) { _tas_innov_threshold = tas_innov_threshold; }
@@ -112,20 +114,31 @@ public:
 
 	void set_airspeed_stall(float airspeed_stall) { _airspeed_stall = airspeed_stall; }
 
+	void set_tas_scale_apply(int tas_scale_apply) { _tas_scale_apply = tas_scale_apply; }
+	void set_CAS_scale_estimated(float scale) { _CAS_scale_estimated = 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:
 
 	WindEstimator _wind_estimator{}; ///< wind estimator instance running in this particular airspeedValidator
 
-	// wind estimator parameter
+	// airspeed scale validity check
-	bool _wind_estimator_scale_estimation_on{false};	///< online scale estimation (IAS-->CAS) is on
+	static constexpr int SCALE_CHECK_SAMPLES = 12; ///< take samples from 12 segments (every 360/12=30°)
-	float _airspeed_scale_manual{1.0f}; ///< manually entered airspeed scale
 
 	// general states
 	bool _in_fixed_wing_flight{false}; ///< variable to bypass innovation and load factor checks
 	float _IAS{0.0f}; ///< indicated airsped in m/s
 	float _CAS{0.0f}; ///< calibrated airspeed in m/s
 	float _TAS{0.0f}; ///< true airspeed in m/s
-	float _CAS_scale{1.0f}; ///< scale factor from IAS to CAS
+	float _CAS_scale_applied{1.0f}; ///< scale factor from IAS to CAS (currently applied value)
+	float _CAS_scale_estimated{1.0f}; ///< scale factor from IAS to CAS (currently estimated value)
+
+	// data stuck check
+	uint64_t _time_last_unequal_data{0};
+	bool _data_stuck_test_failed{false};
+	float _IAS_prev{0.f};
+	static constexpr uint64_t DATA_STUCK_TIMEOUT{2_s}; ///< timeout after which data stuck check triggers when data is flat
 
 	// states of innovation check
 	float _tas_gate{1.0f}; ///< gate size of airspeed innovation (to calculate tas_test_ratio)
@@ -146,22 +159,33 @@ private:
 	// states of airspeed valid declaration
 	bool _airspeed_valid{true}; ///< airspeed valid (pitot or groundspeed-windspeed)
 	int _checks_fail_delay{3}; ///< delay for airspeed invalid declaration after single check failure (Sec)
-	int _checks_clear_delay{3}; ///< delay for airspeed valid declaration after all checks passed again (Sec)
+	int _checks_clear_delay{-1}; ///< delay for airspeed valid declaration after all checks passed again (Sec)
 	uint64_t	_time_checks_passed{0};	///< time the checks have last passed (uSec)
 	uint64_t	_time_checks_failed{0};	///< time the checks have last not passed (uSec)
 
+	hrt_abstime _begin_current_scale_check{0};
+
+	int _tas_scale_apply{0};
+	float _tas_scale_init{1.f};
+
+	matrix::Vector<float, SCALE_CHECK_SAMPLES> _scale_check_TAS {};
+	matrix::Vector<float, SCALE_CHECK_SAMPLES> _scale_check_groundspeed {};
+
 	void update_in_fixed_wing_flight(bool in_fixed_wing_flight) { _in_fixed_wing_flight = in_fixed_wing_flight; }
 
 	void update_wind_estimator(const uint64_t timestamp, float airspeed_true_raw, bool lpos_valid, float lpos_vx,
 				   float lpos_vy,
 				   float lpos_vz,
 				   float lpos_evh, float lpos_evv, const float att_q[4]);
-	void update_CAS_scale();
+	void update_CAS_scale_estimated(bool lpos_valid, float vx, float vy, float vz);
+	void update_CAS_scale_applied();
 	void update_CAS_TAS(float air_pressure_pa, float air_temperature_celsius);
+	void check_airspeed_data_stuck(uint64_t timestamp);
 	void check_airspeed_innovation(uint64_t timestamp, float estimator_status_vel_test_ratio,
 				       float estimator_status_mag_test_ratio);
 	void check_load_factor(float accel_z);
 	void update_airspeed_valid_status(const uint64_t timestamp);
 	void reset();
+	void reset_CAS_scale_check();
 
 };

src/modules/airspeed_selector/airspeed_selector_main.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -149,13 +149,14 @@ private:
 	bool _in_takeoff_situation{true}; /**< in takeoff situation (defined as not yet stall speed reached) */
 	float _ground_minus_wind_TAS{0.0f}; /**< true airspeed from groundspeed minus windspeed */
 	float _ground_minus_wind_CAS{0.0f}; /**< calibrated airspeed from groundspeed minus windspeed */
-
+	bool _armed_prev{false};
-	bool _scale_estimation_previously_on{false}; /**< scale_estimation was on in the last cycle */
 
 	hrt_abstime _time_last_airspeed_update[MAX_NUM_AIRSPEED_SENSORS] {};
 
 	perf_counter_t _perf_elapsed{};
 
+	float _param_airspeed_scale[MAX_NUM_AIRSPEED_SENSORS] {}; /** array to save the airspeed scale params in */
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::ASPD_W_P_NOISE>) _param_west_w_p_noise,
 		(ParamFloat<px4::params::ASPD_SC_P_NOISE>) _param_west_sc_p_noise,
@@ -163,8 +164,10 @@ private:
 		(ParamFloat<px4::params::ASPD_BETA_NOISE>) _param_west_beta_noise,
 		(ParamInt<px4::params::ASPD_TAS_GATE>) _param_west_tas_gate,
 		(ParamInt<px4::params::ASPD_BETA_GATE>) _param_west_beta_gate,
-		(ParamInt<px4::params::ASPD_SCALE_EST>) _param_west_scale_estimation_on,
+		(ParamInt<px4::params::ASPD_SCALE_APPLY>) _param_aspd_scale_apply,
-		(ParamFloat<px4::params::ASPD_SCALE>) _param_west_airspeed_scale,
+		(ParamFloat<px4::params::ASPD_SCALE_1>) _param_airspeed_scale_1,
+		(ParamFloat<px4::params::ASPD_SCALE_2>) _param_airspeed_scale_2,
+		(ParamFloat<px4::params::ASPD_SCALE_3>) _param_airspeed_scale_3,
 		(ParamInt<px4::params::ASPD_PRIMARY>) _param_airspeed_primary_index,
 		(ParamInt<px4::params::ASPD_DO_CHECKS>) _param_airspeed_checks_on,
 		(ParamInt<px4::params::ASPD_FALLBACK_GW>) _param_airspeed_fallback_gw,
@@ -291,6 +294,12 @@ AirspeedModule::Run()
 
 	if (!_initialized) {
 		init(); // initialize airspeed validator instances
+
+		for (int i = 0; i < MAX_NUM_AIRSPEED_SENSORS; i++) {
+			_airspeed_validator[i].set_CAS_scale_estimated(_param_airspeed_scale[i]);
+			_airspeed_validator[i].set_scale_init(_param_airspeed_scale[i]);
+		}
+
 		_initialized = true;
 	}
 
@@ -300,7 +309,7 @@ AirspeedModule::Run()
 		update_params();
 	}
 
-	bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
+	const bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
 
 	// check for new connected airspeed sensors as long as we're disarmed
 	if (!armed) {
@@ -375,11 +384,44 @@ AirspeedModule::Run()
 				_airspeed_validator[i].reset_airspeed_to_invalid(_time_now_usec);
 
 			}
+
+			// save estimated airspeed scale after disarm
+			if (!armed && _armed_prev) {
+				if (_param_aspd_scale_apply.get() > 1) {
+					if (fabsf(_airspeed_validator[i].get_CAS_scale_estimated() - _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,
+								 (double)_param_airspeed_scale[i],
+								 (double)_airspeed_validator[i].get_CAS_scale_estimated());
+
+						switch (i) {
+						case 0:
+							_param_airspeed_scale_1.set(_airspeed_validator[i].get_CAS_scale_estimated());
+							_param_airspeed_scale_1.commit_no_notification();
+							break;
+
+						case 1:
+							_param_airspeed_scale_2.set(_airspeed_validator[i].get_CAS_scale_estimated());
+							_param_airspeed_scale_2.commit_no_notification();
+							break;
+
+						case 2:
+							_param_airspeed_scale_3.set(_airspeed_validator[i].get_CAS_scale_estimated());
+							_param_airspeed_scale_3.commit_no_notification();
+							break;
+						}
+					}
+				}
+
+				_airspeed_validator[i].set_scale_init(_param_airspeed_scale[i]);
+			}
 		}
 	}
 
 	select_airspeed_and_publish();
 
+	_armed_prev = armed;
+
 	perf_end(_perf_elapsed);
 
 	if (should_exit()) {
@@ -391,6 +433,10 @@ void AirspeedModule::update_params()
 {
 	updateParams();
 
+	_param_airspeed_scale[0] = _param_airspeed_scale_1.get();
+	_param_airspeed_scale[1] = _param_airspeed_scale_2.get();
+	_param_airspeed_scale[2] = _param_airspeed_scale_3.get();
+
 	_wind_estimator_sideslip.set_wind_p_noise(_param_west_w_p_noise.get());
 	_wind_estimator_sideslip.set_tas_scale_p_noise(_param_west_sc_p_noise.get());
 	_wind_estimator_sideslip.set_tas_noise(_param_west_tas_noise.get());
@@ -398,62 +444,24 @@ void AirspeedModule::update_params()
 	_wind_estimator_sideslip.set_tas_gate(_param_west_tas_gate.get());
 	_wind_estimator_sideslip.set_beta_gate(_param_west_beta_gate.get());
 
-	for (int i = 0; i < _number_of_airspeed_sensors; i++) {
+	for (int i = 0; i < MAX_NUM_AIRSPEED_SENSORS; i++) {
 		_airspeed_validator[i].set_wind_estimator_wind_p_noise(_param_west_w_p_noise.get());
 		_airspeed_validator[i].set_wind_estimator_tas_scale_p_noise(_param_west_sc_p_noise.get());
 		_airspeed_validator[i].set_wind_estimator_tas_noise(_param_west_tas_noise.get());
 		_airspeed_validator[i].set_wind_estimator_beta_noise(_param_west_beta_noise.get());
 		_airspeed_validator[i].set_wind_estimator_tas_gate(_param_west_tas_gate.get());
 		_airspeed_validator[i].set_wind_estimator_beta_gate(_param_west_beta_gate.get());
-		_airspeed_validator[i].set_wind_estimator_scale_estimation_on(_param_west_scale_estimation_on.get());
 
-		// only apply manual entered airspeed scale to first airspeed measurement
+		_airspeed_validator[i].set_tas_scale_apply(_param_aspd_scale_apply.get());
-		// TODO: enable multiple airspeed sensors
+		_airspeed_validator[i].set_wind_estimator_tas_scale_init(_param_airspeed_scale[i]);
-		_airspeed_validator[0].set_airspeed_scale_manual(_param_west_airspeed_scale.get());
 
 		_airspeed_validator[i].set_tas_innov_threshold(_tas_innov_threshold.get());
 		_airspeed_validator[i].set_tas_innov_integ_threshold(_tas_innov_integ_threshold.get());
 		_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);
 	}
-
-	// if the airspeed scale estimation is enabled and the airspeed is valid,
-	// then set the scale inside the wind estimator to -1 such that it starts to estimate it
-	if (!_scale_estimation_previously_on && _param_west_scale_estimation_on.get()) {
-		if (_valid_airspeed_index > 0) {
-			// set it to a negative value to start estimation inside wind estimator
-			_airspeed_validator[0].set_airspeed_scale_manual(-1.0f);
-
-		} else {
-			mavlink_log_info(&_mavlink_log_pub, "Airspeed: can't estimate scale as no valid sensor.\t");
-			events::send(events::ID("airspeed_selector_cannot_est_scale"), events::Log::Error,
-				     "Airspeed: cannot estimate scale as there is no valid sensor");
-			_param_west_scale_estimation_on.set(0); // reset this param to 0 as estimation was not turned on
-			_param_west_scale_estimation_on.commit_no_notification();
-		}
-
-	} else if (_scale_estimation_previously_on && !_param_west_scale_estimation_on.get()) {
-		if (_valid_airspeed_index > 0) {
-
-			_param_west_airspeed_scale.set(_airspeed_validator[_valid_airspeed_index - 1].get_CAS_scale());
-			_param_west_airspeed_scale.commit_no_notification();
-			_airspeed_validator[_valid_airspeed_index - 1].set_airspeed_scale_manual(_param_west_airspeed_scale.get());
-
-			mavlink_log_info(&_mavlink_log_pub, "Airspeed: estimated scale (ASPD_SCALE): %0.2f\t",
-					 (double)_airspeed_validator[_valid_airspeed_index - 1].get_CAS_scale());
-			events::send<float>(events::ID("airspeed_selector_scale"), events::Log::Info,
-					    "Airspeed: estimated scale (ASPD_SCALE): {1:.2}", _airspeed_validator[_valid_airspeed_index - 1].get_CAS_scale());
-
-		} else {
-			mavlink_log_info(&_mavlink_log_pub, "Airspeed: can't estimate scale as no valid sensor.\t");
-			events::send(events::ID("airspeed_selector_cannot_est_scale2"), events::Log::Error,
-				     "Airspeed: cannot estimate scale as there is no valid sensor");
-		}
-	}
-
-	_scale_estimation_previously_on = _param_west_scale_estimation_on.get();
-
 }
 
 void AirspeedModule::poll_topics()
@@ -505,11 +513,12 @@ void AirspeedModule::update_wind_estimator_sideslip()
 	_wind_estimator_sideslip.get_wind_var(wind_cov);
 	_wind_estimate_sideslip.variance_north = wind_cov[0];
 	_wind_estimate_sideslip.variance_east = wind_cov[1];
-	_wind_estimate_sideslip.tas_innov = _wind_estimator_sideslip.get_tas_innov();
+	_wind_estimate_sideslip.tas_innov = NAN;
-	_wind_estimate_sideslip.tas_innov_var = _wind_estimator_sideslip.get_tas_innov_var();
+	_wind_estimate_sideslip.tas_innov_var = NAN;
 	_wind_estimate_sideslip.beta_innov = _wind_estimator_sideslip.get_beta_innov();
 	_wind_estimate_sideslip.beta_innov_var = _wind_estimator_sideslip.get_beta_innov_var();
-	_wind_estimate_sideslip.tas_scale = _wind_estimator_sideslip.get_tas_scale();
+	_wind_estimate_sideslip.tas_scale = NAN;
+	_wind_estimate_sideslip.tas_scale_var = NAN;
 	_wind_estimate_sideslip.source = airspeed_wind_s::SOURCE_AS_BETA_ONLY;
 }
 

src/modules/airspeed_selector/airspeed_selector_params.c

@@ -7,6 +7,7 @@
  * @min 0
  * @max 1
  * @unit m/s^2
+ * @decimal 2
  * @group Airspeed Validator
  */
 PARAM_DEFINE_FLOAT(ASPD_W_P_NOISE, 0.1f);
@@ -19,9 +20,10 @@ PARAM_DEFINE_FLOAT(ASPD_W_P_NOISE, 0.1f);
  * @min 0
  * @max 0.1
  * @unit Hz
+ * @decimal 5
  * @group Airspeed Validator
  */
-PARAM_DEFINE_FLOAT(ASPD_SC_P_NOISE, 0.0001);
+PARAM_DEFINE_FLOAT(ASPD_SC_P_NOISE, 0.0001f);
 
 /**
  * Airspeed Selector: Wind estimator true airspeed measurement noise
@@ -31,9 +33,10 @@ PARAM_DEFINE_FLOAT(ASPD_SC_P_NOISE, 0.0001);
  * @min 0
  * @max 4
  * @unit m/s
+ * @decimal 1
  * @group Airspeed Validator
  */
-PARAM_DEFINE_FLOAT(ASPD_TAS_NOISE, 1.4);
+PARAM_DEFINE_FLOAT(ASPD_TAS_NOISE, 1.4f);
 
 /**
  * Airspeed Selector: Wind estimator sideslip measurement noise
@@ -43,9 +46,10 @@ PARAM_DEFINE_FLOAT(ASPD_TAS_NOISE, 1.4);
  * @min 0
  * @max 1
  * @unit rad
+ * @decimal 3
  * @group Airspeed Validator
  */
-PARAM_DEFINE_FLOAT(ASPD_BETA_NOISE, 0.3);
+PARAM_DEFINE_FLOAT(ASPD_BETA_NOISE, 0.3f);
 
 /**
  * Airspeed Selector: Gate size for true airspeed fusion
@@ -72,27 +76,57 @@ PARAM_DEFINE_INT32(ASPD_TAS_GATE, 3);
 PARAM_DEFINE_INT32(ASPD_BETA_GATE, 1);
 
 /**
- * Automatic airspeed scale estimation on
+ * Controls when to apply the new esstimated airspeed scale
  *
- * Turns the automatic airspeed scale (scale from IAS to CAS) on or off. It is recommended to fly level
+ * @value 0 Disable airspeed scale estimation completely
- * altitude while performing the estimation. Set to 1 to start estimation (best when already flying).
+ * @value 1 Do not apply the new gains (logging and inside wind estimator)
- * Set to 0 to end scale estimation. The estimated scale is then saved using the ASPD_SCALE parameter.
+ * @value 2 Apply the new scale after disarm
+ * @value 3 Apply the new gains in air
+ * @group Airspeed Validator
+ */
+PARAM_DEFINE_INT32(ASPD_SCALE_APPLY, 2);
+
+/**
+ * Scale of airspeed sensor 1
  *
- * @boolean
+ * This is the scale IAS --> CAS of the first airspeed sensor instance
+ *
+ * @min 0.5
+ * @max 2.0
+ * @decimal 2
+ * @reboot_required true
  * @group Airspeed Validator
+ * @volatile
  */
-PARAM_DEFINE_INT32(ASPD_SCALE_EST, 0);
+PARAM_DEFINE_FLOAT(ASPD_SCALE_1, 1.0f);
 
 /**
- * Airspeed scale (scale from IAS to CAS)
+ * Scale of airspeed sensor 2
  *
- * Scale can either be entered manually, or estimated in-flight by setting ASPD_SCALE_EST to 1.
+ * This is the scale IAS --> CAS of the second airspeed sensor instance
  *
  * @min 0.5
- * @max 1.5
+ * @max 2.0
+ * @decimal 2
+ * @reboot_required true
+ * @group Airspeed Validator
+ * @volatile
+ */
+PARAM_DEFINE_FLOAT(ASPD_SCALE_2, 1.0f);
+
+/**
+ * Scale of airspeed sensor 3
+ *
+ * This is the scale IAS --> CAS of the third airspeed sensor instance
+ *
+ * @min 0.5
+ * @max 2.0
+ * @decimal 2
+ * @reboot_required true
  * @group Airspeed Validator
+ * @volatile
  */
-PARAM_DEFINE_FLOAT(ASPD_SCALE, 1.0f);
+PARAM_DEFINE_FLOAT(ASPD_SCALE_3, 1.0f);
 
 /**
  * Index or primary airspeed measurement source
@@ -142,6 +176,7 @@ PARAM_DEFINE_INT32(ASPD_FALLBACK_GW, 0);
 *
  * @min 0.5
  * @max 3.0
+ * @decimal 1
  * @group Airspeed Validator
  */
 PARAM_DEFINE_FLOAT(ASPD_FS_INNOV, 1.0f);
@@ -156,6 +191,7 @@ PARAM_DEFINE_FLOAT(ASPD_FS_INNOV, 1.0f);
  *
  * @unit s
  * @max 30.0
+ * @decimal 1
  * @group Airspeed Validator
  */
 PARAM_DEFINE_FLOAT(ASPD_FS_INTEG, 5.0f);