diff --git a/msg/vehicle_command.msg b/msg/vehicle_command.msg index c014b6f5a5..f69bacf5c0 100644 --- a/msg/vehicle_command.msg +++ b/msg/vehicle_command.msg @@ -150,6 +150,16 @@ uint8 FAILURE_TYPE_SLOW = 5 uint8 FAILURE_TYPE_DELAYED = 6 uint8 FAILURE_TYPE_INTERMITTENT = 7 +# used as param1 in ARM_DISARM command +int8 ARM_DISARM_ARMING_ACTION_TOGGLE = -1 +int8 ARM_DISARM_ARMING_ACTION_DISARM = 0 +int8 ARM_DISARM_ARMING_ACTION_ARM = 1 + +# used as param3 in ARM_DISARM command +int8 ARM_DISARM_ARMING_ORIGIN_GESTURE = 1 +int8 ARM_DISARM_ARMING_ORIGIN_SWITCH = 2 +int8 ARM_DISARM_ARMING_ORIGIN_BUTTON = 3 + uint8 ORB_QUEUE_LENGTH = 8 float32 param1 # Parameter 1, as defined by MAVLink uint16 VEHICLE_CMD enum. diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 20de3124cd..eeb821e56a 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -274,7 +274,9 @@ int Commander::custom_command(int argc, char *argv[]) param2 = 21196.f; } - send_vehicle_command(vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.f, param2); + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM, + static_cast(vehicle_command_s::ARM_DISARM_ARMING_ACTION_ARM), + param2); return 0; } @@ -287,7 +289,9 @@ int Commander::custom_command(int argc, char *argv[]) param2 = 21196.f; } - send_vehicle_command(vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.f, param2); + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM, + static_cast(vehicle_command_s::ARM_DISARM_ARMING_ACTION_DISARM), + param2); return 0; } @@ -295,7 +299,9 @@ int Commander::custom_command(int argc, char *argv[]) if (!strcmp(argv[0], "takeoff")) { // switch to takeoff mode and arm send_vehicle_command(vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF); - send_vehicle_command(vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.f, 0.f); + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM, + static_cast(vehicle_command_s::ARM_DISARM_ARMING_ACTION_ARM), + 0.f); return 0; } @@ -844,27 +850,31 @@ Commander::handle_command(const vehicle_command_s &cmd) // Adhere to MAVLink specs, but base on knowledge that these fundamentally encode ints // for logic state parameters - const int param1_arm = static_cast(roundf(cmd.param1)); + const int8_t arming_action = static_cast(lroundf(cmd.param1)); - if (param1_arm != 0 && param1_arm != 1 && param1_arm != -1) { + if (arming_action != vehicle_command_s::ARM_DISARM_ARMING_ACTION_ARM + && arming_action != vehicle_command_s::ARM_DISARM_ARMING_ACTION_DISARM + && arming_action != vehicle_command_s::ARM_DISARM_ARMING_ACTION_TOGGLE) { mavlink_log_critical(&_mavlink_log_pub, "Unsupported ARM_DISARM param: %.3f\t", (double)cmd.param1); events::send(events::ID("commander_unsupported_arm_disarm_param"), events::Log::Error, "Unsupported ARM_DISARM param: {1:.3}", cmd.param1); } else { // Arm is forced (checks skipped) when param2 is set to a magic number. - const bool forced = (static_cast(roundf(cmd.param2)) == 21196); + const bool forced = (static_cast(lroundf(cmd.param2)) == 21196); - const bool cmd_from_manual_stick = (static_cast(roundf(cmd.param3)) == 1); - const bool cmd_from_manual_switch = (static_cast(roundf(cmd.param3)) == 2); - const bool cmd_from_manual_button = (static_cast(roundf(cmd.param3)) == 3); + const int8_t arming_origin = static_cast(lroundf(cmd.param3)); + + const bool cmd_from_manual_stick = (arming_origin == vehicle_command_s::ARM_DISARM_ARMING_ORIGIN_GESTURE); + const bool cmd_from_manual_switch = (arming_origin == vehicle_command_s::ARM_DISARM_ARMING_ORIGIN_SWITCH); + const bool cmd_from_manual_button = (arming_origin == vehicle_command_s::ARM_DISARM_ARMING_ORIGIN_BUTTON); const bool cmd_from_io = (static_cast(roundf(cmd.param3)) == 1234); if (cmd_from_manual_stick && !_vehicle_control_mode.flag_control_manual_enabled) { - if (param1_arm == 1) { + if (arming_action == vehicle_command_s::ARM_DISARM_ARMING_ACTION_ARM) { mavlink_log_critical(&_mavlink_log_pub, "Not arming! Switch to a manual mode first"); - } else if (param1_arm == 0) { + } else if (arming_action == vehicle_command_s::ARM_DISARM_ARMING_ACTION_DISARM) { mavlink_log_critical(&_mavlink_log_pub, "Not disarming! Switch to a manual mode first"); } @@ -874,7 +884,7 @@ Commander::handle_command(const vehicle_command_s &cmd) if (!forced) { if (!(_land_detector.landed || _land_detector.maybe_landed) && !is_ground_rover(_status)) { - if (param1_arm == 1) { + if (arming_action == vehicle_command_s::ARM_DISARM_ARMING_ACTION_ARM) { if (_armed.armed) { mavlink_log_warning(&_mavlink_log_pub, "Arming denied! Already armed\t"); events::send(events::ID("commander_arming_denied_already_armed"), @@ -901,14 +911,14 @@ Commander::handle_command(const vehicle_command_s &cmd) // Flick to in-air restore first if this comes from an onboard system and from IO if (cmd.source_system == _status.system_id && cmd.source_component == _status.component_id - && cmd_from_io && (param1_arm == 1)) { + && cmd_from_io && (arming_action == vehicle_command_s::ARM_DISARM_ARMING_ACTION_ARM)) { _status.arming_state = vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE; } } transition_result_t arming_res = TRANSITION_DENIED; - if (param1_arm == 1) { + if (arming_action == vehicle_command_s::ARM_DISARM_ARMING_ACTION_ARM) { if (cmd.from_external) { arming_res = arm(arm_disarm_reason_t::command_external); @@ -924,7 +934,7 @@ Commander::handle_command(const vehicle_command_s &cmd) } } - } else if (param1_arm == 0) { + } else if (arming_action == vehicle_command_s::ARM_DISARM_ARMING_ACTION_DISARM) { if (cmd.from_external) { arming_res = disarm(arm_disarm_reason_t::command_external); @@ -940,7 +950,7 @@ Commander::handle_command(const vehicle_command_s &cmd) } } - } else if (param1_arm == -1) { + } else if (arming_action == vehicle_command_s::ARM_DISARM_ARMING_ACTION_TOGGLE) { // -1 means toggle by a button // This should come from an arming button internally, otherwise something is wrong. if (!cmd.from_external && cmd_from_manual_button) { @@ -964,7 +974,7 @@ Commander::handle_command(const vehicle_command_s &cmd) cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; /* update home position on arming if at least 500 ms from commander start spent to avoid setting home on in-air restart */ - if ((param1_arm == 1) && (arming_res == TRANSITION_CHANGED) && + if ((arming_action == vehicle_command_s::ARM_DISARM_ARMING_ACTION_ARM) && (arming_res == TRANSITION_CHANGED) && (hrt_absolute_time() > (_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) && !_home_pub.get().manual_home) { set_home_position(); diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp index fff52444f0..57ad60f032 100644 --- a/src/modules/manual_control/ManualControl.cpp +++ b/src/modules/manual_control/ManualControl.cpp @@ -131,7 +131,8 @@ void ManualControl::Run() if (_selector.setpoint().arm_gesture && !_previous_arm_gesture) { _previous_arm_gesture = true; - send_arm_command(ArmingAction::ARM, ArmingOrigin::GESTURE); + send_arm_command(vehicle_command_s::ARM_DISARM_ARMING_ACTION_ARM, + vehicle_command_s::ARM_DISARM_ARMING_ORIGIN_GESTURE); } @@ -141,7 +142,8 @@ void ManualControl::Run() if (_selector.setpoint().disarm_gesture && !_previous_disarm_gesture) { _previous_disarm_gesture = true; - send_arm_command(ArmingAction::DISARM, ArmingOrigin::GESTURE); + send_arm_command(vehicle_command_s::ARM_DISARM_ARMING_ACTION_DISARM, + vehicle_command_s::ARM_DISARM_ARMING_ORIGIN_GESTURE); } @@ -180,16 +182,19 @@ void ManualControl::Run() _button_hysteresis.set_state_and_update(switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON, now); if (_button_hysteresis.get_state()) { - send_arm_command(ArmingAction::TOGGLE, ArmingOrigin::BUTTON); + send_arm_command(vehicle_command_s::ARM_DISARM_ARMING_ACTION_TOGGLE, + vehicle_command_s::ARM_DISARM_ARMING_ORIGIN_BUTTON); } } else { // Arming switch if (switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON) { - send_arm_command(ArmingAction::ARM, ArmingOrigin::SWITCH); + send_arm_command(vehicle_command_s::ARM_DISARM_ARMING_ACTION_ARM, + vehicle_command_s::ARM_DISARM_ARMING_ORIGIN_SWITCH); } else if (switches.arm_switch == manual_control_switches_s::SWITCH_POS_OFF) { - send_arm_command(ArmingAction::DISARM, ArmingOrigin::SWITCH); + send_arm_command(vehicle_command_s::ARM_DISARM_ARMING_ACTION_DISARM, + vehicle_command_s::ARM_DISARM_ARMING_ORIGIN_SWITCH); } } } @@ -427,7 +432,7 @@ void ManualControl::send_mode_command(int32_t commander_main_state) command_pub.publish(command); } -void ManualControl::send_arm_command(ArmingAction action, ArmingOrigin origin) +void ManualControl::send_arm_command(int8_t action, int8_t origin) { vehicle_command_s command{}; command.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM; diff --git a/src/modules/manual_control/ManualControl.hpp b/src/modules/manual_control/ManualControl.hpp index 8944b92dbd..cd76229fce 100644 --- a/src/modules/manual_control/ManualControl.hpp +++ b/src/modules/manual_control/ManualControl.hpp @@ -116,23 +116,11 @@ public: private: static constexpr int MAX_MANUAL_INPUT_COUNT = 3; - enum class ArmingAction { - TOGGLE = -1, - DISARM = 0, - ARM = 1, - }; - - enum class ArmingOrigin { - GESTURE = 1, - SWITCH = 2, - BUTTON = 3, - }; - void Run() override; void evaluate_mode_slot(uint8_t mode_slot); void send_mode_command(int32_t commander_main_state); - void send_arm_command(ArmingAction action, ArmingOrigin origin); + void send_arm_command(int8_t action, int8_t origin); void send_rtl_command(); void send_loiter_command(); void send_offboard_command();