Support thrust reversal for vtol back transition

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

src/modules/navigator/mission_block.cpp

@@ -65,7 +65,7 @@ MissionBlock::MissionBlock(Navigator *navigator, const char *name) :
 	_param_vtol_wv_takeoff(this, "VT_WV_TKO_EN", false),
 	_param_vtol_wv_loiter(this, "VT_WV_LTR_EN", false),
 	_param_force_vtol(this, "NAV_FORCE_VT", false),
-	_param_back_trans_dur(this, "VT_B_TRANS_DUR", false)
+	_param_back_trans_dec_mss(this, "VT_B_DEC_MSS", false)
 {
 }
 
@@ -296,12 +296,11 @@ MissionBlock::is_mission_item_reached()
 			/* for vtol back transition calculate acceptance radius based on time and ground speed */
 			if (_mission_item.vtol_back_transition) {
 
-				float groundspeed = sqrtf(_navigator->get_global_position()->vel_n * _navigator->get_global_position()->vel_n +
-							  _navigator->get_global_position()->vel_e * _navigator->get_global_position()->vel_e);
+				float velocity = sqrtf(_navigator->get_local_position()->vx * _navigator->get_local_position()->vx +
+						       _navigator->get_local_position()->vy * _navigator->get_local_position()->vy);
 
-				if (_param_back_trans_dur.get() > FLT_EPSILON && groundspeed > FLT_EPSILON
-				    && groundspeed * _param_back_trans_dur.get() > mission_acceptance_radius) {
-					mission_acceptance_radius = groundspeed * _param_back_trans_dur.get();
+				if (_param_back_trans_dec_mss.get() > FLT_EPSILON && velocity > FLT_EPSILON) {
+					mission_acceptance_radius = (velocity / _param_back_trans_dec_mss.get() / 2) * velocity;
 				}
 
 			}

src/modules/navigator/mission_block.h

@@ -148,7 +148,7 @@ class MissionBlock : public NavigatorMode
 	control::BlockParamInt _param_vtol_wv_takeoff;
 	control::BlockParamInt _param_vtol_wv_loiter;
 	control::BlockParamInt _param_force_vtol;
-	control::BlockParamFloat _param_back_trans_dur;
+	control::BlockParamFloat _param_back_trans_dec_mss;
 };
 
 #endif

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;
@@ -63,6 +64,7 @@ Standard::Standard(VtolAttitudeControl *attc) :
 
 	_params_handles_standard.front_trans_dur = param_find("VT_F_TRANS_DUR");
 	_params_handles_standard.back_trans_dur = param_find("VT_B_TRANS_DUR");
+	_params_handles_standard.back_trans_ramp = param_find("VT_B_TRANS_RAMP");
 	_params_handles_standard.pusher_trans = param_find("VT_TRANS_THR");
 	_params_handles_standard.airspeed_blend = param_find("VT_ARSP_BLEND");
 	_params_handles_standard.airspeed_trans = param_find("VT_ARSP_TRANS");
@@ -72,6 +74,10 @@ 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.back_trans_throttle = param_find("VT_B_TRANS_THR");
+	_params_handles_standard.mpc_xy_cruise = param_find("MPC_XY_CRUISE");
+
 }
 
 Standard::~Standard()
@@ -92,6 +98,10 @@ Standard::parameters_update()
 	param_get(_params_handles_standard.back_trans_dur, &v);
 	_params_standard.back_trans_dur = math::constrain(v, 0.0f, 20.0f);
 
+	/* MC ramp up during back transition to mc mode */
+	param_get(_params_handles_standard.back_trans_ramp, &v);
+	_params_standard.back_trans_ramp = math::constrain(v, 0.0f, _params_standard.back_trans_dur);
+
 	/* target throttle value for pusher motor during the transition to fw mode */
 	param_get(_params_handles_standard.pusher_trans, &v);
 	_params_standard.pusher_trans = math::constrain(v, 0.0f, 5.0f);
@@ -127,6 +137,16 @@ 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.back_trans_throttle, &v);
+	_params_standard.back_trans_throttle = math::constrain(v, -1.0f, 1.0f);
+
+	/* mpc cruise speed */
+	param_get(_params_handles_standard.mpc_xy_cruise, &_params_standard.mpc_xy_cruise);
 
 }
 
@@ -138,6 +158,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,19 +167,26 @@ 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
 			if (_vtol_vehicle_status->vtol_transition_failsafe == true) {
 				// 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();
+				_reverse_output = _params_standard.reverse_output;
+
 			}
 
 		} else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) {
@@ -168,18 +196,22 @@ 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
 			// XXX: base this on XY hold velocity of MC
+			float vel = sqrtf(_local_pos->vx * _local_pos->vx + _local_pos->vy * _local_pos->vy);
+
 			if (hrt_elapsed_time(&_vtol_schedule.transition_start) >
-			    (_params_standard.back_trans_dur * 1000000.0f)) {
+			    (_params_standard.back_trans_dur * 1000000.0f) ||
+			    (_local_pos->v_xy_valid && vel <= _params_standard.mpc_xy_cruise)) {
 				_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
@@ -314,10 +346,16 @@ 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;
+
 		// continually increase mc attitude control as we transition back to mc mode
-		if (_params_standard.back_trans_dur > FLT_EPSILON) {
+		if (_params_standard.back_trans_ramp > FLT_EPSILON) {
 			float weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) /
-				       ((_params_standard.back_trans_dur / 2) * 1000000.0f);
+				       ((_params_standard.back_trans_ramp) * 1000000.0f);
 			weight = math::constrain(weight, 0.0f, 1.0f);
 			_mc_roll_weight = weight;
 			_mc_pitch_weight = weight;
@@ -467,12 +505,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 +527,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

src/modules/vtol_att_control/standard.h

@@ -69,6 +69,7 @@ class Standard : public VtolType
 	struct {
 		float front_trans_dur;
 		float back_trans_dur;
+		float back_trans_ramp;
 		float pusher_trans;
 		float airspeed_blend;
 		float airspeed_trans;
@@ -78,11 +79,15 @@ class Standard : public VtolType
 		float forward_thrust_scale;
 		int airspeed_mode;
 		float pitch_setpoint_offset;
+		float reverse_output;
+		float back_trans_throttle;
+		float mpc_xy_cruise;
 	} _params_standard;
 
 	struct {
 		param_t front_trans_dur;
 		param_t back_trans_dur;
+		param_t back_trans_ramp;
 		param_t pusher_trans;
 		param_t airspeed_blend;
 		param_t airspeed_trans;
@@ -92,6 +97,9 @@ class Standard : public VtolType
 		param_t forward_thrust_scale;
 		param_t airspeed_mode;
 		param_t pitch_setpoint_offset;
+		param_t reverse_output;
+		param_t back_trans_throttle;
+		param_t mpc_xy_cruise;
 	} _params_handles_standard;
 
 	enum vtol_mode {
@@ -108,6 +116,7 @@ class Standard : public VtolType
 
 	bool _flag_enable_mc_motors;
 	float _pusher_throttle;
+	float _reverse_output;
 	float _airspeed_trans_blend_margin;
 
 	void set_max_mc(unsigned pwm_value);

src/modules/vtol_att_control/standard_params.c

@@ -72,3 +72,41 @@ PARAM_DEFINE_FLOAT(VT_DWN_PITCH_MAX, 5.0f);
  * @group VTOL Attitude Control
  */
 PARAM_DEFINE_FLOAT(VT_FWD_THRUST_SC, 0.0f);
+
+/**
+ * Back transition MC motor ramp up time
+ *
+ * This sets the duration during wich the MC motors ramp up to the commanded thrust during the back transition stage.
+ *
+ * @unit s
+ * @min 0.0
+ * @max 20.0
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_B_TRANS_RAMP, 3.0f);
+
+/**
+ * Output on airbrakes channel during back transition
+ * Used for airbrakes or with ESCs that have reverse thrust enabled on a seperate channel
+ * Airbrakes need to be enables for your selected model/mixer
+ *
+ * @min 0
+ * @max 1
+ * @increment 0.01
+ * @decimal 2
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_B_REV_OUT, 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
+ * For ESCs that support thrust reversal with a control channel please set VT_B_REV_OUT and set this to a positive value to apply active breaking
+ *
+ * @min -1
+ * @max 1
+ * @increment 0.01
+ * @decimal 2
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_B_TRANS_THR, 0.0f);

src/modules/vtol_att_control/vtol_att_control_params.c

@@ -220,6 +220,21 @@ PARAM_DEFINE_FLOAT(VT_F_TRANS_DUR, 5.0f);
  */
 PARAM_DEFINE_FLOAT(VT_B_TRANS_DUR, 4.0f);
 
+/**
+ * Approximate deceleration during back transition
+ *
+ * The approximate deceleration during a back transition in m/s/s
+ * Used to calculate back transition distance in mission mode. A lower value will make the VTOL transition further from the destination waypoint.
+ *
+ * @unit m/s/s
+ * @min 0.00
+ * @max 20.00
+ * @increment 1
+ * @decimal 2
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_B_DEC_MSS, 2.0f);
+
 /**
  * Transition blending airspeed
  *