diff options
| author | Julian Oes <julian@oes.ch> | 2014-06-21 12:24:37 +0200 |
|---|---|---|
| committer | Julian Oes <julian@oes.ch> | 2014-06-27 12:53:07 +0200 |
| commit | 5330ffe458bd32bcf0e733c93fa2e659fbaaeeb3 (patch) | |
| tree | e3ca46cff146fc0f8a3fa9b1f1993735faa6d4c3 /src/modules/commander/state_machine_helper.cpp | |
| parent | 4e08457afec89e41bb14fb09293c885f988bfd0c (diff) | |
| download | px4-firmware-5330ffe458bd32bcf0e733c93fa2e659fbaaeeb3.tar.gz px4-firmware-5330ffe458bd32bcf0e733c93fa2e659fbaaeeb3.tar.bz2 px4-firmware-5330ffe458bd32bcf0e733c93fa2e659fbaaeeb3.zip | |
commander: detect if offboard control is lost and missing offboard states
Diffstat (limited to 'src/modules/commander/state_machine_helper.cpp')
| -rw-r--r-- | src/modules/commander/state_machine_helper.cpp | 19 |
1 files changed, 19 insertions, 0 deletions
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 81b489ce3..4339a7a76 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -541,6 +541,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; } |