dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

FW Position control: add option to scale min airspeed wtih wind magnitude 

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2022-09-20 18:16:58 +02:00
parent e8ac9b266b
commit c6c7733587
3 changed files with 26 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
View File

@ -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),

4
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
View File

@ -860,7 +860,9 @@ private:
(ParamFloat<px4::params::FW_LND_FL_SINK>) _param_fw_lnd_fl_sink,
(ParamFloat<px4::params::FW_LND_TD_OFF>) _param_fw_lnd_td_off,
(ParamInt<px4::params::FW_LND_NUDGE>) _param_fw_lnd_nudge,
(ParamInt<px4::params::FW_LND_ABORT>) _param_fw_lnd_abort
(ParamInt<px4::params::FW_LND_ABORT>) _param_fw_lnd_abort,
(ParamFloat<px4::params::FW_WIND_ARSP_SC>) _param_fw_wind_arsp_sc
)
};

17
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
View File

@ -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);