manual_control: implement (dis)arming via command

src/modules/commander/Commander.cpp

@@ -921,6 +921,15 @@ Commander::handle_command(const vehicle_command_s &cmd)
 				// 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_io = (static_cast<int>(roundf(cmd.param3)) == 1234);
+
+				if (cmd_from_manual_stick && !_vehicle_control_mode.flag_control_manual_enabled) {
+					mavlink_log_critical(&_mavlink_log_pub, "Not arming! Switch to a manual mode first");
+					cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
+					break;
+				}
+
 				if (!forced) {
 					if (!(_land_detector.landed || _land_detector.maybe_landed) && !is_ground_rover(_status)) {
 						if (cmd_arms) {
@@ -948,8 +957,6 @@ Commander::handle_command(const vehicle_command_s &cmd)
 						break;
 					}
 
-					const bool cmd_from_io = (static_cast<int>(roundf(cmd.param3)) == 1234);
-
 					// 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) {
@@ -964,7 +971,11 @@ Commander::handle_command(const vehicle_command_s &cmd)
 						arming_res = arm(arm_disarm_reason_t::command_external);
 
 					} else {
-						arming_res = arm(arm_disarm_reason_t::command_internal, !forced);
+						if (cmd_from_manual_stick) {
+							arming_res = arm(arm_disarm_reason_t::rc_stick, !forced);
+						} else {
+							arming_res = arm(arm_disarm_reason_t::command_internal, !forced);
+						}
 					}
 
 				} else {
@@ -972,7 +983,11 @@ Commander::handle_command(const vehicle_command_s &cmd)
 						arming_res = disarm(arm_disarm_reason_t::command_external);
 
 					} else {
-						arming_res = disarm(arm_disarm_reason_t::command_internal);
+						if (cmd_from_manual_stick) {
+							arming_res = disarm(arm_disarm_reason_t::rc_stick);
+						} else {
+							arming_res = disarm(arm_disarm_reason_t::command_internal);
+						}
 					}
 				}
 
@@ -2383,26 +2398,6 @@ Commander::run()
 
 			_status.rc_signal_lost = false;
 
-			const bool rc_arming_enabled = (_status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF);
-
-			if (rc_arming_enabled) {
-				if (_manual_control.wantsDisarm(_vehicle_control_mode, _status, _manual_control_switches, _land_detector.landed)) {
-					disarm(arm_disarm_reason_t::rc_stick);
-				}
-
-				if (_manual_control.wantsArm(_vehicle_control_mode, _status, _manual_control_switches, _land_detector.landed)) {
-					if (_vehicle_control_mode.flag_control_manual_enabled) {
-						arm(arm_disarm_reason_t::rc_stick);
-
-					} else {
-						mavlink_log_critical(&_mavlink_log_pub, "Not arming! Switch to a manual mode first\t");
-						events::send(events::ID("commander_arming_denied_not_manual"), {events::Log::Error, events::LogInternal::Info},
-							     "Not arming! Switch to a manual mode first");
-						tune_negative(true);
-					}
-				}
-			}
-
 			// abort autonomous mode and switch to position mode if sticks are moved significantly
 			if ((_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)
 			    && !in_low_battery_failsafe && !_geofence_warning_action_on

src/modules/manual_control/ManualControl.cpp

@@ -34,6 +34,7 @@
 #include "ManualControl.hpp"
 
 #include <drivers/drv_hrt.h>
+#include <uORB/topics/vehicle_command.h>
 
 namespace manual_control
 {
@@ -91,12 +92,17 @@ void ManualControl::Run()
 		if (_manual_control_input_subs[i].update(&manual_control_input)) {
 
 			found_at_least_one = true;
-
 			_selector.update_manual_control_input(now, manual_control_input, i);
-
 		}
 	}
 
+	bool switches_updated = false;
+	manual_control_switches_s manual_control_switches;
+
+	if (_manual_control_switches_sub.update(&manual_control_switches)) {
+		switches_updated = true;
+	}
+
 	if (!found_at_least_one) {
 		_selector.update_time_only(now);
 	}
@@ -114,6 +120,21 @@ void ManualControl::Run()
 		_selector.setpoint().arm_gesture = _stick_arm_hysteresis.get_state();
 		_selector.setpoint().disarm_gesture = _stick_disarm_hysteresis.get_state();
 
+		if (_selector.setpoint().arm_gesture && !_previous_arm_gesture) {
+			_previous_arm_gesture = true;
+			send_arm_command();
+
+		} else if (!_selector.setpoint().arm_gesture) {
+			_previous_arm_gesture = false;
+		}
+
+		if (_selector.setpoint().disarm_gesture && !_previous_disarm_gesture) {
+			_previous_disarm_gesture = true;
+			send_disarm_command();
+
+		} else if (!_selector.setpoint().disarm_gesture) {
+			_previous_disarm_gesture = false;
+		}
 
 		// user wants override
 		//const float minimum_stick_change = 0.01f * _param_com_rc_stick_ov.get();
@@ -129,18 +150,26 @@ void ManualControl::Run()
 
 		//_selector.setpoint().user_override = rpy_moved || (use_throttle && throttle_moved);
 
+		// TODO: at least 3 samples in a time
+
+		if (switches_updated) {
+			// Only use switches if current source is RC as well.
+			if (_selector.setpoint().data_source == manual_control_input_s::SOURCE_RC) {
+				// TODO: handle buttons
+			}
+		}
 
 		_selector.setpoint().timestamp = now;
 		_manual_control_setpoint_pub.publish(_selector.setpoint());
 
-		_manual_control_input_subs[_selector.instance()].registerCallback();
-
 		if (_last_selected_input != _selector.instance()) {
 
 			PX4_INFO("selected manual_control_input changed %d -> %d", _last_selected_input, _selector.instance());
 			_last_selected_input = _selector.instance();
 		}
 
+		_manual_control_input_subs[_selector.instance()].registerCallback();
+		_manual_control_switches_sub.registerCallback();
 
 	} else {
 		_last_selected_input = -1;
@@ -156,6 +185,34 @@ void ManualControl::Run()
 	perf_end(_loop_perf);
 }
 
+void ManualControl::send_arm_command()
+{
+	vehicle_command_s command{};
+	command.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM;
+	command.param1 = 1.0;
+	command.param3 = 1.0; // We use param3 to signal that the origin is manual control stick.
+	command.target_system = 1;
+	command.target_component = 1;
+
+	uORB::Publication<vehicle_command_s> command_pub{ORB_ID(vehicle_command)};
+	command.timestamp = hrt_absolute_time();
+	command_pub.publish(command);
+}
+
+void ManualControl::send_disarm_command()
+{
+	vehicle_command_s command{};
+	command.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM;
+	command.param1 = 0.0;
+	command.param3 = 1.0; // We use param3 to signal that the origin is manual control stick.
+	command.target_system = 1;
+	command.target_component = 1;
+
+	uORB::Publication<vehicle_command_s> command_pub{ORB_ID(vehicle_command)};
+	command.timestamp = hrt_absolute_time();
+	command_pub.publish(command);
+}
+
 int ManualControl::task_spawn(int argc, char *argv[])
 {
 	ManualControl *instance = new ManualControl();

src/modules/manual_control/ManualControl.hpp

@@ -41,6 +41,7 @@
 #include <lib/hysteresis/hysteresis.h>
 #include <lib/perf/perf_counter.h>
 #include <uORB/topics/manual_control_input.h>
+#include <uORB/topics/manual_control_switches.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/Publication.hpp>
@@ -84,6 +85,9 @@ private:
 
 	void Run() override;
 
+	void send_arm_command();
+	void send_disarm_command();
+
 	uORB::Publication<manual_control_setpoint_s> _manual_control_setpoint_pub{ORB_ID(manual_control_setpoint)};
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
@@ -92,6 +96,7 @@ private:
 		{this, ORB_ID(manual_control_input), 1},
 		{this, ORB_ID(manual_control_input), 2},
 	};
+	uORB::SubscriptionCallbackWorkItem _manual_control_switches_sub{this, ORB_ID(manual_control_switches)};
 
 	systemlib::Hysteresis _stick_arm_hysteresis{false};
 	systemlib::Hysteresis _stick_disarm_hysteresis{false};
@@ -100,6 +105,9 @@ private:
 	bool _published_invalid_once{false};
 	int _last_selected_input{-1};
 
+	bool _previous_arm_gesture{false};
+	bool _previous_disarm_gesture{false};
+
 	perf_counter_t	_loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
 	perf_counter_t	_loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};