From 1a0c23d8b344e049090e140966fcb4d22b156920 Mon Sep 17 00:00:00 2001 From: sanderux Date: Sat, 22 Jul 2017 00:51:39 +0200 Subject: [PATCH] Support thrust reversal for vtol back transition --- ROMFS/px4fmu_common/mixers/vtol_delta.aux.mix | 17 ++++++++ src/modules/vtol_att_control/standard.cpp | 39 +++++++++++++++++-- src/modules/vtol_att_control/standard.h | 5 +++ .../vtol_att_control_params.c | 22 +++++++++++ 4 files changed, 80 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/vtol_delta.aux.mix b/ROMFS/px4fmu_common/mixers/vtol_delta.aux.mix index 6b4990220b..20a9149ae6 100644 --- a/ROMFS/px4fmu_common/mixers/vtol_delta.aux.mix +++ b/ROMFS/px4fmu_common/mixers/vtol_delta.aux.mix @@ -37,3 +37,20 @@ range. Inputs below zero are treated as zero. M: 1 O: 10000 10000 0 -10000 10000 S: 1 3 0 20000 -10000 -10000 10000 + + +Reverse thrust (brake) mixer +----------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 1 6 0 20000 -10000 -10000 10000 + + +Aux1 mixer +----------------- +This is actuated on the AUX5 port + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 3 5 10000 10000 0 -10000 10000 diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 32fd427c50..3ccfc45978 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -50,6 +50,7 @@ Standard::Standard(VtolAttitudeControl *attc) : VtolType(attc), _flag_enable_mc_motors(true), _pusher_throttle(0.0f), + _reverse_output(0.0f), _airspeed_trans_blend_margin(0.0f) { _vtol_schedule.flight_mode = MC_MODE; @@ -72,6 +73,9 @@ Standard::Standard(VtolAttitudeControl *attc) : _params_handles_standard.forward_thrust_scale = param_find("VT_FWD_THRUST_SC"); _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_throttle = param_find("VT_B_REV_THR"); + } Standard::~Standard() @@ -127,6 +131,14 @@ Standard::parameters_update() param_get(_params_handles_standard.pitch_setpoint_offset, &v); _params_standard.pitch_setpoint_offset = math::radians(v); + /* reverse output */ + param_get(_params_handles_standard.reverse_output, &v); + _params_standard.reverse_output = math::constrain(v, 0.0f, 1.0f); + + /* reverse throttle */ + param_get(_params_handles_standard.reverse_throttle, &v); + _params_standard.reverse_throttle = math::constrain(v, 0.0f, 1.0f); + } @@ -138,6 +150,7 @@ void Standard::update_vtol_state() */ if (!_attc->is_fixed_wing_requested()) { + // the transition to fw mode switch is off if (_vtol_schedule.flight_mode == MC_MODE) { // in mc mode @@ -146,6 +159,8 @@ void Standard::update_vtol_state() _mc_pitch_weight = 1.0f; _mc_yaw_weight = 1.0f; _mc_throttle_weight = 1.0f; + _pusher_throttle = 0.0f; + _reverse_output = 0.0f; } else if (_vtol_schedule.flight_mode == FW_MODE) { // transition to mc mode @@ -153,12 +168,24 @@ void Standard::update_vtol_state() // Failsafe event, engage mc motors immediately _vtol_schedule.flight_mode = MC_MODE; _flag_enable_mc_motors = true; + _pusher_throttle = 0.0f; + _reverse_output = 0.0f; + } else { // Regular backtransition _vtol_schedule.flight_mode = TRANSITION_TO_MC; _flag_enable_mc_motors = true; _vtol_schedule.transition_start = hrt_absolute_time(); + + if (_params_handles_standard.reverse_output > FLT_EPSILON) { + _pusher_throttle = _params_standard.reverse_throttle; + _reverse_output = _params_standard.reverse_output; + + } else { + _pusher_throttle = 0.0f; + _reverse_output = 0.0f; + } } } else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) { @@ -168,6 +195,9 @@ void Standard::update_vtol_state() _mc_pitch_weight = 1.0f; _mc_yaw_weight = 1.0f; _mc_throttle_weight = 1.0f; + _pusher_throttle = 0.0f; + _reverse_output = 0.0f; + } else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) { // transition to MC mode if transition time has passed @@ -176,10 +206,8 @@ void Standard::update_vtol_state() (_params_standard.back_trans_dur * 1000000.0f)) { _vtol_schedule.flight_mode = MC_MODE; } - } - // the pusher motor should never be powered when in or transitioning to mc mode - _pusher_throttle = 0.0f; + } } else { // the transition to fw mode switch is on @@ -467,12 +495,15 @@ void Standard::fill_actuator_outputs() _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW]; + _actuators_out_1->control[actuator_controls_s::INDEX_AIRBRAKES] = _reverse_output; + } else { // zero outputs when inactive _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = 0.0f; _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _params->fw_pitch_trim; _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = 0.0f; + _actuators_out_1->control[actuator_controls_s::INDEX_AIRBRAKES] = 0.0f; } // set the fixed wing throttle control @@ -486,6 +517,8 @@ void Standard::fill_actuator_outputs() // otherwise we may be ramping up the throttle during the transition to fw mode _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _pusher_throttle; } + + } void diff --git a/src/modules/vtol_att_control/standard.h b/src/modules/vtol_att_control/standard.h index 306618b44b..c3b23c7ef6 100644 --- a/src/modules/vtol_att_control/standard.h +++ b/src/modules/vtol_att_control/standard.h @@ -78,6 +78,8 @@ private: float forward_thrust_scale; int airspeed_mode; float pitch_setpoint_offset; + float reverse_output; + float reverse_throttle; } _params_standard; struct { @@ -92,6 +94,8 @@ private: param_t forward_thrust_scale; param_t airspeed_mode; param_t pitch_setpoint_offset; + param_t reverse_output; + param_t reverse_throttle; } _params_handles_standard; enum vtol_mode { @@ -108,6 +112,7 @@ private: bool _flag_enable_mc_motors; float _pusher_throttle; + float _reverse_output; float _airspeed_trans_blend_margin; void set_max_mc(unsigned pwm_value); diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index 2175d9d64a..a8bca84e51 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -220,6 +220,28 @@ PARAM_DEFINE_FLOAT(VT_F_TRANS_DUR, 5.0f); */ PARAM_DEFINE_FLOAT(VT_B_TRANS_DUR, 4.0f); +/** + * Output on reverse channel during back transition + * + * @min 0 + * @max 1 + * @increment 0.01 + * @decimal 2 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_B_REV_OUT, 0.0f); + +/** + * reverse thottle output during back transition + * + * @min 0 + * @max 1 + * @increment 0.01 + * @decimal 2 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_B_REV_THR, 0.0f); + /** * Transition blending airspeed *