vtol_att_control: Consolidate logic for front transiton completion (#21107)

* vtol_att_control: consolidate logic for front transiton completion

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

* addressed review comments

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

---------

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

src/modules/vtol_att_control/standard.cpp

@@ -135,26 +135,8 @@ void Standard::update_vtol_state()
 			mc_weight = 0.0f;
 
 		} 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)
+			if (isFrontTransitionCompleted()) {
-					&& !_param_fw_arsp_mode.get();
-			const bool minimum_trans_time_elapsed = _time_since_trans_start > getMinimumFrontTransitionTime();
-
-			bool transition_to_fw = false;
-
-			if (minimum_trans_time_elapsed) {
-				if (airspeed_triggers_transition) {
-					transition_to_fw = _airspeed_validated->calibrated_airspeed_m_s >= _param_vt_arsp_trans.get();
-
-				} else {
-					transition_to_fw = true;
-				}
-			}
-
-			transition_to_fw |= can_transition_on_ground();
-
-			if (transition_to_fw) {
 				_vtol_mode = vtol_mode::FW_MODE;
 
 				// don't set pusher throttle here as it's being ramped up elsewhere

src/modules/vtol_att_control/tailsitter.cpp

@@ -69,7 +69,6 @@ void Tailsitter::update_vtol_state()
 	 * For the backtransition the pitch is controlled in MC mode again and switches to full MC control reaching the sufficient pitch angle.
 	*/
 
-	float pitch = Eulerf(Quatf(_v_att->q)).theta();
 
 	if (_vtol_vehicle_status->fixed_wing_system_failure) {
 		// Failsafe event, switch to MC mode immediately
@@ -92,6 +91,7 @@ void Tailsitter::update_vtol_state()
 			break;
 
 		case vtol_mode::TRANSITION_BACK:
+			const float pitch = Eulerf(Quatf(_v_att->q)).theta();
 
 			// 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()) {
@@ -115,23 +115,7 @@ void Tailsitter::update_vtol_state()
 
 		case vtol_mode::TRANSITION_FRONT_P1: {
 
-				const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)
+				if (isFrontTransitionCompleted()) {
-						&& !_param_fw_arsp_mode.get() ;
-
-				bool transition_to_fw = false;
-
-				if (pitch <= PITCH_TRANSITION_FRONT_P1) {
-					if (airspeed_triggers_transition) {
-						transition_to_fw = _airspeed_validated->calibrated_airspeed_m_s >= _param_vt_arsp_trans.get() ;
-
-					} else {
-						transition_to_fw = true;
-					}
-				}
-
-				transition_to_fw |= can_transition_on_ground();
-
-				if (transition_to_fw) {
 					_vtol_mode = vtol_mode::FW_MODE;
 				}
 
@@ -370,3 +354,24 @@ void Tailsitter::fill_actuator_outputs()
 
 	_actuators_out_0->timestamp = _actuators_out_1->timestamp = hrt_absolute_time();
 }
+
+
+bool Tailsitter::isFrontTransitionCompletedBase()
+{
+	const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)
+			&& !_param_fw_arsp_mode.get() ;
+
+	bool transition_to_fw = false;
+	const float pitch = Eulerf(Quatf(_v_att->q)).theta();
+
+	if (pitch <= PITCH_TRANSITION_FRONT_P1) {
+		if (airspeed_triggers_transition) {
+			transition_to_fw = _airspeed_validated->calibrated_airspeed_m_s >= _param_vt_arsp_trans.get() ;
+
+		} else {
+			transition_to_fw = true;
+		}
+	}
+
+	return transition_to_fw;
+}

src/modules/vtol_att_control/tailsitter.h

@@ -79,6 +79,8 @@ private:
 
 	void parameters_update() override;
 
+	bool isFrontTransitionCompletedBase() override;
+
 	DEFINE_PARAMETERS_CUSTOM_PARENT(VtolType,
 					(ParamFloat<px4::params::FW_PSP_OFF>) _param_fw_psp_off
 				       )

src/modules/vtol_att_control/tiltrotor.cpp

@@ -134,25 +134,7 @@ void Tiltrotor::update_vtol_state()
 			break;
 
 		case vtol_mode::TRANSITION_FRONT_P1: {
-
+				if (isFrontTransitionCompleted()) {
-				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 (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();
-					}
-				}
-
-				transition_to_p2 |= can_transition_on_ground();
-
-				if (transition_to_p2) {
 					_vtol_mode = vtol_mode::TRANSITION_FRONT_P2;
 					resetTransitionStates();
 				}
@@ -526,3 +508,8 @@ float Tiltrotor::moveLinear(float start, float stop, float progress)
 {
 	return start + progress * (stop - start);
 }
+
+bool Tiltrotor::isFrontTransitionCompletedBase()
+{
+	return VtolType::isFrontTransitionCompletedBase() && _tilt_control >= _param_vt_tilt_trans.get();
+}

src/modules/vtol_att_control/tiltrotor.h

@@ -84,7 +84,7 @@ private:
 	float moveLinear(float start, float stop, float progress);
 
 	void blendThrottleDuringBacktransition(const float scale, const float target_throttle);
-
+	bool isFrontTransitionCompletedBase() override;
 
 	hrt_abstime _last_timestamp_disarmed{0}; /**< used for calculating time since arming */
 	bool _tilt_motors_for_startup{false};

src/modules/vtol_att_control/vtol_type.cpp

@@ -207,6 +207,35 @@ float VtolType::update_and_get_backtransition_pitch_sp()
 	return math::constrain(pitch_sp_new, 0.f, pitch_lim);
 }
 
+bool VtolType::isFrontTransitionCompleted()
+{
+	bool ret = isFrontTransitionCompletedBase();
+
+	return ret || can_transition_on_ground();
+}
+
+bool VtolType::isFrontTransitionCompletedBase()
+{
+	// 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 openloop_trans_time_elapsed = _time_since_trans_start > getOpenLoopFrontTransitionTime();
+
+	bool transition_to_fw = false;
+
+	if (airspeed_triggers_transition) {
+		transition_to_fw = minimum_trans_time_elapsed
+				   && _airspeed_validated->calibrated_airspeed_m_s >= _param_vt_arsp_trans.get();
+
+	} else {
+		transition_to_fw = openloop_trans_time_elapsed;
+	}
+
+	return transition_to_fw;
+
+}
+
 bool VtolType::can_transition_on_ground()
 {
 	return !_v_control_mode->flag_armed || _land_detected->landed;

src/modules/vtol_att_control/vtol_type.h

@@ -303,6 +303,8 @@ protected:
 	bool _quadchute_command_treated{false};
 
 	float update_and_get_backtransition_pitch_sp();
+	bool isFrontTransitionCompleted();
+	virtual bool isFrontTransitionCompletedBase();
 
 	SlewRate<float> _spoiler_setpoint_with_slewrate;
 	SlewRate<float> _flaps_setpoint_with_slewrate;