forked from Archive/PX4-Autopilot
commander delete unused data_link_lost_cmd
This commit is contained in:
parent
c0be801b5c
commit
d0fba8bf8b
|
@ -44,7 +44,6 @@ uint16 OFFBOARD_CONTROL_SET_BY_COMMAND_MASK = 16
|
|||
uint16 OFFBOARD_CONTROL_LOSS_TIMEOUT_MASK = 32
|
||||
uint16 RC_SIGNAL_FOUND_ONCE_MASK = 64
|
||||
uint16 RC_INPUT_BLOCKED_MASK = 256
|
||||
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
|
||||
|
@ -57,7 +56,6 @@ uint16 GPS_FAILURE_MASK = 4096
|
|||
# 0x0020 offboard_control_loss_timeout: true if offboard is lost for a certain amount of time
|
||||
# 0x0040 rc_signal_found_once
|
||||
# 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
|
||||
# 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
|
||||
|
|
|
@ -959,7 +959,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.vtol_transition_failure_cmd = false;
|
||||
|
||||
if ((int)cmd->param2 <= 0) {
|
||||
|
@ -971,11 +970,6 @@ Commander::handle_command(vehicle_status_s *status_local, const safety_s *safety
|
|||
status_local->engine_failure_cmd = true;
|
||||
warnx("engine failure mode commanded");
|
||||
|
||||
} else if ((int)cmd->param2 == 2) {
|
||||
/* trigger data link loss mode */
|
||||
status_flags.data_link_lost_cmd = true;
|
||||
warnx("data link loss mode commanded");
|
||||
|
||||
} else if ((int)cmd->param2 == 5) {
|
||||
/* trigger vtol transition failure mode */
|
||||
status_flags.vtol_transition_failure_cmd = true;
|
||||
|
@ -2939,8 +2933,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.data_link_lost && status_flags.gps_failure) ||
|
||||
(status_flags.data_link_lost_cmd))) {
|
||||
((status.data_link_lost && status_flags.gps_failure))) {
|
||||
|
||||
armed.force_failsafe = true;
|
||||
status_changed = true;
|
||||
static bool flight_termination_printed = false;
|
||||
|
@ -4504,9 +4498,6 @@ void publish_status_flags(orb_advert_t &vehicle_status_flags_pub, vehicle_status
|
|||
if (status_flags.rc_input_blocked) {
|
||||
v_flags.other_flags |= vehicle_status_flags_s::RC_INPUT_BLOCKED_MASK;
|
||||
}
|
||||
if (status_flags.data_link_lost_cmd) {
|
||||
v_flags.other_flags |= vehicle_status_flags_s::DATA_LINK_LOST_CMD_MASK;
|
||||
}
|
||||
if (status_flags.vtol_transition_failure) {
|
||||
v_flags.other_flags |= vehicle_status_flags_s::VTOL_TRANSITION_FAILURE_MASK;
|
||||
}
|
||||
|
|
|
@ -672,9 +672,6 @@ bool set_nav_state(struct vehicle_status_s *status,
|
|||
if (status->engine_failure_cmd) {
|
||||
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
|
||||
|
||||
} else if (status_flags->data_link_lost_cmd) {
|
||||
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
|
||||
|
||||
} else if (status_flags->vtol_transition_failure_cmd) {
|
||||
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
|
||||
|
||||
|
|
|
@ -104,7 +104,6 @@ struct status_flags_s {
|
|||
bool offboard_control_loss_timeout; // true if offboard is lost for a certain amount of time
|
||||
bool rc_signal_found_once;
|
||||
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
|
||||
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
|
||||
|
|
Loading…
Reference in New Issue