From ccef260f101d2bb7e1dc96cf554cb5d1c6c93fd9 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 11 May 2023 14:16:06 +0200 Subject: [PATCH] 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 --- .../fw_pos_control/FixedwingPositionControl.cpp | 15 ++++++++------- .../fw_pos_control/FixedwingPositionControl.hpp | 3 ++- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index 9f7c05ce1e..078cab992a 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -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 diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp index 1fccd36099..a1e7b5fca9 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.hpp @@ -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();