diff --git a/msg/vehicle_status_flags.msg b/msg/vehicle_status_flags.msg index 018082e7ae..5d32723221 100644 --- a/msg/vehicle_status_flags.msg +++ b/msg/vehicle_status_flags.msg @@ -43,7 +43,6 @@ uint16 OFFBOARD_CONTROL_SIGNAL_WEAK_MASK = 8 uint16 OFFBOARD_CONTROL_SET_BY_COMMAND_MASK = 16 uint16 OFFBOARD_CONTROL_LOSS_TIMEOUT_MASK = 32 uint16 RC_SIGNAL_FOUND_ONCE_MASK = 64 -uint16 RC_SIGNAL_LOST_CMD_MASK = 128 uint16 RC_INPUT_BLOCKED_MASK = 256 uint16 DATA_LINK_LOST_CMD_MASK = 512 uint16 VTOL_TRANSITION_FAILURE_MASK = 1024 @@ -57,7 +56,6 @@ uint16 GPS_FAILURE_MASK = 4096 # 0x0010 offboard_control_set_by_command: true if the offboard mode was set by a mavlink command and should not be overridden by RC # 0x0020 offboard_control_loss_timeout: true if offboard is lost for a certain amount of time # 0x0040 rc_signal_found_once -# 0x0080 rc_signal_lost_cmd: true if RC lost mode is commanded # 0x0100 rc_input_blocked: set if RC input should be ignored temporarily # 0x0200 data_link_lost_cmd: datalink to GCS lost mode commanded # 0x0400 vtol_transition_failure: Set to true if vtol transition failed diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 52962e9ca5..adc58215b6 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.rc_signal_lost_cmd = false; status_flags.vtol_transition_failure_cmd = false; if ((int)cmd->param2 <= 0) { @@ -977,11 +976,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 == 4) { - /* trigger rc loss mode */ - status_flags.rc_signal_lost_cmd = true; - warnx("RC loss mode commanded"); - } else if ((int)cmd->param2 == 5) { /* trigger vtol transition failure mode */ status_flags.vtol_transition_failure_cmd = true; @@ -2970,8 +2964,8 @@ Commander::run() internal_state.main_state == commander_state_s::MAIN_STATE_STAB || 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.rc_signal_lost && status_flags.gps_failure))) { + armed.force_failsafe = true; status_changed = true; static bool flight_termination_printed = false; @@ -4507,9 +4501,6 @@ void publish_status_flags(orb_advert_t &vehicle_status_flags_pub, vehicle_status if (status_flags.rc_signal_found_once) { v_flags.other_flags |= vehicle_status_flags_s::RC_SIGNAL_FOUND_ONCE_MASK; } - if (status_flags.rc_signal_lost_cmd) { - v_flags.other_flags |= vehicle_status_flags_s::RC_SIGNAL_LOST_CMD_MASK; - } if (status_flags.rc_input_blocked) { v_flags.other_flags |= vehicle_status_flags_s::RC_INPUT_BLOCKED_MASK; } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 61d80b24a7..d39b3965ad 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -584,7 +584,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_act_configured = data_link_loss_act > link_loss_actions_t::DISABLED; const bool rc_loss_act_configured = rc_loss_act > link_loss_actions_t::DISABLED; - const bool rc_lost = rc_loss_act_configured && (status->rc_signal_lost || status_flags->rc_signal_lost_cmd); + const bool rc_lost = rc_loss_act_configured && (status->rc_signal_lost); bool is_armed = (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED || status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR); @@ -675,9 +675,6 @@ bool set_nav_state(struct vehicle_status_s *status, } else if (status_flags->data_link_lost_cmd) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; - } else if (status_flags->rc_signal_lost_cmd) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER; - } else if (status_flags->vtol_transition_failure_cmd) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 63821d30b4..dd76eb8142 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -103,7 +103,6 @@ struct status_flags_s { bool offboard_control_set_by_command; // true if the offboard mode was set by a mavlink command and should not be overridden by RC bool offboard_control_loss_timeout; // true if offboard is lost for a certain amount of time bool rc_signal_found_once; - bool rc_signal_lost_cmd; // true if RC lost mode is commanded bool rc_input_blocked; // set if RC input should be ignored temporarily bool data_link_lost_cmd; // datalink to GCS lost mode commanded bool vtol_transition_failure; // Set to true if vtol transition failed