Browse Source

Commander: remove unused arm mode command residue

release/1.12
Matthias Grob 4 years ago committed by Daniel Agar
parent
commit
c2151cb4fa
  1. 14
      src/modules/commander/Commander.cpp

14
src/modules/commander/Commander.cpp

@ -84,7 +84,7 @@ typedef enum VEHICLE_MODE_FLAG { @@ -84,7 +84,7 @@ typedef enum VEHICLE_MODE_FLAG {
VEHICLE_MODE_FLAG_STABILIZE_ENABLED = 16, /* 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. | */
VEHICLE_MODE_FLAG_HIL_ENABLED = 32, /* 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */
VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, /* 0b01000000 remote control input is enabled. | */
VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. | */
VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. Additional note: this flag is to be ignore when sent in the command MAV_CMD_DO_SET_MODE and MAV_CMD_COMPONENT_ARM_DISARM shall be used instead. The flag can still be used to report the armed state. | */
VEHICLE_MODE_FLAG_ENUM_END = 129, /* | */
} VEHICLE_MODE_FLAG;
@ -575,14 +575,8 @@ Commander::handle_command(const vehicle_command_s &cmd) @@ -575,14 +575,8 @@ Commander::handle_command(const vehicle_command_s &cmd)
uint8_t custom_main_mode = (uint8_t)cmd.param2;
uint8_t custom_sub_mode = (uint8_t)cmd.param3;
transition_result_t arming_ret = TRANSITION_NOT_CHANGED;
transition_result_t main_ret = TRANSITION_NOT_CHANGED;
// We ignore base_mode & VEHICLE_MODE_FLAG_SAFETY_ARMED because
// the command VEHICLE_CMD_COMPONENT_ARM_DISARM should be used
// instead according to the latest mavlink spec.
if (base_mode & VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED) {
/* use autopilot-specific mode */
if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
@ -690,15 +684,11 @@ Commander::handle_command(const vehicle_command_s &cmd) @@ -690,15 +684,11 @@ Commander::handle_command(const vehicle_command_s &cmd)
}
}
if ((arming_ret != TRANSITION_DENIED) && (main_ret != TRANSITION_DENIED)) {
if (main_ret != TRANSITION_DENIED) {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
if (arming_ret == TRANSITION_DENIED) {
mavlink_log_critical(&_mavlink_log_pub, "Arming command rejected");
}
}
}
break;

Loading…
Cancel
Save