diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index e8b424111b..880c14bc4e 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -593,8 +593,17 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_ // failsafe: on engine failure status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; + // Orbit can only be started via vehicle_command (mavlink). Recovery from failsafe into orbit + // is not possible and therefore the internal_state needs to be adjusted. + internal_state->main_state = commander_state_s::MAIN_STATE_POSCTL; + } 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) + // failsafe: necessary position estimate lost; witching is done in check_invalid_pos_nav_state + + // Orbit can only be started via vehicle_command (mavlink). Consequently, recovery from failsafe into orbit + // is not possible and therefore the internal_state needs to be adjusted. + internal_state->main_state = commander_state_s::MAIN_STATE_POSCTL; + } 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, @@ -602,6 +611,10 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_ enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink); + // Orbit can only be started via vehicle_command (mavlink). Consequently, recovery from failsafe into orbit + // is not possible and therefore the internal_state needs to be adjusted. + internal_state->main_state = commander_state_s::MAIN_STATE_POSCTL; + } 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); @@ -609,6 +622,10 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_ set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER); + // Orbit can only be started via vehicle_command (mavlink). Consequently, recovery from failsafe into orbit + // is not possible and therefore the internal_state needs to be adjusted. + internal_state->main_state = commander_state_s::MAIN_STATE_POSCTL; + } else { // no failsafe, RC is not mandatory for orbit status->nav_state = vehicle_status_s::NAVIGATION_STATE_ORBIT;