From 024777ec7ce04b115c66977c282f788b025c2999 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 9 Nov 2021 15:12:31 +0100 Subject: [PATCH] FW Position Control: add airspeed offset based on wind in Eco mode setpoint = FW_AIRSPD_MIN + FW_LND_AIRSPD_SC * wind Signed-off-by: Silvan Fuhrer --- .../FixedwingPositionControl.cpp | 55 +++++-------------- .../FixedwingPositionControl.hpp | 15 +---- .../fw_pos_control_l1_params.c | 50 +++-------------- 3 files changed, 23 insertions(+), 97 deletions(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index b2ed628795..b7849330c7 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -364,6 +364,19 @@ FixedwingPositionControl::get_cruise_airspeed_setpoint(const hrt_abstime &now, c if (_speed_mode_current == FW_SPEED_MODE::FW_SPEED_MODE_ECO) { airspeed_setpoint = _param_fw_airspd_min.get(); + // Adapt cruise airspeed setpoint based on wind estimate (disable in airspeed-less mode) + if (_airspeed_valid && _param_fw_wind_arsp_sc.get() > FLT_EPSILON) { + wind_s wind_estimate; + + if (_wind_sub.update(&wind_estimate)) { + const matrix::Vector2f wind(wind_estimate.windspeed_north, wind_estimate.windspeed_east); + + if (PX4_ISFINITE(wind.length())) { + airspeed_setpoint += _param_fw_wind_arsp_sc.get() * wind.length(); + } + } + } + } else if (_speed_mode_current == FW_SPEED_MODE::FW_SPEED_MODE_DASH) { airspeed_setpoint = _param_fw_airspd_max.get(); } @@ -504,47 +517,6 @@ FixedwingPositionControl::resetAutoSpeedAdaptions() _speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_NORMAL; } -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() { @@ -816,7 +788,6 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c check_eco_dash_allowed(); updateSpeedMode(); - update_wind_mode(); /* restore TECS parameters, in case changed intermittently (e.g. in landing handling) */ _tecs.set_speed_weight(_param_fw_t_spdweight.get()); diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 949f4fc7d7..d52d0f95d5 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -285,17 +285,6 @@ private: bool _conditions_for_eco_dash_met{false}; hrt_abstime _time_conditions_not_met{0}; - // 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}; - float _last_airspeed_setpoint{NAN}; param_t _param_handle_airspeed_trans{PARAM_INVALID}; @@ -477,9 +466,7 @@ private: (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, + (ParamFloat) _param_fw_wind_arsp_sc, (ParamInt) _fw_spd_mode_set, (ParamFloat) fw_alt_min, 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 09d85b639c..a3815e74a9 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 @@ -856,54 +856,22 @@ PARAM_DEFINE_INT32(FW_GPSF_LT, 30); PARAM_DEFINE_FLOAT(FW_GPSF_R, 15.0f); /** - * High wind threshold + * Wind-based airspeed scaling factor in Eco * - * 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. + * Multiplying this factor with the current absolute wind estimate gives the airspeed offset + * added to the setpoint (which is FW_AIRSPD_MIN) in Eco mode. This helps to make the + * system more robust against disturbances in high wind. * - * @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 + * setpoint = FW_AIRSPD_MIN + FW_LND_AIRSPD_SC * wind * - * 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 + * @unit norm * @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 + * @max 2 + * @decimal 2 * @increment 0.01 * @group FW TECS */ -PARAM_DEFINE_FLOAT(FW_WIND_ARSP_OF, 1.0f); +PARAM_DEFINE_FLOAT(FW_WIND_ARSP_SC, 0.0f); /** * FW Speed mode setting