From c99489ee9d2b912ef11292678ba7ed8f1e131f95 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 28 May 2015 23:14:06 -0700 Subject: [PATCH] FW pos control: Implement hardcore throttle limiting in manual interaction mode --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 17ef22b0b7..18698f798a 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1389,13 +1389,19 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/ do_takeoff_help(); + /* throttle limiting */ + throttle_max = _parameters.throttle_max; + if (fabsf(_manual.z) < 0.05f) { + throttle_max = 0.0f; + } + tecs_update_pitch_throttle(_hold_alt, altctrl_airspeed, eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), _parameters.throttle_min, - _parameters.throttle_max, + throttle_max, _parameters.throttle_cruise, false, math::radians(_parameters.pitch_limit_min), @@ -1470,13 +1476,19 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/ do_takeoff_help(); + /* throttle limiting */ + throttle_max = _parameters.throttle_max; + if (fabsf(_manual.z) < 0.05f) { + throttle_max = 0.0f; + } + tecs_update_pitch_throttle(_hold_alt, altctrl_airspeed, eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), _parameters.throttle_min, - _parameters.throttle_max, + throttle_max, _parameters.throttle_cruise, false, math::radians(_parameters.pitch_limit_min),