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

@@ -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

src/lib/events/enums.json

@@ -172,6 +172,10 @@
                         "13": {
                             "name": "unit_test",
                             "description": "unit tests"
+                        },
+                        "14": {
+                            "name": "rc_button",
+                            "description": "RC (button)"
                         }
                     }
                 },

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<int>(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<float>(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<int>(roundf(cmd.param2)) == 21196);
 
 				const bool cmd_from_manual_stick = (static_cast<int>(roundf(cmd.param3)) == 1);
+				const bool cmd_from_manual_switch = (static_cast<int>(roundf(cmd.param3)) == 2);
+				const bool cmd_from_manual_button = (static_cast<int>(roundf(cmd.param3)) == 3);
 				const bool cmd_from_io = (static_cast<int>(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();

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) {
+						if (_param_com_arm_swisbtn.get()) {
-							send_arm_command(true, ArmingOrigin::SWITCH);
+							// Arming button
+							if (switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON) {
+								send_arm_command(ArmingAction::TOGGLE, ArmingOrigin::BUTTON);
+							}
 
-						} else if (switches.arm_switch == manual_control_switches_s::SWITCH_POS_OFF) {
+						} else {
-							send_arm_command(false, ArmingOrigin::SWITCH);
+							// 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<float>(action);
-
 	command.param3 = static_cast<float>(origin); // We use param3 to signal the origin.
 
 	command.target_system = 1;

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<px4::params::COM_RC_LOSS_T>) _param_com_rc_loss_t,
 		(ParamFloat<px4::params::COM_RC_STICK_OV>) _param_com_rc_stick_ov,
 		(ParamInt<px4::params::COM_RC_ARM_HYST>) _param_rc_arm_hyst,
+		(ParamBool<px4::params::COM_ARM_SWISBTN>) _param_com_arm_swisbtn,
 		(ParamInt<px4::params::COM_FLTMODE1>) _param_fltmode_1,
 		(ParamInt<px4::params::COM_FLTMODE2>) _param_fltmode_2,
 		(ParamInt<px4::params::COM_FLTMODE3>) _param_fltmode_3,