From 4d7b014871b8c5e0fb94504256629b0a48e2ec20 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 12 Aug 2024 10:39:29 +0200 Subject: [PATCH] FW Position Control: make explicit when underspeed detection logic is en-/disabled Signed-off-by: Silvan Fuhrer --- .../FixedwingPositionControl.cpp | 31 +++++++++++++------ 1 file changed, 22 insertions(+), 9 deletions(-) diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index e3a67180ed96..ed94f5427e83 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -969,6 +969,7 @@ FixedwingPositionControl::control_auto_descend(const float control_interval) // Hard-code descend rate to 0.5m/s. This is a compromise to give the system to recover, // but not letting it drift too far away. const float descend_rate = -0.5f; + const bool disable_underspeed_handling = false; tecs_update_pitch_throttle(control_interval, _current_altitude, @@ -979,7 +980,7 @@ FixedwingPositionControl::control_auto_descend(const float control_interval) _param_fw_thr_max.get(), _param_sinkrate_target.get(), _param_climbrate_target.get(), - false, + disable_underspeed_handling, descend_rate); _att_sp.roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle @@ -1161,6 +1162,7 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co target_airspeed = _npfg.getAirspeedRef() / _eas2tas; _att_sp.yaw_body = _yaw; + const bool disable_underspeed_handling = false; tecs_update_pitch_throttle(control_interval, position_sp_alt, @@ -1171,7 +1173,7 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co tecs_fw_thr_max, _param_sinkrate_target.get(), _param_climbrate_target.get(), - false, + disable_underspeed_handling, pos_sp_curr.vz); } @@ -1516,6 +1518,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _tecs.resetIntegrals(); } + const bool disable_underspeed_handling = true; + tecs_update_pitch_throttle(control_interval, altitude_setpoint_amsl, target_airspeed, @@ -1525,7 +1529,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _param_fw_thr_max.get(), _param_sinkrate_target.get(), _performance_model.getMaximumClimbRate(_air_density), - true); // disable underspeed handling + disable_underspeed_handling); _tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed()); // reset after TECS calculation @@ -1594,6 +1598,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo const float max_takeoff_throttle = (_launchDetector.getLaunchDetected() < launch_detection_status_s::STATE_FLYING) ? _param_fw_thr_idle.get() : _param_fw_thr_max.get(); + const bool disable_underspeed_handling = true; tecs_update_pitch_throttle(control_interval, altitude_setpoint_amsl, @@ -1604,7 +1609,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo max_takeoff_throttle, _param_sinkrate_target.get(), _performance_model.getMaximumClimbRate(_air_density), - true); // disable underspeed handling + disable_underspeed_handling); if (_launchDetector.getLaunchDetected() < launch_detection_status_s::STATE_FLYING) { // explicitly set idle throttle until motors are enabled @@ -1772,6 +1777,7 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, const float throttle_max = flare_ramp_interpolator_sqrt * _param_fw_thr_idle.get() + (1.0f - flare_ramp_interpolator_sqrt) * _param_fw_thr_max.get(); + const bool disable_underspeed_handling = true; tecs_update_pitch_throttle(control_interval, altitude_setpoint, @@ -1782,7 +1788,7 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, throttle_max, _param_sinkrate_target.get(), _param_climbrate_target.get(), - true, + disable_underspeed_handling, height_rate_setpoint); /* set the attitude and throttle commands */ @@ -1975,6 +1981,7 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, const float throttle_max = flare_ramp_interpolator_sqrt * _param_fw_thr_idle.get() + (1.0f - flare_ramp_interpolator_sqrt) * _param_fw_thr_max.get(); + const bool disable_underspeed_handling = true; tecs_update_pitch_throttle(control_interval, _current_altitude, // is not controlled, control descend rate @@ -1985,7 +1992,7 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, throttle_max, _param_sinkrate_target.get(), _param_climbrate_target.get(), - true, + disable_underspeed_handling, height_rate_setpoint); /* set the attitude and throttle commands */ @@ -2031,6 +2038,8 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, const float glide_slope_sink_rate = airspeed_land * glide_slope / sqrtf(glide_slope * glide_slope + 1.0f); const float desired_max_sinkrate = math::min(math::max(glide_slope_sink_rate, _param_sinkrate_target.get()), _param_fw_t_sink_max.get()); + const float disable_underspeed_handling = false; + tecs_update_pitch_throttle(control_interval, _current_altitude, // is not controlled, control descend rate target_airspeed, @@ -2040,7 +2049,7 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, _param_fw_thr_max.get(), desired_max_sinkrate, _param_climbrate_target.get(), - false, + disable_underspeed_handling, -glide_slope_sink_rate); // heightrate = -sinkrate /* set the attitude and throttle commands */ @@ -2093,6 +2102,8 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval, throttle_max = 0.0f; } + const bool disable_underspeed_handling = false; + tecs_update_pitch_throttle(control_interval, _current_altitude, calibrated_airspeed_sp, @@ -2102,7 +2113,7 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval, throttle_max, _param_sinkrate_target.get(), _param_climbrate_target.get(), - false, + disable_underspeed_handling, height_rate_sp); _att_sp.roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get()); @@ -2185,6 +2196,8 @@ FixedwingPositionControl::control_manual_position(const float control_interval, } } + const bool disable_underspeed_handling = false; + tecs_update_pitch_throttle(control_interval, _current_altitude, // TODO: check if this is really what we want.. or if we want to lock the altitude. calibrated_airspeed_sp, @@ -2194,7 +2207,7 @@ FixedwingPositionControl::control_manual_position(const float control_interval, throttle_max, _param_sinkrate_target.get(), _param_climbrate_target.get(), - false, + disable_underspeed_handling, height_rate_sp); if (!_yaw_lock_engaged || fabsf(_manual_control_setpoint.roll) >= HDG_HOLD_MAN_INPUT_THRESH ||