VTOL (standard, tiltrotor, tailsitter): check for airspeed validity a…

…nd switch to non-airspeed transition if invalid

Signed-off-by: Silvan Fuhrer <[email protected]>

src/modules/vtol_att_control/standard.cpp

@@ -181,8 +181,8 @@ void Standard::update_vtol_state()
 
 		} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_FW) {
 			// continue the transition to fw mode while monitoring airspeed for a final switch to fw mode
-			if (((_params->airspeed_disabled ||
-			      _airspeed_validated->indicated_airspeed_m_s >= _params->transition_airspeed) &&
+			if (((_params->airspeed_disabled || !PX4_ISFINITE(_airspeed_validated->equivalent_airspeed_m_s) ||
+			      _airspeed_validated->equivalent_airspeed_m_s >= _params->transition_airspeed) &&
 			     time_since_trans_start > _params->front_trans_time_min) ||
 			    can_transition_on_ground()) {
 
@@ -242,15 +242,16 @@ void Standard::update_transition_state()
 
 		// do blending of mc and fw controls if a blending airspeed has been provided and the minimum transition time has passed
 		if (_airspeed_trans_blend_margin > 0.0f &&
-		    _airspeed_validated->indicated_airspeed_m_s > 0.0f &&
-		    _airspeed_validated->indicated_airspeed_m_s >= _params->airspeed_blend &&
+		    PX4_ISFINITE(_airspeed_validated->equivalent_airspeed_m_s) &&
+		    _airspeed_validated->equivalent_airspeed_m_s > 0.0f &&
+		    _airspeed_validated->equivalent_airspeed_m_s >= _params->airspeed_blend &&
 		    time_since_trans_start > _params->front_trans_time_min) {
 
-			mc_weight = 1.0f - fabsf(_airspeed_validated->indicated_airspeed_m_s - _params->airspeed_blend) /
+			mc_weight = 1.0f - fabsf(_airspeed_validated->equivalent_airspeed_m_s - _params->airspeed_blend) /
 				    _airspeed_trans_blend_margin;
 			// time based blending when no airspeed sensor is set
 
-		} else if (_params->airspeed_disabled) {
+		} else if (_params->airspeed_disabled || !PX4_ISFINITE(_airspeed_validated->equivalent_airspeed_m_s)) {
 			mc_weight = 1.0f - time_since_trans_start / _params->front_trans_time_min;
 			mc_weight = math::constrain(2.0f * mc_weight, 0.0f, 1.0f);
 

src/modules/vtol_att_control/tailsitter.cpp

@@ -130,8 +130,10 @@ void Tailsitter::update_vtol_state()
 
 		case vtol_mode::TRANSITION_FRONT_P1: {
 
-				bool airspeed_condition_satisfied = _airspeed_validated->indicated_airspeed_m_s >= _params->transition_airspeed;
+				bool airspeed_condition_satisfied = _airspeed_validated->equivalent_airspeed_m_s >= _params->transition_airspeed;
 				airspeed_condition_satisfied |= _params->airspeed_disabled;
+				airspeed_condition_satisfied |= !PX4_ISFINITE(
+									_airspeed_validated->equivalent_airspeed_m_s); // switch to non-airspeed mode if airspeed invalid
 
 				// check if we have reached airspeed  and pitch angle to switch to TRANSITION P2 mode
 				if ((airspeed_condition_satisfied && pitch <= PITCH_TRANSITION_FRONT_P1) || can_transition_on_ground()) {

src/modules/vtol_att_control/tiltrotor.cpp

@@ -144,11 +144,13 @@ void Tiltrotor::update_vtol_state()
 
 				// check if we have reached airspeed to switch to fw mode
 				transition_to_p2 |= !_params->airspeed_disabled &&
-						    _airspeed_validated->indicated_airspeed_m_s >= _params->transition_airspeed &&
+						    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 &&
+				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;
 
@@ -244,18 +246,20 @@ void Tiltrotor::update_transition_state()
 		_mc_yaw_weight = 1.0f;
 
 		// reduce MC controls once the plane has picked up speed
-		if (!_params->airspeed_disabled && _airspeed_validated->indicated_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) {
+		if (!_params->airspeed_disabled && PX4_ISFINITE(_airspeed_validated->equivalent_airspeed_m_s) &&
+		    _airspeed_validated->equivalent_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) {
 			_mc_yaw_weight = 0.0f;
 		}
 
-		if (!_params->airspeed_disabled && _airspeed_validated->indicated_airspeed_m_s >= _params->airspeed_blend) {
-			_mc_roll_weight = 1.0f - (_airspeed_validated->indicated_airspeed_m_s - _params->airspeed_blend) /
+		if (!_params->airspeed_disabled && PX4_ISFINITE(_airspeed_validated->equivalent_airspeed_m_s) &&
+		    _airspeed_validated->equivalent_airspeed_m_s >= _params->airspeed_blend) {
+			_mc_roll_weight = 1.0f - (_airspeed_validated->equivalent_airspeed_m_s - _params->airspeed_blend) /
 					  (_params->transition_airspeed - _params->airspeed_blend);
 		}
 
 		// without airspeed do timed weight changes
-		if (_params->airspeed_disabled
-		    && time_since_trans_start > _params->front_trans_time_min) {
+		if ((_params->airspeed_disabled || !PX4_ISFINITE(_airspeed_validated->equivalent_airspeed_m_s)) &&
+		    time_since_trans_start > _params->front_trans_time_min) {
 			_mc_roll_weight = 1.0f - (time_since_trans_start - _params->front_trans_time_min) /
 					  (_params->front_trans_time_openloop - _params->front_trans_time_min);
 			_mc_yaw_weight = _mc_roll_weight;