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

FW Pos Control: add in_takeoff_situation argument to adapt_airspeed_setpoint() 

when we're in a takeoff situation, we only want to adapt the airspeed to
avoid accelerated stall due to load factor changes. Disable othre logic
like minimum ground speed, wind based adaption and airspeed slew rating.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2023-05-11 14:16:06 +02:00 committed by Daniel Agar
parent 3c9c5f7184
commit ccef260f10
2 changed files with 10 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/modules/fw_pos_control/FixedwingPositionControl.cpp
View File

@ -399,14 +399,14 @@ FixedwingPositionControl::get_manual_airspeed_setpoint()
float
FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval, float calibrated_airspeed_setpoint,
float calibrated_min_airspeed, const Vector2f &ground_speed)
float calibrated_min_airspeed, const Vector2f &ground_speed, bool in_takeoff_situation)
{
if (!PX4_ISFINITE(calibrated_airspeed_setpoint) || calibrated_airspeed_setpoint <= FLT_EPSILON) {
calibrated_airspeed_setpoint = _param_fw_airspd_trim.get();
}
// Adapt cruise airspeed when otherwise the min groundspeed couldn't be maintained
if (!_wind_valid) {
if (!_wind_valid && !in_takeoff_situation) {
/*
* This error value ensures that a plane (as long as its throttle capability is
* not exceeded) travels towards a waypoint (and is not pushed more and more away
@ -441,7 +441,7 @@ FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval,
calibrated_min_airspeed *= sqrtf(load_factor_from_bank_angle * weight_ratio);
// 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) {
if (!in_takeoff_situation && _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());
}
@ -453,7 +453,8 @@ FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval,
const bool slewed_airspeed_outside_of_limits = _airspeed_slew_rate_controller.getState() < calibrated_min_airspeed
|| _airspeed_slew_rate_controller.getState() > _param_fw_airspd_max.get();
if (!PX4_ISFINITE(_airspeed_slew_rate_controller.getState()) || slewed_airspeed_outside_of_limits) {
if (in_takeoff_situation || !PX4_ISFINITE(_airspeed_slew_rate_controller.getState())
|| slewed_airspeed_outside_of_limits) {
_airspeed_slew_rate_controller.setForcedValue(calibrated_airspeed_setpoint);
} else if (control_interval > FLT_EPSILON) {
@ -1302,7 +1303,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
}
float target_airspeed = adapt_airspeed_setpoint(control_interval, takeoff_airspeed, adjusted_min_airspeed,
ground_speed);
ground_speed, true);
if (_runway_takeoff.runwayTakeoffEnabled()) {
if (!_runway_takeoff.isInitialized()) {
@ -1938,7 +1939,7 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval,
updateManualTakeoffStatus();
const float calibrated_airspeed_sp = adapt_airspeed_setpoint(control_interval, get_manual_airspeed_setpoint(),
_param_fw_airspd_min.get(), ground_speed);
_param_fw_airspd_min.get(), ground_speed, !_completed_manual_takeoff);
const float height_rate_sp = getManualHeightRateSetpoint();
// TECS may try to pitch down to gain airspeed if we underspeed, constrain the pitch when underspeeding if we are
@ -1979,7 +1980,7 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
updateManualTakeoffStatus();
float calibrated_airspeed_sp = adapt_airspeed_setpoint(control_interval, get_manual_airspeed_setpoint(),
_param_fw_airspd_min.get(), ground_speed);
_param_fw_airspd_min.get(), ground_speed, !_completed_manual_takeoff);
const float height_rate_sp = getManualHeightRateSetpoint();
// TECS may try to pitch down to gain airspeed if we underspeed, constrain the pitch when underspeeding if we are

3
src/modules/fw_pos_control/FixedwingPositionControl.hpp
View File

@ -669,10 +669,11 @@ private:
* @param calibrated_airspeed_setpoint Calibrated airspeed septoint (generally from the position setpoint) [m/s]
* @param calibrated_min_airspeed Minimum calibrated airspeed [m/s]
* @param ground_speed Vehicle ground velocity vector (NE) [m/s]
* @param in_takeoff_situation Vehicle is currently in a takeoff situation
* @return Adjusted calibrated airspeed setpoint [m/s]
*/
float adapt_airspeed_setpoint(const float control_interval, float calibrated_airspeed_setpoint,
float calibrated_min_airspeed, const Vector2f &ground_speed);
float calibrated_min_airspeed, const Vector2f &ground_speed, bool in_takeoff_situation = false);
void reset_takeoff_state();
void reset_landing_state();