tiltrotor.cpp:cleanup transition logic

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

src/modules/vtol_att_control/tiltrotor.cpp

@@ -137,22 +137,25 @@ void Tiltrotor::update_vtol_state()
 			break;
 
 		case vtol_mode::TRANSITION_FRONT_P1: {
-				// allow switch if we are not armed for the sake of bench testing
-				bool transition_to_p2 = can_transition_on_ground();
 
 				float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
 
-				// check if we have reached airspeed to switch to fw mode
-				transition_to_p2 |= !_params->airspeed_disabled &&
-						    PX4_ISFINITE(_airspeed_validated->equivalent_airspeed_m_s) && // only consider airspeed if valid (not NAN)
-						    _airspeed_validated->equivalent_airspeed_m_s >= _params->transition_airspeed &&
-						    time_since_trans_start > _params->front_trans_time_min;
-
-				// check if airspeed is invalid and transition by time
-				transition_to_p2 |= (_params->airspeed_disabled ||
-						     !PX4_ISFINITE(_airspeed_validated->equivalent_airspeed_m_s)) && // do openloop if either airspeed disabled or invalid
-						    _tilt_control >= _params_tiltrotor.tilt_transition &&
-						    time_since_trans_start > _params->front_trans_time_openloop;
+				const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->equivalent_airspeed_m_s)
+						&& !_params->airspeed_disabled;
+
+				bool transition_to_p2 = false;
+
+				if (time_since_trans_start > _params->front_trans_time_min) {
+					if (airspeed_triggers_transition) {
+						transition_to_p2 = _airspeed_validated->equivalent_airspeed_m_s >= _params->transition_airspeed;
+
+					} else {
+						transition_to_p2 = _tilt_control >= _params_tiltrotor.tilt_transition &&
+								   time_since_trans_start > _params->front_trans_time_openloop;;
+					}
+				}
+
+				transition_to_p2 |= can_transition_on_ground();
 
 				if (transition_to_p2) {
 					_vtol_schedule.flight_mode = vtol_mode::TRANSITION_FRONT_P2;