diff --git a/msg/vehicle_status_flags.msg b/msg/vehicle_status_flags.msg index ac92a29eb0..018082e7ae 100644 --- a/msg/vehicle_status_flags.msg +++ b/msg/vehicle_status_flags.msg @@ -49,7 +49,6 @@ uint16 DATA_LINK_LOST_CMD_MASK = 512 uint16 VTOL_TRANSITION_FAILURE_MASK = 1024 uint16 VTOL_TRANSITION_FAILURE_CMD_MASK = 2048 uint16 GPS_FAILURE_MASK = 4096 -uint16 GPS_FAILURE_CMD_MASK = 8192 # 0x0001 usb_connected: status of the USB power supply # 0x0002 offboard_control_signal_found_once @@ -64,6 +63,5 @@ uint16 GPS_FAILURE_CMD_MASK = 8192 # 0x0400 vtol_transition_failure: Set to true if vtol transition failed # 0x0800 vtol_transition_failure_cmd: Set to true if vtol transition failure mode is commanded # 0x1000 gps_failure: Set to true if a gps failure is detected -# 0x2000 gps_failure_cmd: Set to true if a gps failure mode is commanded uint16 other_flags diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index acb82ce03d..52962e9ca5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -960,7 +960,6 @@ Commander::handle_command(vehicle_status_s *status_local, const safety_s *safety /* param2 is currently used for other failsafe modes */ status_local->engine_failure_cmd = false; status_flags.data_link_lost_cmd = false; - status_flags.gps_failure_cmd = false; status_flags.rc_signal_lost_cmd = false; status_flags.vtol_transition_failure_cmd = false; @@ -978,11 +977,6 @@ Commander::handle_command(vehicle_status_s *status_local, const safety_s *safety status_flags.data_link_lost_cmd = true; warnx("data link loss mode commanded"); - } else if ((int)cmd->param2 == 3) { - /* trigger gps loss mode */ - status_flags.gps_failure_cmd = true; - warnx("GPS loss mode commanded"); - } else if ((int)cmd->param2 == 4) { /* trigger rc loss mode */ status_flags.rc_signal_lost_cmd = true; @@ -2952,7 +2946,7 @@ Commander::run() internal_state.main_state != commander_state_s::MAIN_STATE_ALTCTL && internal_state.main_state != commander_state_s::MAIN_STATE_POSCTL && ((status.data_link_lost && status_flags.gps_failure) || - (status_flags.data_link_lost_cmd && status_flags.gps_failure_cmd))) { + (status_flags.data_link_lost_cmd))) { armed.force_failsafe = true; status_changed = true; static bool flight_termination_printed = false; @@ -2977,7 +2971,7 @@ Commander::run() internal_state.main_state == commander_state_s::MAIN_STATE_ALTCTL || internal_state.main_state == commander_state_s::MAIN_STATE_POSCTL) && ((status.rc_signal_lost && status_flags.gps_failure) || - (status_flags.rc_signal_lost_cmd && status_flags.gps_failure_cmd))) { + (status_flags.rc_signal_lost_cmd))) { armed.force_failsafe = true; status_changed = true; static bool flight_termination_printed = false; @@ -4531,9 +4525,6 @@ void publish_status_flags(orb_advert_t &vehicle_status_flags_pub, vehicle_status if (status_flags.gps_failure) { v_flags.other_flags |= vehicle_status_flags_s::GPS_FAILURE_MASK; } - if (status_flags.gps_failure_cmd) { - v_flags.other_flags |= vehicle_status_flags_s::GPS_FAILURE_CMD_MASK; - } if ((v_flags.conditions != vehicle_status_flags.conditions) || (v_flags.other_flags != vehicle_status_flags.other_flags) || diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index e42f195c9c..61d80b24a7 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -681,7 +681,7 @@ bool set_nav_state(struct vehicle_status_s *status, } else if (status_flags->vtol_transition_failure_cmd) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; - } else if (status_flags->gps_failure || status_flags->gps_failure_cmd) { + } else if (status_flags->gps_failure) { if (status->is_rotary_wing) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 54eb740d3a..63821d30b4 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -109,7 +109,6 @@ struct status_flags_s { bool vtol_transition_failure; // Set to true if vtol transition failed bool vtol_transition_failure_cmd; // Set to true if vtol transition failure mode is commanded bool gps_failure; // Set to true if a gps failure is detected - bool gps_failure_cmd; // Set to true if a gps failure mode is commanded }; bool is_safe(const struct safety_s *safety, const struct actuator_armed_s *armed);