commander: fix orbit failsafes for data link & rc loss

src/modules/commander/state_machine_helper.cpp

@@ -588,16 +588,31 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
 		break;
 
 	case commander_state_s::MAIN_STATE_ORBIT:
-		/* require local position */
 		if (status->engine_failure) {
+			// failsafe: on engine failure
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
 
-		} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) {
-			// nothing to do - everything done in check_invalid_pos_nav_state
+		} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
+			// failsafe: necessary position estimate lost (nothing to do - everything done in check_invalid_pos_nav_state)
+		} else if (status->data_link_lost && data_link_loss_act_configured && !landed && is_armed) {
+			// failsafe: just datalink is lost and we're in air
+			set_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act,
+						vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS);
+
+			enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
+
+		} else if (rc_lost && !data_link_loss_act_configured && is_armed) {
+			// failsafe: RC is lost, datalink loss is not set up and rc loss is not disabled
+			enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc);
+
+			set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act,
+						vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER);
 
 		} else {
+			// no failsafe, RC is not mandatory for orbit
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_ORBIT;
 		}
+
 		break;
 
 	case commander_state_s::MAIN_STATE_AUTO_TAKEOFF: