From 595d706eaf71b23831a1eb1f2d31f3a14b134ec5 Mon Sep 17 00:00:00 2001 From: sanderux Date: Wed, 30 Aug 2017 00:36:52 +0200 Subject: [PATCH] Reverse pusher delay Thist adds a delay for the reverse thrust to allow the motor to brake and avoid sync issues. --- src/modules/vtol_att_control/standard.cpp | 21 ++++++++++++++----- src/modules/vtol_att_control/standard.h | 2 ++ .../vtol_att_control/standard_params.c | 14 +++++++++++++ 3 files changed, 32 insertions(+), 5 deletions(-) diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 29a7ba99ce..638f4c6dca 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -75,6 +75,7 @@ Standard::Standard(VtolAttitudeControl *attc) : _params_handles_standard.airspeed_mode = param_find("FW_ARSP_MODE"); _params_handles_standard.pitch_setpoint_offset = param_find("FW_PSP_OFF"); _params_handles_standard.reverse_output = param_find("VT_B_REV_OUT"); + _params_handles_standard.reverse_delay = param_find("VT_B_REV_DEL"); _params_handles_standard.back_trans_throttle = param_find("VT_B_TRANS_THR"); _params_handles_standard.mpc_xy_cruise = param_find("MPC_XY_CRUISE"); @@ -141,6 +142,10 @@ Standard::parameters_update() param_get(_params_handles_standard.reverse_output, &v); _params_standard.reverse_output = math::constrain(v, 0.0f, 1.0f); + /* reverse output */ + param_get(_params_handles_standard.reverse_delay, &v); + _params_standard.reverse_delay = math::constrain(v, 0.0f, 10.0f); + /* reverse throttle */ param_get(_params_handles_standard.back_trans_throttle, &v); _params_standard.back_trans_throttle = math::constrain(v, -1.0f, 1.0f); @@ -346,11 +351,17 @@ void Standard::update_transition_state() q_sp.copyTo(_v_att_sp->q_d); _v_att_sp->q_d_valid = true; - // Handle throttle reversal for active breaking - float thrscale = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.front_trans_dur * - 1000000.0f); - thrscale = math::constrain(thrscale, 0.0f, 1.0f); - _pusher_throttle = thrscale * _params_standard.back_trans_throttle; + hrt_abstime btrans_start; + btrans_start = _vtol_schedule.transition_start + uint64_t(_params_standard.reverse_delay) * 1000000.0f; + _pusher_throttle = 0.0f; + + if (hrt_absolute_time() >= btrans_start) { + // Handle throttle reversal for active breaking + float thrscale = (float)hrt_elapsed_time(&btrans_start) / (_params_standard.front_trans_dur * + 1000000.0f); + thrscale = math::constrain(thrscale, 0.0f, 1.0f); + _pusher_throttle = thrscale * _params_standard.back_trans_throttle; + } // continually increase mc attitude control as we transition back to mc mode if (_params_standard.back_trans_ramp > FLT_EPSILON) { diff --git a/src/modules/vtol_att_control/standard.h b/src/modules/vtol_att_control/standard.h index 77afcbfb5d..8488c50a0d 100644 --- a/src/modules/vtol_att_control/standard.h +++ b/src/modules/vtol_att_control/standard.h @@ -80,6 +80,7 @@ private: int airspeed_mode; float pitch_setpoint_offset; float reverse_output; + float reverse_delay; float back_trans_throttle; float mpc_xy_cruise; } _params_standard; @@ -98,6 +99,7 @@ private: param_t airspeed_mode; param_t pitch_setpoint_offset; param_t reverse_output; + param_t reverse_delay; param_t back_trans_throttle; param_t mpc_xy_cruise; } _params_handles_standard; diff --git a/src/modules/vtol_att_control/standard_params.c b/src/modules/vtol_att_control/standard_params.c index 03622c41b6..c74ba7595a 100644 --- a/src/modules/vtol_att_control/standard_params.c +++ b/src/modules/vtol_att_control/standard_params.c @@ -98,6 +98,20 @@ PARAM_DEFINE_FLOAT(VT_B_TRANS_RAMP, 3.0f); */ PARAM_DEFINE_FLOAT(VT_B_REV_OUT, 0.0f); + +/** + * Delay in seconds before applying back transition throttle + * Set this to a value greater than 0 to give the motor time to spin down. + * + * unit s + * @min 0 + * @max 10 + * @increment 1 + * @decimal 2 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_B_REV_DEL, 0.0f); + /** * Thottle output during back transition * For ESCs and mixers that support reverse thrust on low PWM values set this to a negative value to apply active breaking