From bd0c1014d9acac400028809441a5a0c280c1402c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 18 May 2021 12:36:37 +0200 Subject: [PATCH] manual_control: support arming button The arming button required some refactoring in order to support to toggle arm/disarm using the vehicle_command. Otherwise manual_control would have to subscribe to the arming topic and we would spread out the logic again, and risk race conditions. --- msg/vehicle_command.msg | 2 +- src/lib/events/enums.json | 4 ++ src/modules/commander/Commander.cpp | 40 ++++++++++++++++---- src/modules/manual_control/ManualControl.cpp | 30 +++++++++------ src/modules/manual_control/ManualControl.hpp | 9 ++++- 5 files changed, 63 insertions(+), 22 deletions(-) diff --git a/msg/vehicle_command.msg b/msg/vehicle_command.msg index 29516d6454..c014b6f5a5 100644 --- a/msg/vehicle_command.msg +++ b/msg/vehicle_command.msg @@ -69,7 +69,7 @@ uint16 VEHICLE_CMD_OBLIQUE_SURVEY=260 # Mission command to set a Camera Auto M uint16 VEHICLE_CMD_GIMBAL_DEVICE_INFORMATION = 283 # Command to ask information about a low level gimbal uint16 VEHICLE_CMD_MISSION_START = 300 # start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| -uint16 VEHICLE_CMD_COMPONENT_ARM_DISARM = 400 # Arms / Disarms a component |1 to arm, 0 to disarm| +uint16 VEHICLE_CMD_COMPONENT_ARM_DISARM = 400 # Arms / Disarms a component |1 to arm, 0 to disarm|-1 to toggle uint16 VEHICLE_CMD_INJECT_FAILURE = 420 # Inject artificial failure for testing purposes uint16 VEHICLE_CMD_START_RX_PAIR = 500 # Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| uint16 VEHICLE_CMD_REQUEST_MESSAGE = 512 # Request to send a single instance of the specified message diff --git a/src/lib/events/enums.json b/src/lib/events/enums.json index f472545478..43e82fc04f 100644 --- a/src/lib/events/enums.json +++ b/src/lib/events/enums.json @@ -172,6 +172,10 @@ "13": { "name": "unit_test", "description": "unit tests" + }, + "14": { + "name": "rc_button", + "description": "RC (button)" } } }, diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 56106f6d92..cf07e308a2 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -468,6 +468,8 @@ static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_r case arm_disarm_reason_t::shutdown: return "shutdown request"; case arm_disarm_reason_t::unit_test: return "unit tests"; + + case arm_disarm_reason_t::rc_button: return "RC (button)"; } return ""; @@ -911,18 +913,18 @@ Commander::handle_command(const vehicle_command_s &cmd) // for logic state parameters const int param1_arm = static_cast(roundf(cmd.param1)); - if (param1_arm != 0 && param1_arm != 1) { + if (param1_arm != 0 && param1_arm != 1 && param1_arm != -1) { 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 { - const bool cmd_arms = (param1_arm == 1); - // Arm is forced (checks skipped) when param2 is set to a magic number. const bool forced = (static_cast(roundf(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 bool cmd_from_io = (static_cast(roundf(cmd.param3)) == 1234); if (cmd_from_manual_stick && !_vehicle_control_mode.flag_control_manual_enabled) { @@ -933,7 +935,7 @@ Commander::handle_command(const vehicle_command_s &cmd) if (!forced) { if (!(_land_detector.landed || _land_detector.maybe_landed) && !is_ground_rover(_status)) { - if (cmd_arms) { + if (param1_arm == 1) { if (_armed.armed) { mavlink_log_warning(&_mavlink_log_pub, "Arming denied! Already armed\t"); events::send(events::ID("commander_arming_denied_already_armed"), @@ -960,14 +962,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 && cmd_arms) { + && cmd_from_io && (param1_arm == 1)) { _status.arming_state = vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE; } } transition_result_t arming_res = TRANSITION_DENIED; - if (cmd_arms) { + if (param1_arm == 1) { if (cmd.from_external) { arming_res = arm(arm_disarm_reason_t::command_external); @@ -975,12 +977,15 @@ Commander::handle_command(const vehicle_command_s &cmd) if (cmd_from_manual_stick) { arming_res = arm(arm_disarm_reason_t::rc_stick, !forced); + } else if (cmd_from_manual_switch) { + arming_res = arm(arm_disarm_reason_t::rc_switch, !forced); + } else { arming_res = arm(arm_disarm_reason_t::command_internal, !forced); } } - } else { + } else if (param1_arm == 0) { if (cmd.from_external) { arming_res = disarm(arm_disarm_reason_t::command_external); @@ -988,10 +993,29 @@ Commander::handle_command(const vehicle_command_s &cmd) if (cmd_from_manual_stick) { arming_res = disarm(arm_disarm_reason_t::rc_stick); + } else if (cmd_from_manual_switch) { + arming_res = disarm(arm_disarm_reason_t::rc_switch); + } else { arming_res = disarm(arm_disarm_reason_t::command_internal); } } + + } else if (param1_arm == -1) { + // -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) { + if (_armed.armed) { + arming_res = disarm(arm_disarm_reason_t::rc_button); + + } else { + arming_res = arm(arm_disarm_reason_t::rc_button); + } + + } else { + PX4_WARN("ARM_DISARM toggle command ignored"); + } + } if (arming_res == TRANSITION_DENIED) { @@ -1001,7 +1025,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 (cmd_arms && (arming_res == TRANSITION_CHANGED) && + if ((param1_arm == 1) && (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 5d12c7a779..1d2998b4ba 100644 --- a/src/modules/manual_control/ManualControl.cpp +++ b/src/modules/manual_control/ManualControl.cpp @@ -127,7 +127,7 @@ void ManualControl::Run() if (_selector.setpoint().arm_gesture && !_previous_arm_gesture) { _previous_arm_gesture = true; - send_arm_command(true, ArmingOrigin::GESTURE); + send_arm_command(ArmingAction::ARM, ArmingOrigin::GESTURE); } else if (!_selector.setpoint().arm_gesture) { _previous_arm_gesture = false; @@ -135,7 +135,7 @@ void ManualControl::Run() if (_selector.setpoint().disarm_gesture && !_previous_disarm_gesture) { _previous_disarm_gesture = true; - send_arm_command(false, ArmingOrigin::GESTURE); + send_arm_command(ArmingAction::DISARM, ArmingOrigin::GESTURE); } else if (!_selector.setpoint().disarm_gesture) { _previous_disarm_gesture = false; @@ -167,16 +167,23 @@ void ManualControl::Run() } if (switches.arm_switch != _previous_switches.arm_switch) { - if (switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON) { - send_arm_command(true, ArmingOrigin::SWITCH); - - } else if (switches.arm_switch == manual_control_switches_s::SWITCH_POS_OFF) { - send_arm_command(false, ArmingOrigin::SWITCH); + if (_param_com_arm_swisbtn.get()) { + // Arming button + if (switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON) { + send_arm_command(ArmingAction::TOGGLE, ArmingOrigin::BUTTON); + } + + } else { + // Arming switch + if (switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON) { + send_arm_command(ArmingAction::ARM, ArmingOrigin::SWITCH); + + } else if (switches.arm_switch == manual_control_switches_s::SWITCH_POS_OFF) { + send_arm_command(ArmingAction::DISARM, ArmingOrigin::SWITCH); + } } } - // TODO: handle case with arming switch as button - if (switches.return_switch != _previous_switches.return_switch) { if (switches.return_switch == manual_control_switches_s::SWITCH_POS_ON) { send_rtl_command(); @@ -413,12 +420,11 @@ void ManualControl::send_mode_command(int32_t commander_main_state) command_pub.publish(command); } -void ManualControl::send_arm_command(bool should_arm, ArmingOrigin origin) +void ManualControl::send_arm_command(ArmingAction action, ArmingOrigin origin) { vehicle_command_s command{}; command.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM; - command.param1 = should_arm ? 1.0f : 0.0f; - + command.param1 = static_cast(action); command.param3 = static_cast(origin); // We use param3 to signal the origin. command.target_system = 1; diff --git a/src/modules/manual_control/ManualControl.hpp b/src/modules/manual_control/ManualControl.hpp index 9936f4bc65..604410d827 100644 --- a/src/modules/manual_control/ManualControl.hpp +++ b/src/modules/manual_control/ManualControl.hpp @@ -105,6 +105,12 @@ 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, @@ -115,7 +121,7 @@ private: void evaluate_mode_slot(uint8_t mode_slot); void send_mode_command(int32_t commander_main_state); - void send_arm_command(bool should_arm, ArmingOrigin origin); + void send_arm_command(ArmingAction action, ArmingOrigin origin); void send_rtl_command(); void send_loiter_command(); void send_offboard_command(); @@ -162,6 +168,7 @@ private: (ParamFloat) _param_com_rc_loss_t, (ParamFloat) _param_com_rc_stick_ov, (ParamInt) _param_rc_arm_hyst, + (ParamBool) _param_com_arm_swisbtn, (ParamInt) _param_fltmode_1, (ParamInt) _param_fltmode_2, (ParamInt) _param_fltmode_3,