FW Pos Control: add wind_mode to adapt airspeed setpoint based on wind

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

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -400,6 +400,47 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand, const
 }
 
 
+void
+FixedwingPositionControl::update_wind_mode()
+{
+	/* If the corresponding wind threshold values are set, the wind state will change if these values are
+	 * passed for a certain amount of time. The wind state is then used to increase airspeed (high wind),
+	 * resp. decrease (low wind). It takes longer to move to a lower wind state than to a higher,
+	 * as in general higher airspeed is safer. */
+
+	wind_s wind_estimate;
+
+	if (_wind_sub.update(&wind_estimate)) {
+		const matrix::Vector2f wind(wind_estimate.windspeed_north, wind_estimate.windspeed_east);
+		FW_WIND_MODE fw_wind_mode_detected(FW_WIND_MODE_NORMAL);
+
+		if (_param_fw_wind_thld_h.get() > FLT_EPSILON && wind.length() > _param_fw_wind_thld_h.get()) {
+			fw_wind_mode_detected = FW_WIND_MODE_HIGH;
+
+		} else if (_param_fw_wind_thld_l.get() > FLT_EPSILON && wind.length() < _param_fw_wind_thld_l.get()) {
+			fw_wind_mode_detected = FW_WIND_MODE_LOW;
+		}
+
+		if (fw_wind_mode_detected != _fw_wind_mode_detected_prev) {
+			_first_time_current_mode_detected = hrt_absolute_time();
+		}
+
+		const float min_time_detection_mode_down = 60.0f; // min time to switch to lower wind state
+		const float time_since_new_mode_detected = hrt_elapsed_time(&_first_time_current_mode_detected) * 1e-6f;
+
+		// immediately switch to higher wind mode after detection, but require minimum time within lower wind to switch to lower mode
+		if (fw_wind_mode_detected > _fw_wind_mode_current) {
+			_fw_wind_mode_current = fw_wind_mode_detected;
+
+		} else if (fw_wind_mode_detected < _fw_wind_mode_current
+			   && time_since_new_mode_detected > min_time_detection_mode_down) {
+			_fw_wind_mode_current = fw_wind_mode_detected;
+		}
+
+		_fw_wind_mode_detected_prev = fw_wind_mode_detected;
+	}
+}
+
 void
 FixedwingPositionControl::status_publish()
 {
@@ -669,6 +710,8 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
 	/* get circle mode */
 	const bool was_circle_mode = _l1_control.circle_mode();
 
+	update_wind_mode();
+
 	/* restore TECS parameters, in case changed intermittently (e.g. in landing handling) */
 	_tecs.set_speed_weight(_param_fw_t_spdweight.get());
 	_tecs.set_height_error_time_constant(_param_fw_t_h_error_tc.get());

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -88,6 +88,7 @@
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/orbit_status.h>
+#include <uORB/topics/wind.h>
 #include <uORB/uORB.h>
 #include <vtol_att_control/vtol_type.h>
 
@@ -151,6 +152,7 @@ private:
 	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+	uORB::Subscription _wind_sub{ORB_ID(wind)};
 
 	uORB::Publication<vehicle_attitude_setpoint_s>		_attitude_sp_pub;
 	uORB::Publication<position_controller_status_s>		_pos_ctrl_status_pub{ORB_ID(position_controller_status)};			///< navigation capabilities publication
@@ -266,6 +268,17 @@ private:
 		FW_POSCTRL_MODE_OTHER
 	} _control_mode_current{FW_POSCTRL_MODE_OTHER};		///< used to check the mode in the last control loop iteration. Use to check if the last iteration was in the same mode.
 
+	// wind state
+	hrt_abstime _first_time_current_mode_detected{0};		///< last time in normal wind mode
+
+	enum FW_WIND_MODE {
+		FW_WIND_MODE_LOW,
+		FW_WIND_MODE_NORMAL,
+		FW_WIND_MODE_HIGH,
+	} _fw_wind_mode_current{FW_WIND_MODE_NORMAL};		///< used to make wind-based airspeed setpoint adaptions
+
+	FW_WIND_MODE _fw_wind_mode_detected_prev{FW_WIND_MODE_NORMAL};
+
 	param_t _param_handle_airspeed_trans{PARAM_INVALID};
 	float _param_airspeed_trans{NAN};
 
@@ -348,7 +361,10 @@ private:
 	Vector2f 	get_nav_speed_2d(const Vector2f &ground_speed);
 	void		set_control_mode_current(const hrt_abstime &now, bool pos_sp_curr_valid);
 
-	void publishOrbitStatus(const position_setpoint_s pos_sp);
+	void 		publishOrbitStatus(const position_setpoint_s pos_sp);
+
+
+	void		update_wind_mode();
 
 	/*
 	 * Call TECS : a wrapper function to call the TECS implementation
@@ -433,7 +449,12 @@ private:
 
 		(ParamFloat<px4::params::NAV_LOITER_RAD>) _param_nav_loiter_rad,
 
-		(ParamFloat<px4::params::FW_TKO_PITCH_MIN>) _takeoff_pitch_min
+		(ParamFloat<px4::params::FW_TKO_PITCH_MIN>) _takeoff_pitch_min,
+
+		//Speed mode params
+		(ParamFloat<px4::params::FW_WIND_THLD_H>) _param_fw_wind_thld_h,
+		(ParamFloat<px4::params::FW_WIND_THLD_L>) _param_fw_wind_thld_l,
+		(ParamFloat<px4::params::FW_WIND_ARSP_OF>) _param_fw_wind_arsp_of,
 
 	)
 

src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c

@@ -854,3 +854,53 @@ PARAM_DEFINE_INT32(FW_GPSF_LT, 30);
  * @group Mission
  */
 PARAM_DEFINE_FLOAT(FW_GPSF_R, 15.0f);
+
+/**
+ * High wind threshold
+ *
+ * If the current wind estimate is above this threshold,the value of FW_WIND_ARSP_OF
+ * is added to the cruise airspeed setpoint.
+ * Set to zero number to disable wind-based airspeed setpoint up-scaling.
+ *
+ * @unit m/s
+ * @min 0.0
+ * @max 30
+ * @decimal 1
+ * @increment 0.5
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_WIND_THLD_H, 0.0f);
+
+/**
+ * Low wind threshold
+ *
+ * If the current wind estimate is below this threshold for 60 seconds,the value of
+ * FW_WIND_ARSP_OF is subtracted from the cruise airspeed setpoint.
+ * Set to zero to disable wind-based airspeed setpoint down-scaling.
+ *
+ * @unit m/s
+ * @min 0
+ * @max 30
+ * @decimal 1
+ * @increment 0.5
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_WIND_THLD_L, 0.0f);
+
+/**
+ * Wind-based airspeed setpoint offset
+ *
+ * Airspeed setpoint offset that is added/subtracted to the cruise airspeed setpoint if wind-based setpoint
+ * adaption is enabled for either or both high and low winds (FW_WIND_THLD_H and FW_WIND_THLD_L).
+ * If in high wind condition, this offset is added to the cruise airspeed setpoint.
+ * If in low wind condition, this offset is subtracted from the cruise airspeed setpoint.
+ * Note: manual airspeed setpoints via sticks must be disabled for wind-based adaption to work (see FW_POS_STK_CONF).
+ *
+ * @unit m/s
+ * @min 0
+ * @max 10
+ * @decimal 1
+ * @increment 0.01
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_WIND_ARSP_OF, 1.0f);