diff options
Diffstat (limited to 'src/modules/commander/state_machine_helper.cpp')
| -rw-r--r-- | src/modules/commander/state_machine_helper.cpp | 28 |
1 files changed, 28 insertions, 0 deletions
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 7fb2e08db..6b96e3a3f 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -286,6 +286,15 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta } break; + case MAIN_STATE_OFFBOARD: + + /* need offboard signal */ + if (!status->offboard_control_signal_lost) { + ret = TRANSITION_CHANGED; + } + + break; + case MAIN_STATE_MAX: default: break; @@ -584,6 +593,25 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en } break; + case MAIN_STATE_OFFBOARD: + /* require offboard control, otherwise stay where you are */ + if (status->offboard_control_signal_lost && !status->rc_signal_lost) { + status->failsafe = true; + + status->nav_state = NAVIGATION_STATE_POSCTL; + } else if (status->offboard_control_signal_lost && status->rc_signal_lost) { + status->failsafe = true; + + if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = NAVIGATION_STATE_TERMINATION; + } + } else { + status->nav_state = NAVIGATION_STATE_OFFBOARD; + } default: break; } |