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

FW Pos C: clearly define FW_AIRSPD_MIN as stall+margin, and automatically increase f(load_factor) 

Previously the minimum airspeed setpoint was adjusted to the load_factor compensated
stall speed, which, when the stall speed was set without margin, gave the controller
no room for error (the vehicle would stall if the controller has even a small airspeed
error).

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2022-11-18 16:03:41 +01:00
parent f2ca9387cf
commit c4c94febfa
2 changed files with 19 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -425,10 +425,10 @@ FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval,
}
}
float load_factor = 1.0f;
float load_factor_from_bank_angle = 1.0f;
if (PX4_ISFINITE(_att_sp.roll_body)) {
load_factor = 1.0f / cosf(_att_sp.roll_body);
load_factor_from_bank_angle = 1.0f / cosf(_att_sp.roll_body);
}
float weight_ratio = 1.0f;
@ -438,16 +438,18 @@ FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval,
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());
}
// Here we make sure that the set minimum airspeed is automatically adapted to the current load factor.
// The minimum airspeed is the controller limit (FW_AIRSPD_MIN, unless either in takeoff or landing) that should
// resemble the vehicles stall speed (CAS) with a 1g load plus some safety margin (as no controller tracks perfectly).
// Stall speed increases with the square root of the load factor: V_stall ~ sqrt(load_factor).
// The load_factor is composed of a term from the bank angle and a term from the weight ratio.
calibrated_min_airspeed *= sqrtf(load_factor_from_bank_angle * weight_ratio);
// 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),
calibrated_min_airspeed, _param_fw_airspd_max.get());
// Aditional option to increase the 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());
}
calibrated_airspeed_setpoint = constrain(calibrated_airspeed_setpoint, calibrated_min_airspeed,
_param_fw_airspd_max.get());

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

@ -524,6 +524,10 @@ PARAM_DEFINE_FLOAT(FW_LND_THRTC_SC, 1.0f);
* The minimal airspeed (calibrated airspeed) the user is able to command.
* Further, if the airspeed falls below this value, the TECS controller will try to
* increase airspeed more aggressively.
* Has to be set according to the vehicle's stall speed (which should be set in FW_AIRSPD_STALL),
* with some margin between the stall speed and minimum airspeed.
* This value corresponds to the desired minimum speed with the default load factor (level flight, default weight),
* and is automatically adpated to the current load factor (calculated from roll setpoint and WEIGHT_GROSS/WEIGHT_BASE).
*
* @unit m/s
* @min 0.5
@ -537,7 +541,8 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 10.0f);
/**
* Maximum Airspeed (CAS)
*
* If the CAS (calibrated airspeed) is above this value, the TECS controller will try to decrease
* The maximal airspeed (calibrated airspeed) the user is able to command.
* Further, if the airspeed is above this value, the TECS controller will try to decrease
* airspeed more aggressively.
*
* @unit m/s