From 43b665ae01639b76477de5038e0b7db1331810c6 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Wed, 12 Oct 2016 01:51:09 -0400 Subject: [PATCH] Fix gps circuit breaker logic in state machine. --- src/modules/commander/commander.cpp | 11 +++++++-- .../commander/state_machine_helper.cpp | 24 +++++++++++++++++++ src/modules/commander/state_machine_helper.h | 1 + 3 files changed, 34 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 6a4b3cee3c..634d799d56 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1314,8 +1314,7 @@ int commander_thread_main(int argc, char *argv[]) status_flags.offboard_control_signal_found_once = false; status_flags.rc_signal_found_once = false; - /* assume we don't have a valid GPS & baro on startup */ - status_flags.gps_failure = true; + /* assume we don't have a valid baro on startup */ status_flags.barometer_failure = true; status_flags.ever_had_barometer_data = false; @@ -1341,6 +1340,13 @@ int commander_thread_main(int argc, char *argv[]) status_flags.circuit_breaker_engaged_gpsfailure_check = false; get_circuit_breaker_params(); + // initialize gps failure to false if circuit breaker enabled + if (status_flags.circuit_breaker_engaged_gpsfailure_check) { + status_flags.gps_failure = false; + } else { + status_flags.gps_failure = true; + } + /* publish initial state */ status_pub = orb_advertise(ORB_ID(vehicle_status), &status); @@ -2816,6 +2822,7 @@ 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, &internal_state, + &mavlink_log_pub, (datalink_loss_enabled > 0), mission_result.finished, mission_result.stay_in_failsafe, diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index e6de118953..1fa7aea75c 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -80,6 +80,12 @@ using namespace DriverFramework; #define AVIONICS_WARN_VOLTAGE 4.9f #endif +const char *reason_no_rc = "no rc"; +const char *reason_no_gps = "no gps"; +const char *reason_no_gps_cmd = "no gps cmd"; +const char *reason_no_home = "no home"; +const char *reason_no_datalink = "no datalink"; + // This array defines the arming state transitions. The rows are the new state, and the columns // are the current state. Using new state and current state you can index into the array which // will be true for a valid transition or false for a invalid transition. In some cases even @@ -630,6 +636,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta * Check failsafe and main status and set navigation status for navigator accordingly */ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *internal_state, + orb_advert_t *mavlink_log_pub, const bool data_link_loss_enabled, const bool mission_finished, const bool stay_in_failsafe, status_flags_s *status_flags, bool landed, const bool rc_loss_enabled, const int offb_loss_act, const int offb_loss_rc_act) @@ -651,6 +658,7 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in /* require RC for all manual modes */ if (rc_loss_enabled && (status->rc_signal_lost || status_flags->rc_signal_lost_cmd) && armed) { status->failsafe = true; + mavlink_and_console_log_info(mavlink_log_pub, reason_no_rc); if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER; @@ -700,6 +708,7 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in if (rc_lost && armed) { status->failsafe = true; + mavlink_and_console_log_info(mavlink_log_pub, reason_no_rc); if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid && @@ -726,6 +735,7 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in (!status->is_rotary_wing && !status_flags->condition_global_position_valid)) && armed) { status->failsafe = true; + mavlink_and_console_log_info(mavlink_log_pub, reason_no_rc); if (status_flags->condition_local_altitude_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL; @@ -758,6 +768,7 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in } else if (status_flags->gps_failure_cmd) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; status->failsafe = true; + mavlink_and_console_log_info(mavlink_log_pub, reason_no_gps_cmd); } else if (status_flags->rc_signal_lost_cmd) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER; @@ -769,6 +780,7 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in } else if (status_flags->gps_failure) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + mavlink_and_console_log_info(mavlink_log_pub, reason_no_gps); status->failsafe = true; } else if (status->engine_failure) { @@ -784,6 +796,7 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in * check for datalink lost: this should always trigger RTGS */ } else if (data_link_loss_enabled && status->data_link_lost) { + mavlink_and_console_log_info(mavlink_log_pub, reason_no_datalink); status->failsafe = true; if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) { @@ -805,6 +818,7 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in } else if (!data_link_loss_enabled && status->rc_signal_lost && status->data_link_lost && !landed && mission_finished) { status->failsafe = true; + mavlink_and_console_log_info(mavlink_log_pub, reason_no_datalink); if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER; @@ -836,11 +850,13 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in } else if (status_flags->gps_failure) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; status->failsafe = true; + mavlink_and_console_log_info(mavlink_log_pub, reason_no_gps); /* also go into failsafe if just datalink is lost */ } else if (status->data_link_lost && data_link_loss_enabled) { status->failsafe = true; + mavlink_and_console_log_info(mavlink_log_pub, reason_no_datalink); if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; @@ -860,6 +876,7 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in } else if (status->rc_signal_lost && rc_loss_enabled && !data_link_loss_enabled) { status->failsafe = true; + mavlink_and_console_log_info(mavlink_log_pub, reason_no_rc); if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; @@ -898,10 +915,12 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in } else if (status_flags->gps_failure) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; status->failsafe = true; + mavlink_and_console_log_info(mavlink_log_pub, reason_no_gps); } else if ((!status_flags->condition_global_position_valid || !status_flags->condition_home_position_valid)) { status->failsafe = true; + mavlink_and_console_log_info(mavlink_log_pub, reason_no_home); if (status_flags->condition_local_position_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; @@ -928,6 +947,7 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in } else if (!status_flags->condition_global_position_valid) { status->failsafe = true; + mavlink_and_console_log_info(mavlink_log_pub, reason_no_gps); if (status_flags->condition_local_position_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; @@ -955,6 +975,7 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in } else if (status_flags->gps_failure || (!status_flags->condition_global_position_valid || !status_flags->condition_home_position_valid)) { status->failsafe = true; + mavlink_and_console_log_info(mavlink_log_pub, reason_no_gps); if (status_flags->condition_local_position_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; @@ -982,6 +1003,7 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in } else if (status_flags->gps_failure || (!status_flags->condition_global_position_valid || !status_flags->condition_home_position_valid)) { status->failsafe = true; + mavlink_and_console_log_info(mavlink_log_pub, reason_no_gps); if (status_flags->condition_local_altitude_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; @@ -1001,6 +1023,7 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in /* require offboard control, otherwise stay where you are */ if (status_flags->offboard_control_signal_lost && !status->rc_signal_lost) { status->failsafe = true; + mavlink_and_console_log_info(mavlink_log_pub, reason_no_rc); if (status_flags->offboard_control_loss_timeout && offb_loss_rc_act < 5 && offb_loss_rc_act >= 0) { if (offb_loss_rc_act == 3 && status_flags->condition_global_position_valid @@ -1040,6 +1063,7 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in } else if (status_flags->offboard_control_signal_lost && status->rc_signal_lost) { status->failsafe = true; + mavlink_and_console_log_info(mavlink_log_pub, reason_no_rc); if (status_flags->offboard_control_loss_timeout && offb_loss_act < 3 && offb_loss_act >= 0) { if (offb_loss_act == 2 && status_flags->condition_global_position_valid diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 44b468b6cd..2dc804b92c 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -115,6 +115,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t status_pub, struct vehicle_status_s *current_state, orb_advert_t *mavlink_log_pub); bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *internal_state, + orb_advert_t *mavlink_log_pub, const bool data_link_loss_enabled, const bool mission_finished, const bool stay_in_failsafe, status_flags_s *status_flags, bool landed, const bool rc_loss_enabled, const int offb_loss_act, const int offb_loss_rc_act);