From faf06ea42ad8a7fda7363b0795fe5f196e637c04 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 8 Nov 2021 17:58:46 +0100 Subject: [PATCH] FW Pos Control: add wind_mode to adapt airspeed setpoint based on wind Signed-off-by: Silvan Fuhrer --- .../FixedwingPositionControl.cpp | 43 ++++++++++++++++ .../FixedwingPositionControl.hpp | 25 +++++++++- .../fw_pos_control_l1_params.c | 50 +++++++++++++++++++ 3 files changed, 116 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 21b602f6ed..f538858cc8 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/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()); diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index ddca722668..f5d72bafd3 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -88,6 +88,7 @@ #include #include #include +#include #include #include @@ -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 _attitude_sp_pub; uORB::Publication _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) _param_nav_loiter_rad, - (ParamFloat) _takeoff_pitch_min + (ParamFloat) _takeoff_pitch_min, + + //Speed mode params + (ParamFloat) _param_fw_wind_thld_h, + (ParamFloat) _param_fw_wind_thld_l, + (ParamFloat) _param_fw_wind_arsp_of, ) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 156006326a..63305de15d 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/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);