Refactor quadchute logic (#20704)

* moved computation of _time_since_trans_start into base class

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* refactor quadchute logic
- move entired logic into VtolType class
- split into smaller functions

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* Update src/modules/vtol_att_control/vtol_type.h

Co-authored-by: Silvan Fuhrer <silvan@auterion.com>

Signed-off-by: RomanBapst <bapstroman@gmail.com>
Co-authored-by: Silvan Fuhrer <silvan@auterion.com>

src/modules/vtol_att_control/standard.cpp

@@ -52,13 +52,6 @@ using namespace matrix;
 Standard::Standard(VtolAttitudeControl *attc) :
 	VtolType(attc)
 {
-	_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
-	_vtol_schedule.transition_start = 0;
-
-	_mc_roll_weight = 1.0f;
-	_mc_pitch_weight = 1.0f;
-	_mc_yaw_weight = 1.0f;
-	_mc_throttle_weight = 1.0f;
 }
 
 void
@@ -78,11 +71,10 @@ void Standard::update_vtol_state()
 	 */
 
 	float mc_weight = _mc_roll_weight;
-	float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
 
 	if (_vtol_vehicle_status->vtol_transition_failsafe) {
 		// Failsafe event, engage mc motors immediately
-		_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
+		_vtol_mode = vtol_mode::MC_MODE;
 		_pusher_throttle = 0.0f;
 		_reverse_output = 0.0f;
 
@@ -94,26 +86,26 @@ void Standard::update_vtol_state()
 	} else if (!_attc->is_fixed_wing_requested()) {
 
 		// the transition to fw mode switch is off
-		if (_vtol_schedule.flight_mode == vtol_mode::MC_MODE) {
+		if (_vtol_mode == vtol_mode::MC_MODE) {
 			// in mc mode
-			_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
+			_vtol_mode = vtol_mode::MC_MODE;
 			mc_weight = 1.0f;
 			_reverse_output = 0.0f;
 
-		} else if (_vtol_schedule.flight_mode == vtol_mode::FW_MODE) {
+		} else if (_vtol_mode == vtol_mode::FW_MODE) {
 			// Regular backtransition
-			_vtol_schedule.flight_mode = vtol_mode::TRANSITION_TO_MC;
+			_vtol_mode = vtol_mode::TRANSITION_TO_MC;
-			_vtol_schedule.transition_start = hrt_absolute_time();
+			_transition_start_timestamp = hrt_absolute_time();
 			_reverse_output = _param_vt_b_rev_out.get();
 
-		} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_FW) {
+		} else if (_vtol_mode == vtol_mode::TRANSITION_TO_FW) {
 			// failsafe back to mc mode
-			_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
+			_vtol_mode = vtol_mode::MC_MODE;
 			mc_weight = 1.0f;
 			_pusher_throttle = 0.0f;
 			_reverse_output = 0.0f;
 
-		} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_MC) {
+		} else if (_vtol_mode == vtol_mode::TRANSITION_TO_MC) {
 			// speed exit condition: use ground if valid, otherwise airspeed
 			bool exit_backtransition_speed_condition = false;
 
@@ -126,33 +118,33 @@ void Standard::update_vtol_state()
 				exit_backtransition_speed_condition = _airspeed_validated->calibrated_airspeed_m_s < _param_mpc_xy_cruise.get();
 			}
 
-			const bool exit_backtransition_time_condition = time_since_trans_start > _param_vt_b_trans_dur.get();
+			const bool exit_backtransition_time_condition = _time_since_trans_start > _param_vt_b_trans_dur.get();
 
 			if (can_transition_on_ground() || exit_backtransition_speed_condition || exit_backtransition_time_condition) {
-				_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
+				_vtol_mode = vtol_mode::MC_MODE;
 			}
 		}
 
 	} else {
 		// the transition to fw mode switch is on
-		if (_vtol_schedule.flight_mode == vtol_mode::MC_MODE || _vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_MC) {
+		if (_vtol_mode == vtol_mode::MC_MODE || _vtol_mode == vtol_mode::TRANSITION_TO_MC) {
 			// start transition to fw mode
 			/* NOTE: The failsafe transition to fixed-wing was removed because it can result in an
 			 * unsafe flying state. */
-			_vtol_schedule.flight_mode = vtol_mode::TRANSITION_TO_FW;
+			_vtol_mode = vtol_mode::TRANSITION_TO_FW;
-			_vtol_schedule.transition_start = hrt_absolute_time();
+			_transition_start_timestamp = hrt_absolute_time();
 
-		} else if (_vtol_schedule.flight_mode == vtol_mode::FW_MODE) {
+		} else if (_vtol_mode == vtol_mode::FW_MODE) {
 			// in fw mode
-			_vtol_schedule.flight_mode = vtol_mode::FW_MODE;
+			_vtol_mode = vtol_mode::FW_MODE;
 			mc_weight = 0.0f;
 
-		} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_FW) {
+		} else if (_vtol_mode == vtol_mode::TRANSITION_TO_FW) {
 			// continue the transition to fw mode while monitoring airspeed for a final switch to fw mode
 
 			const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)
 					&& !_param_fw_arsp_mode.get();
-			const bool minimum_trans_time_elapsed = time_since_trans_start > getMinimumFrontTransitionTime();
+			const bool minimum_trans_time_elapsed = _time_since_trans_start > getMinimumFrontTransitionTime();
 
 			bool transition_to_fw = false;
 
@@ -168,7 +160,7 @@ void Standard::update_vtol_state()
 			transition_to_fw |= can_transition_on_ground();
 
 			if (transition_to_fw) {
-				_vtol_schedule.flight_mode = vtol_mode::FW_MODE;
+				_vtol_mode = vtol_mode::FW_MODE;
 
 				// don't set pusher throttle here as it's being ramped up elsewhere
 				_trans_finished_ts = hrt_absolute_time();
@@ -182,21 +174,21 @@ void Standard::update_vtol_state()
 	_mc_throttle_weight = mc_weight;
 
 	// map specific control phases to simple control modes
-	switch (_vtol_schedule.flight_mode) {
+	switch (_vtol_mode) {
 	case vtol_mode::MC_MODE:
-		_vtol_mode = mode::ROTARY_WING;
+		_common_vtol_mode = mode::ROTARY_WING;
 		break;
 
 	case vtol_mode::FW_MODE:
-		_vtol_mode = mode::FIXED_WING;
+		_common_vtol_mode = mode::FIXED_WING;
 		break;
 
 	case vtol_mode::TRANSITION_TO_FW:
-		_vtol_mode = mode::TRANSITION_TO_FW;
+		_common_vtol_mode = mode::TRANSITION_TO_FW;
 		break;
 
 	case vtol_mode::TRANSITION_TO_MC:
-		_vtol_mode = mode::TRANSITION_TO_MC;
+		_common_vtol_mode = mode::TRANSITION_TO_MC;
 		break;
 	}
 }
@@ -205,7 +197,6 @@ void Standard::update_transition_state()
 {
 	const hrt_abstime now = hrt_absolute_time();
 	float mc_weight = 1.0f;
-	const float time_since_trans_start = (float)(now - _vtol_schedule.transition_start) * 1e-6f;
 
 	VtolType::update_transition_state();
 
@@ -230,7 +221,7 @@ void Standard::update_transition_state()
 		_v_att_sp->thrust_body[2] = -_fw_virtual_att_sp->thrust_body[0];
 	}
 
-	if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_FW) {
+	if (_vtol_mode == vtol_mode::TRANSITION_TO_FW) {
 		if (_param_vt_psher_slew.get() <= FLT_EPSILON) {
 			// just set the final target throttle value
 			_pusher_throttle = _param_vt_f_trans_thr.get();
@@ -248,14 +239,14 @@ void Standard::update_transition_state()
 		    PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s) &&
 		    _airspeed_validated->calibrated_airspeed_m_s > 0.0f &&
 		    _airspeed_validated->calibrated_airspeed_m_s >= _param_vt_arsp_blend.get() &&
-		    time_since_trans_start > getMinimumFrontTransitionTime()) {
+		    _time_since_trans_start > getMinimumFrontTransitionTime()) {
 
 			mc_weight = 1.0f - fabsf(_airspeed_validated->calibrated_airspeed_m_s - _param_vt_arsp_blend.get()) /
 				    _airspeed_trans_blend_margin;
 			// time based blending when no airspeed sensor is set
 
 		} else if (_param_fw_arsp_mode.get() || !PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) {
-			mc_weight = 1.0f - time_since_trans_start / getMinimumFrontTransitionTime();
+			mc_weight = 1.0f - _time_since_trans_start / getMinimumFrontTransitionTime();
 			mc_weight = math::constrain(2.0f * mc_weight, 0.0f, 1.0f);
 
 		}
@@ -266,19 +257,11 @@ void Standard::update_transition_state()
 		const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body));
 		q_sp.copyTo(_v_att_sp->q_d);
 
-		// check front transition timeout
-		if (_param_vt_trans_timeout.get() > FLT_EPSILON) {
-			if (time_since_trans_start > _param_vt_trans_timeout.get()) {
-				// transition timeout occured, abort transition
-				_attc->quadchute(VtolAttitudeControl::QuadchuteReason::TransitionTimeout);
-			}
-		}
-
 		// set spoiler and flaps to 0
 		_flaps_setpoint_with_slewrate.update(0.f, _dt);
 		_spoiler_setpoint_with_slewrate.update(0.f, _dt);
 
-	} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_MC) {
+	} else if (_vtol_mode == vtol_mode::TRANSITION_TO_MC) {
 
 		if (_v_control_mode->flag_control_climb_rate_enabled) {
 			// control backtransition deceleration using pitch.
@@ -290,15 +273,15 @@ void Standard::update_transition_state()
 
 		_pusher_throttle = 0.0f;
 
-		if (time_since_trans_start >= _param_vt_b_rev_del.get()) {
+		if (_time_since_trans_start >= _param_vt_b_rev_del.get()) {
 			// Handle throttle reversal for active breaking
-			_pusher_throttle = math::constrain((time_since_trans_start - _param_vt_b_rev_del.get())
+			_pusher_throttle = math::constrain((_time_since_trans_start - _param_vt_b_rev_del.get())
 							   * _param_vt_psher_slew.get(), 0.0f, _param_vt_b_trans_thr.get());
 		}
 
 		// continually increase mc attitude control as we transition back to mc mode
 		if (_param_vt_b_trans_ramp.get() > FLT_EPSILON) {
-			mc_weight = time_since_trans_start / _param_vt_b_trans_ramp.get();
+			mc_weight = _time_since_trans_start / _param_vt_b_trans_ramp.get();
 		}
 	}
 
@@ -334,7 +317,7 @@ void Standard::fill_actuator_outputs()
 	auto &mc_out = _actuators_out_0->control;
 	auto &fw_out = _actuators_out_1->control;
 
-	switch (_vtol_schedule.flight_mode) {
+	switch (_vtol_mode) {
 	case vtol_mode::MC_MODE:
 
 		// MC out = MC in

src/modules/vtol_att_control/standard.h

@@ -72,10 +72,7 @@ private:
 		FW_MODE
 	};
 
-	struct {
+	vtol_mode _vtol_mode{vtol_mode::MC_MODE};			/**< vtol flight mode, defined by enum vtol_mode */
-		vtol_mode flight_mode;			// indicates in which mode the vehicle is in
-		hrt_abstime transition_start;	// at what time did we start a transition (front- or backtransition)
-	} _vtol_schedule;
 
 	float _pusher_throttle{0.0f};
 	float _reverse_output{0.0f};

src/modules/vtol_att_control/tailsitter.cpp

@@ -52,8 +52,6 @@ using namespace matrix;
 Tailsitter::Tailsitter(VtolAttitudeControl *attc) :
 	VtolType(attc)
 {
-	_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
-	_vtol_schedule.transition_start = 0;
 }
 
 void
@@ -75,7 +73,7 @@ void Tailsitter::update_vtol_state()
 
 	if (_vtol_vehicle_status->vtol_transition_failsafe) {
 		// Failsafe event, switch to MC mode immediately
-		_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
+		_vtol_mode = vtol_mode::MC_MODE;
 
 		//reset failsafe when FW is no longer requested
 		if (!_attc->is_fixed_wing_requested()) {
@@ -84,26 +82,25 @@ void Tailsitter::update_vtol_state()
 
 	} else if (!_attc->is_fixed_wing_requested()) {
 
-		switch (_vtol_schedule.flight_mode) { // user switchig to MC mode
+		switch (_vtol_mode) { // user switchig to MC mode
 		case vtol_mode::MC_MODE:
 			break;
 
 		case vtol_mode::FW_MODE:
-			_vtol_schedule.flight_mode = vtol_mode::TRANSITION_BACK;
+			_vtol_mode = vtol_mode::TRANSITION_BACK;
-			_vtol_schedule.transition_start = hrt_absolute_time();
+			_transition_start_timestamp = hrt_absolute_time();
 			break;
 
 		case vtol_mode::TRANSITION_FRONT_P1:
 			// failsafe into multicopter mode
-			_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
+			_vtol_mode = vtol_mode::MC_MODE;
 			break;
 
 		case vtol_mode::TRANSITION_BACK:
-			float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
 
 			// check if we have reached pitch angle to switch to MC mode
-			if (pitch >= PITCH_TRANSITION_BACK || time_since_trans_start > _param_vt_b_trans_dur.get()) {
+			if (pitch >= PITCH_TRANSITION_BACK || _time_since_trans_start > _param_vt_b_trans_dur.get()) {
-				_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
+				_vtol_mode = vtol_mode::MC_MODE;
 			}
 
 			break;
@@ -111,11 +108,11 @@ void Tailsitter::update_vtol_state()
 
 	} else {  // user switchig to FW mode
 
-		switch (_vtol_schedule.flight_mode) {
+		switch (_vtol_mode) {
 		case vtol_mode::MC_MODE:
 			// initialise a front transition
-			_vtol_schedule.flight_mode = vtol_mode::TRANSITION_FRONT_P1;
+			_vtol_mode = vtol_mode::TRANSITION_FRONT_P1;
-			_vtol_schedule.transition_start = hrt_absolute_time();
+			_transition_start_timestamp = hrt_absolute_time();
 			break;
 
 		case vtol_mode::FW_MODE:
@@ -123,9 +120,6 @@ void Tailsitter::update_vtol_state()
 
 		case vtol_mode::TRANSITION_FRONT_P1: {
 
-				const float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
-
-
 				const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)
 						&& !_param_fw_arsp_mode.get() ;
 
@@ -143,15 +137,7 @@ void Tailsitter::update_vtol_state()
 				transition_to_fw |= can_transition_on_ground();
 
 				if (transition_to_fw) {
-					_vtol_schedule.flight_mode = vtol_mode::FW_MODE;
+					_vtol_mode = vtol_mode::FW_MODE;
-				}
-
-				// check front transition timeout
-				if (_param_vt_trans_timeout.get()  > FLT_EPSILON) {
-					if (time_since_trans_start > _param_vt_trans_timeout.get()) {
-						// transition timeout occured, abort transition
-						_attc->quadchute(VtolAttitudeControl::QuadchuteReason::TransitionTimeout);
-					}
 				}
 
 				break;
@@ -159,29 +145,29 @@ void Tailsitter::update_vtol_state()
 
 		case vtol_mode::TRANSITION_BACK:
 			// failsafe into fixed wing mode
-			_vtol_schedule.flight_mode = vtol_mode::FW_MODE;
+			_vtol_mode = vtol_mode::FW_MODE;
 			break;
 		}
 	}
 
 	// map tailsitter specific control phases to simple control modes
-	switch (_vtol_schedule.flight_mode) {
+	switch (_vtol_mode) {
 	case vtol_mode::MC_MODE:
-		_vtol_mode = mode::ROTARY_WING;
+		_common_vtol_mode = mode::ROTARY_WING;
 		_flag_was_in_trans_mode = false;
 		break;
 
 	case vtol_mode::FW_MODE:
-		_vtol_mode = mode::FIXED_WING;
+		_common_vtol_mode = mode::FIXED_WING;
 		_flag_was_in_trans_mode = false;
 		break;
 
 	case vtol_mode::TRANSITION_FRONT_P1:
-		_vtol_mode = mode::TRANSITION_TO_FW;
+		_common_vtol_mode = mode::TRANSITION_TO_FW;
 		break;
 
 	case vtol_mode::TRANSITION_BACK:
-		_vtol_mode = mode::TRANSITION_TO_MC;
+		_common_vtol_mode = mode::TRANSITION_TO_MC;
 		break;
 	}
 }
@@ -189,7 +175,6 @@ void Tailsitter::update_vtol_state()
 void Tailsitter::update_transition_state()
 {
 	const hrt_abstime now = hrt_absolute_time();
-	const float time_since_trans_start = (float)(now - _vtol_schedule.transition_start) * 1e-6f;
 
 	// we need the incoming (virtual) mc attitude setpoints to be recent, otherwise return (means the previous setpoint stays active)
 	if (_mc_virtual_att_sp->timestamp < (now - 1_s)) {
@@ -199,7 +184,7 @@ void Tailsitter::update_transition_state()
 	if (!_flag_was_in_trans_mode) {
 		_flag_was_in_trans_mode = true;
 
-		if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_BACK) {
+		if (_vtol_mode == vtol_mode::TRANSITION_BACK) {
 			// calculate rotation axis for transition.
 			_q_trans_start = Quatf(_v_att->q);
 			Vector3f z = -_q_trans_start.dcm_z();
@@ -223,7 +208,7 @@ void Tailsitter::update_transition_state()
 			// multirotor frame
 			_q_trans_start = _q_trans_start * Quatf(Eulerf(0, -M_PI_2_F, 0));
 
-		} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_FRONT_P1) {
+		} else if (_vtol_mode == vtol_mode::TRANSITION_FRONT_P1) {
 			// initial attitude setpoint for the transition should be with wings level
 			_q_trans_start = Eulerf(0.0f, _mc_virtual_att_sp->pitch_body, _mc_virtual_att_sp->yaw_body);
 			Vector3f x = Dcmf(Quatf(_v_att->q)) * Vector3f(1, 0, 0);
@@ -243,32 +228,24 @@ void Tailsitter::update_transition_state()
 	cos_tilt = cos_tilt < -1.0f ? -1.0f : cos_tilt;
 	const float tilt = acosf(cos_tilt);
 
-	if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_FRONT_P1) {
+	if (_vtol_mode == vtol_mode::TRANSITION_FRONT_P1) {
 
 		// calculate pitching rate - and constrain to at least 0.1s transition time
 		const float trans_pitch_rate = M_PI_2_F / math::max(_param_vt_f_trans_dur.get(), 0.1f);
 
 		if (tilt < M_PI_2_F - math::radians(_param_fw_psp_off.get())) {
 			_q_trans_sp = Quatf(AxisAnglef(_trans_rot_axis,
-						       time_since_trans_start * trans_pitch_rate)) * _q_trans_start;
+						       _time_since_trans_start * trans_pitch_rate)) * _q_trans_start;
 		}
 
-		// check front transition timeout
+	} else if (_vtol_mode == vtol_mode::TRANSITION_BACK) {
-		if (_param_vt_trans_timeout.get()  > FLT_EPSILON) {
-			if (time_since_trans_start > _param_vt_trans_timeout.get()) {
-				// transition timeout occured, abort transition
-				_attc->quadchute(VtolAttitudeControl::QuadchuteReason::TransitionTimeout);
-			}
-		}
-
-	} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_BACK) {
 
 		// calculate pitching rate - and constrain to at least 0.1s transition time
 		const float trans_pitch_rate = M_PI_2_F / math::max(_param_vt_b_trans_dur.get(), 0.1f);
 
 		if (tilt > 0.01f) {
 			_q_trans_sp = Quatf(AxisAnglef(_trans_rot_axis,
-						       time_since_trans_start * trans_pitch_rate)) * _q_trans_start;
+						       _time_since_trans_start * trans_pitch_rate)) * _q_trans_start;
 		}
 	}
 
@@ -336,7 +313,7 @@ void Tailsitter::fill_actuator_outputs()
 	mc_out[actuator_controls_s::INDEX_PITCH] = mc_in[actuator_controls_s::INDEX_PITCH];
 	mc_out[actuator_controls_s::INDEX_YAW]   = mc_in[actuator_controls_s::INDEX_YAW];
 
-	if (_vtol_schedule.flight_mode == vtol_mode::FW_MODE) {
+	if (_vtol_mode == vtol_mode::FW_MODE) {
 		mc_out[actuator_controls_s::INDEX_THROTTLE] = fw_in[actuator_controls_s::INDEX_THROTTLE];
 
 		// FW thrust is allocated on mc_thrust_sp[0] for tailsitter with dynamic control allocation
@@ -371,14 +348,14 @@ void Tailsitter::fill_actuator_outputs()
 	}
 
 	// Landing Gear
-	if (_vtol_schedule.flight_mode == vtol_mode::MC_MODE) {
+	if (_vtol_mode == vtol_mode::MC_MODE) {
 		mc_out[actuator_controls_s::INDEX_LANDING_GEAR] = landing_gear_s::GEAR_DOWN;
 
 	} else {
 		mc_out[actuator_controls_s::INDEX_LANDING_GEAR] = landing_gear_s::GEAR_UP;
 	}
 
-	if (_param_vt_elev_mc_lock.get() && _vtol_schedule.flight_mode == vtol_mode::MC_MODE) {
+	if (_param_vt_elev_mc_lock.get() && _vtol_mode == vtol_mode::MC_MODE) {
 		fw_out[actuator_controls_s::INDEX_ROLL]  = 0;
 		fw_out[actuator_controls_s::INDEX_PITCH] = 0;
 

src/modules/vtol_att_control/tailsitter.h

@@ -69,10 +69,7 @@ private:
 		FW_MODE					/**< vtol is in fixed wing mode */
 	};
 
-	struct {
+	vtol_mode _vtol_mode{vtol_mode::MC_MODE};			/**< vtol flight mode, defined by enum vtol_mode */
-		vtol_mode flight_mode;			/**< vtol flight mode, defined by enum vtol_mode */
-		hrt_abstime transition_start;	/**< absoulte time at which front transition started */
-	} _vtol_schedule;
 
 	bool _flag_was_in_trans_mode = false;	// true if mode has just switched to transition
 

src/modules/vtol_att_control/tiltrotor.cpp

@@ -55,12 +55,6 @@ using namespace time_literals;
 Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) :
 	VtolType(attc)
 {
-	_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
-	_vtol_schedule.transition_start = 0;
-
-	_mc_roll_weight = 1.0f;
-	_mc_pitch_weight = 1.0f;
-	_mc_yaw_weight = 1.0f;
 }
 
 void
@@ -79,7 +73,7 @@ void Tiltrotor::update_vtol_state()
 
 	if (_vtol_vehicle_status->vtol_transition_failsafe) {
 		// Failsafe event, switch to MC mode immediately
-		_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
+		_vtol_mode = vtol_mode::MC_MODE;
 
 		//reset failsafe when FW is no longer requested
 		if (!_attc->is_fixed_wing_requested()) {
@@ -89,23 +83,23 @@ void Tiltrotor::update_vtol_state()
 	} else 	if (!_attc->is_fixed_wing_requested()) {
 
 		// plane is in multicopter mode
-		switch (_vtol_schedule.flight_mode) {
+		switch (_vtol_mode) {
 		case vtol_mode::MC_MODE:
 			break;
 
 		case vtol_mode::FW_MODE:
-			_vtol_schedule.flight_mode = vtol_mode::TRANSITION_BACK;
+			_vtol_mode = vtol_mode::TRANSITION_BACK;
-			_vtol_schedule.transition_start = hrt_absolute_time();
+			_transition_start_timestamp = hrt_absolute_time();
 			break;
 
 		case vtol_mode::TRANSITION_FRONT_P1:
 			// failsafe into multicopter mode
-			_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
+			_vtol_mode = vtol_mode::MC_MODE;
 			break;
 
 		case vtol_mode::TRANSITION_FRONT_P2:
 			// failsafe into multicopter mode
-			_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
+			_vtol_mode = vtol_mode::MC_MODE;
 			break;
 
 		case vtol_mode::TRANSITION_BACK:
@@ -123,11 +117,10 @@ void Tiltrotor::update_vtol_state()
 				exit_backtransition_speed_condition = _airspeed_validated->calibrated_airspeed_m_s < _param_mpc_xy_cruise.get() ;
 			}
 
-			const float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
+			const bool exit_backtransition_time_condition = _time_since_trans_start > _param_vt_b_trans_dur.get() ;
-			const bool exit_backtransition_time_condition = time_since_trans_start > _param_vt_b_trans_dur.get() ;
 
 			if (exit_backtransition_tilt_condition && (exit_backtransition_speed_condition || exit_backtransition_time_condition)) {
-				_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
+				_vtol_mode = vtol_mode::MC_MODE;
 			}
 
 			break;
@@ -135,11 +128,11 @@ void Tiltrotor::update_vtol_state()
 
 	} else {
 
-		switch (_vtol_schedule.flight_mode) {
+		switch (_vtol_mode) {
 		case vtol_mode::MC_MODE:
 			// initialise a front transition
-			_vtol_schedule.flight_mode = vtol_mode::TRANSITION_FRONT_P1;
+			_vtol_mode = vtol_mode::TRANSITION_FRONT_P1;
-			_vtol_schedule.transition_start = hrt_absolute_time();
+			_transition_start_timestamp = hrt_absolute_time();
 			break;
 
 		case vtol_mode::FW_MODE:
@@ -147,36 +140,26 @@ void Tiltrotor::update_vtol_state()
 
 		case vtol_mode::TRANSITION_FRONT_P1: {
 
-				float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
-
 				const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)
 						&& !_param_fw_arsp_mode.get() ;
 
 				bool transition_to_p2 = false;
 
-				if (time_since_trans_start > getMinimumFrontTransitionTime()) {
+				if (_time_since_trans_start > getMinimumFrontTransitionTime()) {
 					if (airspeed_triggers_transition) {
 						transition_to_p2 = _airspeed_validated->calibrated_airspeed_m_s >= _param_vt_arsp_trans.get() ;
 
 					} else {
 						transition_to_p2 = _tilt_control >= _param_vt_tilt_trans.get() &&
-								   time_since_trans_start > getOpenLoopFrontTransitionTime();
+								   _time_since_trans_start > getOpenLoopFrontTransitionTime();
 					}
 				}
 
 				transition_to_p2 |= can_transition_on_ground();
 
 				if (transition_to_p2) {
-					_vtol_schedule.flight_mode = vtol_mode::TRANSITION_FRONT_P2;
+					_vtol_mode = vtol_mode::TRANSITION_FRONT_P2;
-					_vtol_schedule.transition_start = hrt_absolute_time();
+					_transition_start_timestamp = hrt_absolute_time();
-				}
-
-				// check front transition timeout
-				if (_param_vt_trans_timeout.get()  > FLT_EPSILON) {
-					if (time_since_trans_start > _param_vt_trans_timeout.get()) {
-						// transition timeout occured, abort transition
-						_attc->quadchute(VtolAttitudeControl::QuadchuteReason::TransitionTimeout);
-					}
 				}
 
 				break;
@@ -186,7 +169,7 @@ void Tiltrotor::update_vtol_state()
 
 			// if the rotors have been tilted completely we switch to fw mode
 			if (_tilt_control >= _param_vt_tilt_fw.get()) {
-				_vtol_schedule.flight_mode = vtol_mode::FW_MODE;
+				_vtol_mode = vtol_mode::FW_MODE;
 				_tilt_control = _param_vt_tilt_fw.get();
 			}
 
@@ -194,28 +177,28 @@ void Tiltrotor::update_vtol_state()
 
 		case vtol_mode::TRANSITION_BACK:
 			// failsafe into fixed wing mode
-			_vtol_schedule.flight_mode = vtol_mode::FW_MODE;
+			_vtol_mode = vtol_mode::FW_MODE;
 			break;
 		}
 	}
 
 	// map tiltrotor specific control phases to simple control modes
-	switch (_vtol_schedule.flight_mode) {
+	switch (_vtol_mode) {
 	case vtol_mode::MC_MODE:
-		_vtol_mode = mode::ROTARY_WING;
+		_common_vtol_mode = mode::ROTARY_WING;
 		break;
 
 	case vtol_mode::FW_MODE:
-		_vtol_mode = mode::FIXED_WING;
+		_common_vtol_mode = mode::FIXED_WING;
 		break;
 
 	case vtol_mode::TRANSITION_FRONT_P1:
 	case vtol_mode::TRANSITION_FRONT_P2:
-		_vtol_mode = mode::TRANSITION_TO_FW;
+		_common_vtol_mode = mode::TRANSITION_TO_FW;
 		break;
 
 	case vtol_mode::TRANSITION_BACK:
-		_vtol_mode = mode::TRANSITION_TO_MC;
+		_common_vtol_mode = mode::TRANSITION_TO_MC;
 		break;
 	}
 }
@@ -304,16 +287,15 @@ void Tiltrotor::update_transition_state()
 		_thrust_transition = _fw_virtual_att_sp->thrust_body[0];
 	}
 
-	const float time_since_trans_start = (float)(now - _vtol_schedule.transition_start) * 1e-6f;
 
-	if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_FRONT_P1) {
+	if (_vtol_mode == vtol_mode::TRANSITION_FRONT_P1) {
 		// for the first part of the transition all rotors are enabled
 
 		// tilt rotors forward up to certain angle
 		if (_tilt_control <= _param_vt_tilt_trans.get()) {
 			const float ramped_up_tilt = _param_vt_tilt_mc.get() +
 						     fabsf(_param_vt_tilt_trans.get() - _param_vt_tilt_mc.get()) *
-						     time_since_trans_start / _param_vt_f_trans_dur.get() ;
+						     _time_since_trans_start / _param_vt_f_trans_dur.get() ;
 
 			// only allow increasing tilt (tilt in hover can already be non-zero)
 			_tilt_control = math::max(_tilt_control, ramped_up_tilt);
@@ -333,8 +315,8 @@ void Tiltrotor::update_transition_state()
 
 		// without airspeed do timed weight changes
 		if ((_param_fw_arsp_mode.get() || !PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) &&
-		    time_since_trans_start > getMinimumFrontTransitionTime()) {
+		    _time_since_trans_start > getMinimumFrontTransitionTime()) {
-			_mc_roll_weight = 1.0f - (time_since_trans_start - getMinimumFrontTransitionTime()) /
+			_mc_roll_weight = 1.0f - (_time_since_trans_start - getMinimumFrontTransitionTime()) /
 					  (getOpenLoopFrontTransitionTime() - getMinimumFrontTransitionTime());
 			_mc_yaw_weight = _mc_roll_weight;
 		}
@@ -346,10 +328,10 @@ void Tiltrotor::update_transition_state()
 		_flaps_setpoint_with_slewrate.update(0.f, _dt);
 		_spoiler_setpoint_with_slewrate.update(0.f, _dt);
 
-	} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_FRONT_P2) {
+	} else if (_vtol_mode == vtol_mode::TRANSITION_FRONT_P2) {
 		// the plane is ready to go into fixed wing mode, tilt the rotors forward completely
 		_tilt_control = math::constrain(_param_vt_tilt_trans.get() +
-						fabsf(_param_vt_tilt_fw.get() - _param_vt_tilt_trans.get()) * time_since_trans_start /
+						fabsf(_param_vt_tilt_fw.get() - _param_vt_tilt_trans.get()) * _time_since_trans_start /
 						_param_vt_trans_p2_dur.get(), _param_vt_tilt_trans.get(), _param_vt_tilt_fw.get());
 
 		_mc_roll_weight = 0.0f;
@@ -366,12 +348,12 @@ void Tiltrotor::update_transition_state()
 		_flaps_setpoint_with_slewrate.update(0.f, _dt);
 		_spoiler_setpoint_with_slewrate.update(0.f, _dt);
 
-	} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_BACK) {
+	} else if (_vtol_mode == vtol_mode::TRANSITION_BACK) {
 
 		// tilt rotors back once motors are idle
-		if (time_since_trans_start > BACKTRANS_THROTTLE_DOWNRAMP_DUR_S) {
+		if (_time_since_trans_start > BACKTRANS_THROTTLE_DOWNRAMP_DUR_S) {
 
-			float progress = (time_since_trans_start - BACKTRANS_THROTTLE_DOWNRAMP_DUR_S) / BACKTRANS_MOTORS_UPTILT_DUR_S;
+			float progress = (_time_since_trans_start - BACKTRANS_THROTTLE_DOWNRAMP_DUR_S) / BACKTRANS_MOTORS_UPTILT_DUR_S;
 			progress = math::constrain(progress, 0.0f, 1.0f);
 			_tilt_control = moveLinear(_param_vt_tilt_fw.get(), _param_vt_tilt_mc.get(), progress);
 		}
@@ -383,14 +365,14 @@ void Tiltrotor::update_transition_state()
 			_v_att_sp->pitch_body = update_and_get_backtransition_pitch_sp();
 		}
 
-		if (time_since_trans_start < BACKTRANS_THROTTLE_DOWNRAMP_DUR_S) {
+		if (_time_since_trans_start < BACKTRANS_THROTTLE_DOWNRAMP_DUR_S) {
 			// blend throttle from FW value to 0
 			_mc_throttle_weight = 1.0f;
 			const float target_throttle = 0.0f;
-			const float progress = time_since_trans_start / BACKTRANS_THROTTLE_DOWNRAMP_DUR_S;
+			const float progress = _time_since_trans_start / BACKTRANS_THROTTLE_DOWNRAMP_DUR_S;
 			blendThrottleDuringBacktransition(progress, target_throttle);
 
-		} else if (time_since_trans_start < timeUntilMotorsAreUp()) {
+		} else if (_time_since_trans_start < timeUntilMotorsAreUp()) {
 			// while we quickly rotate back the motors keep throttle at idle
 
 			// turn on all MC motors
@@ -402,7 +384,7 @@ void Tiltrotor::update_transition_state()
 			_mc_roll_weight = 1.0f;
 			_mc_pitch_weight = 1.0f;
 			// slowly ramp up throttle to avoid step inputs
-			float progress = (time_since_trans_start - timeUntilMotorsAreUp()) / BACKTRANS_THROTTLE_UPRAMP_DUR_S;
+			float progress = (_time_since_trans_start - timeUntilMotorsAreUp()) / BACKTRANS_THROTTLE_UPRAMP_DUR_S;
 			progress = math::constrain(progress, 0.0f, 1.0f);
 			_mc_throttle_weight = moveLinear(0.0f, 1.0f, progress);
 		}
@@ -469,7 +451,7 @@ void Tiltrotor::fill_actuator_outputs()
 	_torque_setpoint_0->xyz[1] = mc_in[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;
 	_torque_setpoint_0->xyz[2] = mc_in[actuator_controls_s::INDEX_YAW]   * _mc_yaw_weight;
 
-	if (_vtol_schedule.flight_mode == vtol_mode::FW_MODE) {
+	if (_vtol_mode == vtol_mode::FW_MODE) {
 
 		// for the legacy mixing system pubish FW throttle on the MC output
 		mc_out[actuator_controls_s::INDEX_THROTTLE] = fw_in[actuator_controls_s::INDEX_THROTTLE];
@@ -494,7 +476,7 @@ void Tiltrotor::fill_actuator_outputs()
 	}
 
 	// Landing gear
-	if (_vtol_schedule.flight_mode == vtol_mode::MC_MODE) {
+	if (_vtol_mode == vtol_mode::MC_MODE) {
 		mc_out[actuator_controls_s::INDEX_LANDING_GEAR] = landing_gear_s::GEAR_DOWN;
 
 	} else {
@@ -504,7 +486,7 @@ void Tiltrotor::fill_actuator_outputs()
 	// Fixed wing output
 	fw_out[actuator_controls_s::INDEX_COLLECTIVE_TILT] = _tilt_control;
 
-	if (_param_vt_elev_mc_lock.get()  && _vtol_schedule.flight_mode == vtol_mode::MC_MODE) {
+	if (_param_vt_elev_mc_lock.get()  && _vtol_mode == vtol_mode::MC_MODE) {
 		fw_out[actuator_controls_s::INDEX_ROLL]  = 0;
 		fw_out[actuator_controls_s::INDEX_PITCH] = 0;
 		fw_out[actuator_controls_s::INDEX_YAW]   = 0;

src/modules/vtol_att_control/tiltrotor.h

@@ -75,11 +75,7 @@ private:
 	 * they need to idle otherwise they need too much time to spin up for mc mode.
 	 */
 
-
+	vtol_mode _vtol_mode{vtol_mode::MC_MODE};			/**< vtol flight mode, defined by enum vtol_mode */
-	struct {
-		vtol_mode flight_mode;			/**< vtol flight mode, defined by enum vtol_mode */
-		hrt_abstime transition_start;	/**< absoulte time at which front transition started */
-	} _vtol_schedule;
 
 	float _tilt_control{0.0f};		/**< actuator value for the tilt servo */
 

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -225,6 +225,10 @@ VtolAttitudeControl::quadchute(QuadchuteReason reason)
 			events::send(events::ID("vtol_att_ctrl_quadchute_max_roll"), events::Log::Critical,
 				     "Quadchute triggered, due to maximum roll angle exceeded");
 			break;
+
+		case QuadchuteReason::None:
+			// should never get in here
+			return;
 		}
 
 		_vtol_vehicle_status.vtol_transition_failsafe = true;

src/modules/vtol_att_control/vtol_att_control_main.h

@@ -98,16 +98,6 @@ class VtolAttitudeControl : public ModuleBase<VtolAttitudeControl>, public Modul
 {
 public:
 
-	enum class QuadchuteReason {
-		TransitionTimeout = 0,
-		ExternalCommand,
-		MinimumAltBreached,
-		LossOfAlt,
-		LargeAltError,
-		MaximumPitchExceeded,
-		MaximumRollExceeded,
-	};
-
 	VtolAttitudeControl();
 	~VtolAttitudeControl() override;
 

src/modules/vtol_att_control/vtol_type.cpp

@@ -54,7 +54,7 @@ using namespace matrix;
 VtolType::VtolType(VtolAttitudeControl *att_controller) :
 	ModuleParams(nullptr),
 	_attc(att_controller),
-	_vtol_mode(mode::ROTARY_WING)
+	_common_vtol_mode(mode::ROTARY_WING)
 {
 	_v_att = _attc->get_att();
 	_v_att_sp = _attc->get_att_sp();
@@ -171,6 +171,8 @@ void VtolType::update_transition_state()
 	_last_loop_ts = t_now;
 	_throttle_blend_start_ts = t_now;
 
+	_time_since_trans_start = (float)(t_now - _transition_start_timestamp) * 1e-6f;
+
 	check_quadchute_condition();
 }
 
@@ -210,76 +212,167 @@ bool VtolType::can_transition_on_ground()
 	return !_v_control_mode->flag_armed || _land_detected->landed;
 }
 
-void VtolType::check_quadchute_condition()
+bool VtolType::isQuadchuteEnabled()
+{
+	return _v_control_mode->flag_armed && !_land_detected->landed;
+}
+
+bool VtolType::isMinAltBreached()
+{
+	// fixed-wing minimum altitude
+	if (_param_vt_fw_min_alt.get() > FLT_EPSILON) {
+
+		if (-(_local_pos->z) < _param_vt_fw_min_alt.get()) {
+			return true;
+
+		}
+	}
+
+	return false;
+}
+
+bool VtolType::largeAltitudeLoss()
+{
+	// adaptive quadchute
+	if (_param_vt_fw_alt_err.get() > FLT_EPSILON && _v_control_mode->flag_control_altitude_enabled && _tecs_running) {
+
+		// are we dropping while requesting significant ascend?
+		if (((_tecs_status->altitude_sp - _tecs_status->altitude_filtered) > _param_vt_fw_alt_err.get()) &&
+		    (_ra_hrate < -1.0f) &&
+		    (_ra_hrate_sp > 1.0f)) {
+			return true;
+		}
+	}
+
+	return false;
+}
+
+bool VtolType::largeAltitudeError()
+{
+	// adaptive quadchute
+	if (_param_vt_fw_alt_err.get() > FLT_EPSILON && _v_control_mode->flag_control_altitude_enabled && !_tecs_running) {
+
+
+		const bool height_error = _local_pos->z_valid && ((-_local_pos_sp->z - -_local_pos->z) > _param_vt_fw_alt_err.get());
+		const bool height_rate_error = _local_pos->v_z_valid && (_local_pos->vz > 1.0f) && (_local_pos->z_deriv > 1.0f);
+
+		if (height_error && height_rate_error) {
+			return true;
+		}
+	}
+
+	return false;
+}
+
+bool VtolType::isPitchExceeded()
+{
+	// fixed-wing maximum pitch angle
+	if (_param_vt_fw_qc_p.get() > 0) {
+		Eulerf euler = Quatf(_v_att->q);
+
+		if (fabsf(euler.theta()) > fabsf(math::radians(static_cast<float>(_param_vt_fw_qc_p.get())))) {
+			return true;
+		}
+	}
+
+	return false;
+}
+
+bool VtolType::isRollExceeded()
+{
+	// fixed-wing maximum roll angle
+	if (_param_vt_fw_qc_r.get() > 0) {
+		Eulerf euler = Quatf(_v_att->q);
+
+		if (fabsf(euler.phi()) > fabsf(math::radians(static_cast<float>(_param_vt_fw_qc_r.get())))) {
+			return true;
+		}
+	}
+
+	return false;
+}
+
+bool VtolType::isFrontTransitionTimeout()
+{
+	// check front transition timeout
+	if (_param_vt_trans_timeout.get()  > FLT_EPSILON && _common_vtol_mode == mode::TRANSITION_TO_FW) {
+
+		if (_time_since_trans_start > _param_vt_trans_timeout.get()) {
+			// transition timeout occured, abort transition
+			return true;
+		}
+	}
+
+	return false;
+}
+
+QuadchuteReason VtolType::getQuadchuteReason()
+{
+	if (isMinAltBreached()) {
+		return QuadchuteReason::MinimumAltBreached;
+	}
+
+	if (largeAltitudeLoss()) {
+		return QuadchuteReason::LossOfAlt;
+	}
+
+	if (largeAltitudeError()) {
+		return QuadchuteReason::LargeAltError;
+	}
+
+	if (isPitchExceeded()) {
+		return QuadchuteReason::MaximumPitchExceeded;
+	}
+
+	if (isRollExceeded()) {
+		return QuadchuteReason::MaximumRollExceeded;
+	}
+
+	if (isFrontTransitionTimeout()) {
+		return QuadchuteReason::TransitionTimeout;
+	}
+
+	return QuadchuteReason::None;
+}
+
+void VtolType::filterTecsHeightRates()
+{
+	if (_tecs_running) {
+		// 1 second rolling average
+		_ra_hrate = (49 * _ra_hrate + _tecs_status->height_rate) / 50;
+		_ra_hrate_sp = (49 * _ra_hrate_sp + _tecs_status->height_rate_setpoint) / 50;
+
+	} else {
+		// reset the filtered height rate and heigh rate setpoint if TECS is not running
+		_ra_hrate = 0.0f;
+		_ra_hrate_sp = 0.0f;
+	}
+}
+
+void VtolType::handleSpecialExternalCommandQuadchute()
 {
 	if (_attc->get_transition_command() == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC && _attc->get_immediate_transition()
 	    && !_quadchute_command_treated) {
-		_attc->quadchute(VtolAttitudeControl::QuadchuteReason::ExternalCommand);
+		_attc->quadchute(QuadchuteReason::ExternalCommand);
 		_quadchute_command_treated = true;
 		_attc->reset_immediate_transition();
 
 	} else {
 		_quadchute_command_treated = false;
 	}
+}
 
-	if (!_tecs_running) {
+void VtolType::check_quadchute_condition()
-		// reset the filtered height rate and heigh rate setpoint if TECS is not running
+{
-		_ra_hrate = 0.0f;
-		_ra_hrate_sp = 0.0f;
-	}
 
-	if (_v_control_mode->flag_armed && !_land_detected->landed) {
+	filterTecsHeightRates();
-		Eulerf euler = Quatf(_v_att->q);
+	handleSpecialExternalCommandQuadchute();
 
-		// fixed-wing minimum altitude
+	if (isQuadchuteEnabled()) {
-		if (_param_vt_fw_min_alt.get() > FLT_EPSILON) {
+		QuadchuteReason reason = getQuadchuteReason();
 
-			if (-(_local_pos->z) < _param_vt_fw_min_alt.get()) {
+		if (reason != QuadchuteReason::None) {
-				_attc->quadchute(VtolAttitudeControl::QuadchuteReason::MinimumAltBreached);
+			_attc->quadchute(reason);
-			}
-		}
-
-		// adaptive quadchute
-		if (_param_vt_fw_alt_err.get() > FLT_EPSILON && _v_control_mode->flag_control_altitude_enabled) {
-
-			// We use tecs for tracking in FW and local_pos_sp during transitions
-			if (_tecs_running) {
-				// 1 second rolling average
-				_ra_hrate = (49 * _ra_hrate + _tecs_status->height_rate) / 50;
-				_ra_hrate_sp = (49 * _ra_hrate_sp + _tecs_status->height_rate_setpoint) / 50;
-
-				// are we dropping while requesting significant ascend?
-				if (((_tecs_status->altitude_sp - _tecs_status->altitude_filtered) > _param_vt_fw_alt_err.get()) &&
-				    (_ra_hrate < -1.0f) &&
-				    (_ra_hrate_sp > 1.0f)) {
-
-					_attc->quadchute(VtolAttitudeControl::QuadchuteReason::LossOfAlt);
-				}
-
-			} else {
-				const bool height_error = _local_pos->z_valid && ((-_local_pos_sp->z - -_local_pos->z) > _param_vt_fw_alt_err.get());
-				const bool height_rate_error = _local_pos->v_z_valid && (_local_pos->vz > 1.0f) && (_local_pos->z_deriv > 1.0f);
-
-				if (height_error && height_rate_error) {
-					_attc->quadchute(VtolAttitudeControl::QuadchuteReason::LargeAltError);
-				}
-			}
-		}
-
-		// fixed-wing maximum pitch angle
-		if (_param_vt_fw_qc_p.get() > 0) {
-
-			if (fabsf(euler.theta()) > fabsf(math::radians(static_cast<float>(_param_vt_fw_qc_p.get())))) {
-				_attc->quadchute(VtolAttitudeControl::QuadchuteReason::MaximumPitchExceeded);
-			}
-		}
-
-		// fixed-wing maximum roll angle
-		if (_param_vt_fw_qc_r.get() > 0) {
-
-			if (fabsf(euler.phi()) > fabsf(math::radians(static_cast<float>(_param_vt_fw_qc_r.get())))) {
-				_attc->quadchute(VtolAttitudeControl::QuadchuteReason::MaximumRollExceeded);
-			}
 		}
 	}
 }

src/modules/vtol_att_control/vtol_type.h

@@ -84,6 +84,17 @@ enum class VtFwDifthrEnBits : int32_t {
 	PITCH_BIT = (1 << 2),
 };
 
+enum class QuadchuteReason {
+	None = 0,
+	TransitionTimeout,
+	ExternalCommand,
+	MinimumAltBreached,
+	LossOfAlt,
+	LargeAltError,
+	MaximumPitchExceeded,
+	MaximumRollExceeded,
+};
+
 class VtolAttitudeControl;
 
 class VtolType : public ModuleParams
@@ -132,6 +143,73 @@ public:
 	 */
 	virtual void waiting_on_tecs() {}
 
+	/**
+	 *  @brief Indicates if quadchute is enabled.
+	 *
+	 * @return     true if enabled
+	 */
+	bool isQuadchuteEnabled();
+
+	/**
+	 *  @brief Evaluates quadchute conditions and returns a reson for quadchute.
+	 *
+	 * @return     QuadchuteReason, can be None
+	 */
+	QuadchuteReason getQuadchuteReason();
+
+	/**
+	 *  @brief Indicates if the vehicle is lower than VT_FW_MIN_ALT above the local origin.
+	 *
+	 * @return     true if below threshold
+	 */
+	bool isMinAltBreached();
+
+	/**
+	 *  @brief Indicates if the vehicle has an altitude error larger than VT_FW_ALT_ERR and is losing altitude quickly.
+	 * 		This only applies when TECS is running.
+	 *
+	 * @return     true if error larger than threshold
+	 */
+	bool largeAltitudeLoss();
+
+	/**
+	 *  @brief Indicates if the vehicle has an altitude error larger than VT_FW_ALT_ERR. This only applied when TECS is not running.
+	 *
+	 * @return     true if error larger than threshold
+	 */
+	bool largeAltitudeError();
+
+	/**
+	 *  @brief Indicates if the absolute value of the vehicle pitch angle exceeds the threshold defined by VT_FW_QC_P
+	 *
+	 * @return     true if exeeded
+	 */
+	bool isPitchExceeded();
+
+	/**
+	 *  @brief Indicates if the absolute value of the vehicle roll angle exceeds the threshold defined by VT_FW_QC_R
+	 *
+	 * @return     true if exeeded
+	 */
+	bool isRollExceeded();
+
+	/**
+	 *  @brief Indicates if the front transition duration has exceeded the timeout definded by VT_TRANS_TIMEOUT
+	 *
+	 * @return     true if exeeded
+	 */
+	bool isFrontTransitionTimeout();
+
+	/**
+	 *  @brief Applied a first order low pass filte to TECS height rate and heigh rate setpoint.
+	 */
+	void filterTecsHeightRates();
+
+	/**
+	 *  @brief Special handling of QuadchuteReason::ReasonExternal
+	 */
+	void handleSpecialExternalCommandQuadchute();
+
 	/**
 	 * Checks for fixed-wing failsafe condition and issues abort request if needed.
 	 */
@@ -149,7 +227,7 @@ public:
 
 	virtual void blendThrottleAfterFrontTransition(float scale) {};
 
-	mode get_mode() {return _vtol_mode;}
+	mode get_mode() {return _common_vtol_mode;}
 
 	/**
 	 * @return Minimum front transition time scaled for air density (if available) [s]
@@ -172,7 +250,7 @@ public:
 
 protected:
 	VtolAttitudeControl *_attc;
-	mode _vtol_mode;
+	mode _common_vtol_mode;
 
 	static constexpr const int num_outputs_max = 8;
 
@@ -210,6 +288,8 @@ protected:
 	float _ra_hrate_sp = 0.0f;		// rolling average on height rate setpoint for quadchute condition
 
 	hrt_abstime _trans_finished_ts = 0;
+	hrt_abstime _transition_start_timestamp{0};
+	float _time_since_trans_start{0};
 
 	bool _tecs_running = false;
 	hrt_abstime _tecs_running_ts = 0;