wind_estimator: added simple check for validity of synthetic airspeed

- synthetic airspeed will only be declared valid as soon as the wind variance
has dropped below a parameterized threshold. This is useful for vehicles without
an airspeed sensor which rely on synthetic airspeed but only once the vehicle
has turned sufficiently for the wind estimates to be reliable.

Signed-off-by: RomanBapst <bapstroman@gmail.com>

src/lib/wind_estimator/WindEstimator.cpp

@@ -78,8 +78,8 @@ WindEstimator::initialise(const matrix::Vector3f &velI, const matrix::Vector2f &
 	// get an estimate of the state covariance matrix given the estimated variance of ground velocity
 	// and measured airspeed
 	_P.setZero();
-	_P(INDEX_W_N, INDEX_W_N) = velIvar(0);
-	_P(INDEX_W_E, INDEX_W_E) = velIvar(1);
+	_P(INDEX_W_N, INDEX_W_N) = INITIAL_WIND_VAR;
+	_P(INDEX_W_E, INDEX_W_E) = INITIAL_WIND_VAR;
 	_P(INDEX_TAS_SCALE, INDEX_TAS_SCALE) = 0.0001f;
 
 	_P = L * _P * L.transpose();

src/lib/wind_estimator/WindEstimator.hpp

@@ -97,6 +97,8 @@ private:
 		INDEX_TAS_SCALE
 	};	///< enum which can be used to access state.
 
+	static constexpr float INITIAL_WIND_VAR = 5.f; // initial variance for each wind state
+
 	matrix::Vector3f _state{0.f, 0.f, 1.f};
 	matrix::Matrix3f _P;		///< state covariance matrix
 

src/modules/airspeed_selector/airspeed_selector_main.cpp

@@ -147,8 +147,8 @@ private:
 	bool _initialized{false}; /**< module initialized*/
 	bool _vehicle_local_position_valid{false}; /**< local position (from GPS) valid */
 	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 */
+	float _ground_minus_wind_TAS{NAN}; /**< true airspeed from groundspeed minus windspeed */
+	float _ground_minus_wind_CAS{NAN}; /**< calibrated airspeed from groundspeed minus windspeed */
 	bool _armed_prev{false};
 
 	hrt_abstime _time_last_airspeed_update[MAX_NUM_AIRSPEED_SENSORS] {};
@@ -184,7 +184,8 @@ private:
 		(ParamInt<px4::params::ASPD_FS_T_STOP>) _checks_fail_delay, /**< delay to declare airspeed invalid */
 		(ParamInt<px4::params::ASPD_FS_T_START>) _checks_clear_delay, /**<  delay to declare airspeed valid again */
 
-		(ParamFloat<px4::params::FW_AIRSPD_STALL>) _param_fw_airspd_stall
+		(ParamFloat<px4::params::FW_AIRSPD_STALL>) _param_fw_airspd_stall,
+		(ParamFloat<px4::params::ASPD_WVAR_THR>) _param_wind_var_threshold
 	)
 
 	void 		init(); 	/**< initialization of the airspeed validator instances */
@@ -373,7 +374,7 @@ AirspeedModule::Run()
 				// takeoff situation is active from start till one of the sensors' IAS or groundspeed_CAS is above stall speed
 				if (_in_takeoff_situation &&
 				    (airspeed_raw.indicated_airspeed_m_s > _param_fw_airspd_stall.get() ||
-				     _ground_minus_wind_CAS > _param_fw_airspd_stall.get())) {
+				     (PX4_ISFINITE(_ground_minus_wind_CAS) && _ground_minus_wind_CAS > _param_fw_airspd_stall.get()))) {
 					_in_takeoff_situation = false;
 				}
 
@@ -540,13 +541,21 @@ void AirspeedModule::update_wind_estimator_sideslip()
 
 void AirspeedModule::update_ground_minus_wind_airspeed()
 {
-	// calculate airspeed estimate based on groundspeed-windspeed to use as fallback
-	const float TAS_north = _vehicle_local_position.vx - _wind_estimate_sideslip.windspeed_north;
-	const float TAS_east = _vehicle_local_position.vy - _wind_estimate_sideslip.windspeed_east;
-	const float TAS_down = _vehicle_local_position.vz; // no wind estimate in z
-	_ground_minus_wind_TAS = sqrtf(TAS_north * TAS_north + TAS_east * TAS_east + TAS_down * TAS_down);
-	_ground_minus_wind_CAS = calc_CAS_from_TAS(_ground_minus_wind_TAS, _vehicle_air_data.baro_pressure_pa,
-				 _vehicle_air_data.baro_temp_celcius);
+	const float wind_variance = sqrtf(_wind_estimate_sideslip.variance_north * _wind_estimate_sideslip.variance_north +
+					  _wind_estimate_sideslip.variance_east * _wind_estimate_sideslip.variance_east);
+
+	if (wind_variance < _param_wind_var_threshold.get()) {
+		// calculate airspeed estimate based on groundspeed-windspeed
+		const float TAS_north = _vehicle_local_position.vx - _wind_estimate_sideslip.windspeed_north;
+		const float TAS_east = _vehicle_local_position.vy - _wind_estimate_sideslip.windspeed_east;
+		const float TAS_down = _vehicle_local_position.vz; // no wind estimate in z
+		_ground_minus_wind_TAS = sqrtf(TAS_north * TAS_north + TAS_east * TAS_east + TAS_down * TAS_down);
+		_ground_minus_wind_CAS = calc_CAS_from_TAS(_ground_minus_wind_TAS, _vehicle_air_data.baro_pressure_pa,
+					 _vehicle_air_data.baro_temp_celcius);
+
+	} else {
+		_ground_minus_wind_TAS = _ground_minus_wind_CAS = NAN;
+	}
 }
 
 

src/modules/airspeed_selector/airspeed_selector_params.c

@@ -227,3 +227,17 @@ PARAM_DEFINE_INT32(ASPD_FS_T_STOP, 2);
  * @max 1000
  */
 PARAM_DEFINE_INT32(ASPD_FS_T_START, -1);
+
+/**
+ * Horizontal wind variance threshold for synthetic airspeed.
+ *
+ * The synthetic airspeed estimate (from groundspeed and heading) will be declared valid
+ * as soon as the wind variance drops below this value.
+ *
+ * @unit (m/s)^2
+ * @min 0.001
+ * @max 5
+ * @decimal 3
+ * @group Airspeed Validator
+ */
+PARAM_DEFINE_FLOAT(ASPD_WVAR_THR, 0.3f);