diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 60fe50f696..bba68307c4 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -438,6 +438,12 @@ FixedwingPositionControl::get_auto_airspeed_setpoint(const float control_interva MAX_WEIGHT_RATIO); } + // Adapt min airspeed setpoint based on wind estimate for more stability in higher winds. + if (_airspeed_valid && _wind_valid && _param_fw_wind_arsp_sc.get() > FLT_EPSILON) { + calibrated_min_airspeed = math::min(calibrated_min_airspeed + _param_fw_wind_arsp_sc.get() * _wind_vel.length(), + _param_fw_airspd_max.get()); + } + // Stall speed increases with the square root of the load factor times the weight ratio // Vs ~ sqrt(load_factor * weight_ratio) calibrated_min_airspeed = constrain(_param_fw_airspd_stall.get() * sqrtf(load_factor * weight_ratio), diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 6ec4a9e990..7e54b0cb3b 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -860,7 +860,9 @@ private: (ParamFloat) _param_fw_lnd_fl_sink, (ParamFloat) _param_fw_lnd_td_off, (ParamInt) _param_fw_lnd_nudge, - (ParamInt) _param_fw_lnd_abort + (ParamInt) _param_fw_lnd_abort, + + (ParamFloat) _param_fw_wind_arsp_sc ) }; 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 c66b7d8c09..29e0975e53 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 @@ -1076,3 +1076,20 @@ PARAM_DEFINE_INT32(FW_LND_NUDGE, 2); * @group FW Auto Landing */ PARAM_DEFINE_INT32(FW_LND_ABORT, 3); + +/** + * Wind-based airspeed scaling factor + * + * Multiplying this factor with the current absolute wind estimate gives the airspeed offset + * added to the minimum airspeed setpoint limit. This helps to make the + * system more robust against disturbances (turbulence) in high wind. + * Only applies to AUTO flight mode. + * + * airspeed_min_adjusted = FW_AIRSPD_MIN + FW_WIND_ARSP_SC * wind.length() + * + * @min 0 + * @decimal 2 + * @increment 0.01 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_WIND_ARSP_SC, 0.f);