manual_control: add instance to selector

This instance is then used to schedule the callback for the
subscription.

src/modules/manual_control/ManualControl.cpp

@@ -92,7 +92,7 @@ void ManualControl::Run()
 
 			found_at_least_one = true;
 
-			_selector.update_manual_control_input(now, manual_control_input);
+			_selector.update_manual_control_input(now, manual_control_input, i);
 
 		}
 	}
@@ -133,21 +133,23 @@ void ManualControl::Run()
 		_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();
+		}
+
+
 	} else {
-		_published_invalid_once = true;
+		_last_selected_input = -1;
-		_manual_control_setpoint_pub.publish(_selector.setpoint());
+		if (!_published_invalid_once) {
+			_published_invalid_once = true;
+			_manual_control_setpoint_pub.publish(_selector.setpoint());
+		}
 	}
 
-	// FIXME: get from selector
-	//if ((selected_manual_input >= 0) && (selected_manual_input != _selected_manual_input)) {
-	//	if (_selected_manual_input >= 0) {
-	//		PX4_INFO("selected manual_control_input changed %d -> %d", _selected_manual_input, selected_manual_input);
-	//	}
-
-	//	_manual_control_input_subs[selected_manual_input].registerCallback();
-	//	_selected_manual_input = selected_manual_input;
-	//}
-
 	// reschedule timeout
 	ScheduleDelayed(200_ms);
 
@@ -198,7 +200,7 @@ int ManualControl::print_usage(const char *reason)
 	PRINT_MODULE_DESCRIPTION(
 		R"DESCR_STR(
 ### Description
-
+Module consuming manual_control_inputs publishing one manual_control_setpoint.
 
 )DESCR_STR");
 

src/modules/manual_control/ManualControl.hpp

@@ -98,6 +98,7 @@ private:
 
 	ManualControlSelector _selector;
 	bool _published_invalid_once{false};
+	int _last_selected_input{-1};
 
 	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")};

src/modules/manual_control/ManualControlSelector.cpp

@@ -41,10 +41,11 @@ void ManualControlSelector::update_time_only(uint64_t now)
 {
 	if (_setpoint.timestamp_sample + _timeout < now) {
 		_setpoint.valid = false;
+		_instance = -1;
 	}
 }
 
-void ManualControlSelector::update_manual_control_input(uint64_t now, const manual_control_input_s &input)
+void ManualControlSelector::update_manual_control_input(uint64_t now, const manual_control_input_s &input, int instance)
 {
 	// This method requires the current timestamp explicitly in order to prevent weird cases
 	// if the timestamp_sample of some source is invalid/wrong.
@@ -55,6 +56,7 @@ void ManualControlSelector::update_manual_control_input(uint64_t now, const manu
 	if (_rc_in_mode == 0 && input.data_source == manual_control_input_s::SOURCE_RC) {
 		_setpoint = setpoint_from_input(input);
 		_setpoint.valid = true;
+		_instance = instance;
 
 	} else if (_rc_in_mode == 1 && (input.data_source == manual_control_input_s::SOURCE_MAVLINK_0
 			|| input.data_source == manual_control_input_s::SOURCE_MAVLINK_1
@@ -67,6 +69,7 @@ void ManualControlSelector::update_manual_control_input(uint64_t now, const manu
 		if (_setpoint.data_source == input.data_source || !_setpoint.valid) {
 			_setpoint = setpoint_from_input(input);
 			_setpoint.valid = true;
+			_instance = instance;
 		}
 	} else if (_rc_in_mode == 2) {
 		// FIXME: what to do in the legacy case?
@@ -76,6 +79,7 @@ void ManualControlSelector::update_manual_control_input(uint64_t now, const manu
 		if (_setpoint.data_source == input.data_source || !_setpoint.valid) {
 			_setpoint = setpoint_from_input(input);
 			_setpoint.valid = true;
+			_instance = instance;
 		}
 	} else {
 		// FIXME: param value unknown, what to do?

src/modules/manual_control/ManualControlSelector.hpp

@@ -45,8 +45,9 @@ public:
 	void set_rc_in_mode(int32_t rc_in_mode) { _rc_in_mode = rc_in_mode; }
 	void set_timeout(uint64_t timeout) { _timeout = timeout; }
 	void update_time_only(uint64_t now);
-	void update_manual_control_input(uint64_t now, const manual_control_input_s &input);
+	void update_manual_control_input(uint64_t now, const manual_control_input_s &input, int instance);
 	manual_control_setpoint_s &setpoint();
+	int instance() const { return _instance; };
 
 private:
 	manual_control_setpoint_s setpoint_from_input(const manual_control_input_s &input);
@@ -54,6 +55,7 @@ private:
 	manual_control_setpoint_s _setpoint {};
 	uint64_t _timeout {0};
 	int32_t _rc_in_mode {0};
+	int _instance {-1};
 };
 
 } // namespace manual_control

src/modules/manual_control/ManualControlSelectorTest.cpp

@@ -52,7 +52,7 @@ TEST(ManualControlSelector, RcInputOnly)
 	manual_control_input_s input {};
 	input.data_source = manual_control_input_s::SOURCE_MAVLINK_0;
 	input.timestamp_sample = timestamp;
-	selector.update_manual_control_input(timestamp, input);
+	selector.update_manual_control_input(timestamp, input, 0);
 
 	EXPECT_FALSE(selector.setpoint().valid);
 
@@ -61,10 +61,11 @@ TEST(ManualControlSelector, RcInputOnly)
 	// Now provide input with the correct source.
 	input.data_source = manual_control_input_s::SOURCE_RC;
 	input.timestamp_sample = timestamp;
-	selector.update_manual_control_input(timestamp, input);
+	selector.update_manual_control_input(timestamp, input, 1);
 
 	EXPECT_TRUE(selector.setpoint().valid);
 	EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_RC);
+	EXPECT_EQ(selector.instance(), 1);
 }
 
 TEST(ManualControlSelector, MavlinkInputOnly)
@@ -78,7 +79,7 @@ TEST(ManualControlSelector, MavlinkInputOnly)
 	manual_control_input_s input {};
 	input.data_source = manual_control_input_s::SOURCE_RC;
 	input.timestamp_sample = timestamp;
-	selector.update_manual_control_input(timestamp, input);
+	selector.update_manual_control_input(timestamp, input, 0);
 
 	EXPECT_FALSE(selector.setpoint().valid);
 
@@ -87,10 +88,11 @@ TEST(ManualControlSelector, MavlinkInputOnly)
 	// Now provide input with the correct source.
 	input.data_source = manual_control_input_s::SOURCE_MAVLINK_3;
 	input.timestamp_sample = timestamp;
-	selector.update_manual_control_input(timestamp, input);
+	selector.update_manual_control_input(timestamp, input, 1);
 
 	EXPECT_TRUE(selector.setpoint().valid);
 	EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_MAVLINK_3);
+	EXPECT_EQ(selector.instance(), 1);
 
 	// But only the first MAVLink source wins, others are too late.
 
@@ -99,10 +101,11 @@ TEST(ManualControlSelector, MavlinkInputOnly)
 	// Now provide input with the correct source.
 	input.data_source = manual_control_input_s::SOURCE_MAVLINK_4;
 	input.timestamp_sample = timestamp;
-	selector.update_manual_control_input(timestamp, input);
+	selector.update_manual_control_input(timestamp, input, 1);
 
 	EXPECT_TRUE(selector.setpoint().valid);
 	EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_MAVLINK_3);
+	EXPECT_EQ(selector.instance(), 1);
 }
 
 TEST(ManualControlSelector, AutoInput)
@@ -116,30 +119,33 @@ TEST(ManualControlSelector, AutoInput)
 	manual_control_input_s input {};
 	input.data_source = manual_control_input_s::SOURCE_RC;
 	input.timestamp_sample = timestamp;
-	selector.update_manual_control_input(timestamp, input);
+	selector.update_manual_control_input(timestamp, input, 0);
 
 	EXPECT_TRUE(selector.setpoint().valid);
 	EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_RC);
+	EXPECT_EQ(selector.instance(), 0);
 
 	timestamp += 100_ms;
 
 	// Now provide input from MAVLink as well which should get ignored.
 	input.data_source = manual_control_input_s::SOURCE_MAVLINK_0;
 	input.timestamp_sample = timestamp;
-	selector.update_manual_control_input(timestamp, input);
+	selector.update_manual_control_input(timestamp, input, 1);
 
 	EXPECT_TRUE(selector.setpoint().valid);
 	EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_RC);
+	EXPECT_EQ(selector.instance(), 0);
 
 	timestamp += 500_ms;
 
 	// Now we'll let RC time out, so it should switch to MAVLINK.
 	input.data_source = manual_control_input_s::SOURCE_MAVLINK_0;
 	input.timestamp_sample = timestamp;
-	selector.update_manual_control_input(timestamp, input);
+	selector.update_manual_control_input(timestamp, input, 1);
 
 	EXPECT_TRUE(selector.setpoint().valid);
 	EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_MAVLINK_0);
+	EXPECT_EQ(selector.instance(), 1);
 }
 
 TEST(ManualControlSelector, RcTimeout)
@@ -153,10 +159,11 @@ TEST(ManualControlSelector, RcTimeout)
 	manual_control_input_s input {};
 	input.data_source = manual_control_input_s::SOURCE_RC;
 	input.timestamp_sample = timestamp;
-	selector.update_manual_control_input(timestamp, input);
+	selector.update_manual_control_input(timestamp, input, 0);
 
 	EXPECT_TRUE(selector.setpoint().valid);
 	EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_RC);
+	EXPECT_EQ(selector.instance(), 0);
 
 	timestamp += 600_ms;
 
@@ -164,4 +171,5 @@ TEST(ManualControlSelector, RcTimeout)
 	selector.update_time_only(timestamp);
 
 	EXPECT_FALSE(selector.setpoint().valid);
+	EXPECT_EQ(selector.instance(), -1);
 }