RTL on RC failsafe

1 files changed, 8 insertions, 0 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index dbfca38b1..cedcbb3fb 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1133,6 +1133,14 @@ int commander_thread_main(int argc, char *argv[])
 					mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST");
 					status.rc_signal_lost = true;
 					status_changed = true;
+					if (status.main_state != MAIN_STATE_AUTO) {
+						transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO);
+						if (res == TRANSITION_CHANGED) {
+							mavlink_log_critical(mavlink_fd, "#audio: switching to RTL mode");
+							status.set_nav_state = NAV_STATE_RTL;
+							status.set_nav_state_timestamp = hrt_absolute_time();
+						}
+					}
 				}
 			}
 		}