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 | |
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
-rw-r--r-- | src/modules/commander/commander.cpp | 14 | ||||
-rw-r--r-- | src/modules/commander/state_machine_helper.cpp | 19 |
2 files changed, 33 insertions, 0 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index fab80e325..0c9ce4418 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -661,6 +661,7 @@ int commander_thread_main(int argc, char *argv[]) nav_states_str[NAVIGATION_STATE_LAND] = "LAND"; nav_states_str[NAVIGATION_STATE_DESCEND] = "DESCEND"; nav_states_str[NAVIGATION_STATE_TERMINATION] = "TERMINATION"; + nav_states_str[NAVIGATION_STATE_OFFBOARD] = "OFFBOARD"; /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -932,6 +933,19 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); } + if (sp_offboard.timestamp != 0 && + sp_offboard.timestamp + OFFBOARD_TIMEOUT > hrt_absolute_time()) { + if (status.offboard_control_signal_lost) { + status.offboard_control_signal_lost = false; + status_changed = true; + } + } else { + if (!status.offboard_control_signal_lost) { + status.offboard_control_signal_lost = true; + status_changed = true; + } + } + orb_check(telemetry_sub, &updated); if (updated) { 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; } |