diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-07-23 22:58:19 +0200 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-07-23 22:58:19 +0200 |
commit | 24f380137ecb91fb9647e22e1d29c13da5fc0357 (patch) | |
tree | 8202bd0cad2410fd04d550897d9c994bd6d0bc3d /src/modules/commander | |
parent | ed19faf4289eed5eeb3339e9609e976b9195020c (diff) | |
download | px4-firmware-24f380137ecb91fb9647e22e1d29c13da5fc0357.tar.gz px4-firmware-24f380137ecb91fb9647e22e1d29c13da5fc0357.tar.bz2 px4-firmware-24f380137ecb91fb9647e22e1d29c13da5fc0357.zip |
add method to block fallback to mission
failsafe navigation modes can use a flag in mission_result to tell the
commander to not switch back to mission
Diffstat (limited to 'src/modules/commander')
-rw-r--r-- | src/modules/commander/commander.cpp | 5 | ||||
-rw-r--r-- | src/modules/commander/state_machine_helper.cpp | 3 | ||||
-rw-r--r-- | src/modules/commander/state_machine_helper.h | 2 |
3 files changed, 6 insertions, 4 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5c89e0123..cb09a68e3 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1461,7 +1461,7 @@ int commander_thread_main(int argc, char *argv[]) for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { if (telemetry_last_heartbeat[i] != 0 && hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout) { /* handle the case where data link was regained */ - if (telemetry_lost[i]) { + if (telemetry_lost[i]) {//XXX also add hysteresis here mavlink_log_critical(mavlink_fd, "data link %i regained", i); telemetry_lost[i] = false; } @@ -1545,7 +1545,8 @@ int commander_thread_main(int argc, char *argv[]) /* now set navigation state according to failsafe and main state */ bool nav_state_changed = set_nav_state(&status, (bool)datalink_loss_enabled, - mission_result.finished); + mission_result.finished, + mission_result.stay_in_failsafe); // TODO handle mode changes by commands if (main_state_changed) { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 7b26e3e8c..4e1cfb987 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -435,7 +435,8 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, /** * Check failsafe and main status and set navigation status for navigator accordingly */ -bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished) +bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, + const bool stay_in_failsafe) { navigation_state_t nav_state_old = status->nav_state; diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index bb1b87e71..4285d2977 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -63,7 +63,7 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd); -bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished); +bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, const bool stay_in_failsafe); int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd); |