FWPositionControl: add extra check for low wind variance to use TAS r…

…ate estimate in TECS

Signed-off-by: Silvan Fuhrer <[email protected]>

src/modules/fw_path_navigation/FixedwingPositionControl.cpp

@@ -296,22 +296,19 @@ FixedwingPositionControl::wind_poll()
 		_wind_sub.update(&wind);
 
 		// assumes wind is valid if finite
-		_wind_valid = PX4_ISFINITE(wind.windspeed_north)
-			      && PX4_ISFINITE(wind.windspeed_east);
-
+		_wind_valid = PX4_ISFINITE(wind.windspeed_north) && PX4_ISFINITE(wind.windspeed_east);
 		_time_wind_last_received = hrt_absolute_time();
 
-		_wind_vel(0) = wind.windspeed_north;
-		_wind_vel(1) = wind.windspeed_east;
+		_wind_vel = {wind.windspeed_north, wind.windspeed_east};
+		_wind_var = {wind.variance_north, wind.variance_east};
 
 	} else {
 		// invalidate wind estimate usage (and correspondingly NPFG, if enabled) after subscription timeout
 		_wind_valid = _wind_valid && (hrt_absolute_time() - _time_wind_last_received) < WIND_EST_TIMEOUT;
 	}
 
 	if (!_wind_valid) {
-		_wind_vel(0) = 0.f;
-		_wind_vel(1) = 0.f;
+		_wind_vel.setZero();
 	}
 }
 
@@ -2556,6 +2553,9 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
 	const Vector3f local_pos_a = Vector3f{_local_pos.ax, _local_pos.ay, _local_pos.az};
 	const Vector3f groundspeed_minus_wind = Vector3f{_local_pos.vx - _wind_vel(0), _local_pos.vy - _wind_vel(1), _local_pos.vz};
 
+	// extra check: wind variance of each component max FW_WIND_VAR_THLD
+	const bool wind_valid_extra = _wind_valid && math::max(_wind_var(0), _wind_var(1)) < _param_fw_wind_var_thld.get();
+
 	// additionally only use it if local position has valid velocitis and we're currently in a position-controlled mode, as
 	// then we can assume that local_position has valid fields
 	const bool use_groundspeed_acceleration = wind_valid_extra && _local_pos.v_xy_valid && _local_pos.v_z_valid

src/modules/fw_path_navigation/FixedwingPositionControl.hpp

@@ -378,8 +378,8 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
 
 	// WIND
 
-	// [m/s] wind velocity vector
-	Vector2f _wind_vel{0.0f, 0.0f};
+	Vector2f _wind_vel{0.f, 0.f}; // [m/s] wind velocity vector
+	Vector2f _wind_var{0.f, 0.f}; // [(m/s)^2] wind velocity variance
 
 	bool _wind_valid{false};
 
@@ -954,7 +954,8 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
 		(ParamFloat<px4::params::FW_TKO_AIRSPD>) _param_fw_tko_airspd,
 
 		(ParamFloat<px4::params::RWTO_PSP>) _param_rwto_psp,
-		(ParamBool<px4::params::FW_LAUN_DETCN_ON>) _param_fw_laun_detcn_on
+		(ParamBool<px4::params::FW_LAUN_DETCN_ON>) _param_fw_laun_detcn_on,
+		(ParamFloat<px4::params::FW_WIND_VAR_THLD>) _param_fw_wind_var_thld
 	)
 
 };

src/modules/fw_path_navigation/fw_path_navigation_params.c

@@ -1063,3 +1063,23 @@ PARAM_DEFINE_FLOAT(FW_SPOILERS_LND, 0.f);
  * @group FW Attitude Control
  */
 PARAM_DEFINE_FLOAT(FW_SPOILERS_DESC, 0.f);
+
+/**
+ * Wind variance threshold for usage of TAS rate estimate
+ *
+ * The wind estimate is used to determine a 3D airspeed vector which is then used to estimate the
+ * current airspeed derivative, which in turn is an input for TECS.
+ *
+ * To pass the check both the north and east component of the wind estimate need to have reported
+ * variances below this threshold.
+ * Set to 0 if the airspeed rate estimate shouldn't be used at all (not possible to estimate without wind).
+ * If airspeed rate is not estimated, it is assumed to be 0. TECS is then generally slower to reduce
+ * airspeed errors.
+ *
+ * @unit (m/s)^2
+ * @min 0.0
+ * @decimal 2
+ * @increment 0.01
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_WIND_VAR_THLD, 0.1f);