rc_update: make payload power switch an optional 3-way switch

to support 3 different power states for the payload.

src/modules/manual_control/ManualControl.cpp

@@ -277,7 +277,8 @@ void ManualControl::processSwitches(hrt_abstime &now)
 					if (switches.payload_power_switch == manual_control_switches_s::SWITCH_POS_ON) {
 						PAYLOAD_POWER_EN(true);
 
-					} else if (switches.payload_power_switch == manual_control_switches_s::SWITCH_POS_OFF) {
+					} else if (switches.payload_power_switch == manual_control_switches_s::SWITCH_POS_OFF
+						   || switches.payload_power_switch == manual_control_switches_s::SWITCH_POS_MIDDLE) {
 						PAYLOAD_POWER_EN(false);
 					}
 				}

src/modules/rc_update/RCUpdateTest.cpp

@@ -128,16 +128,19 @@ public:
 		EXPECT_EQ(manual_control_switches_sub.get().return_switch, expected_position);
 	}
 
-	void checkPayloadPowerSwitch(float channel_value, float threshold, uint8_t expected_position)
+	void checkPayloadPowerSwitch(float channel_value, float on_threshold, float mid_threshold, uint8_t expected_position)
 	{
 		// GIVEN: First channel is configured as payload power switch
 		_param_rc_map_pay_sw.set(1);
 		_param_rc_map_pay_sw.commit();
-		_param_rc_payload_th.set(threshold);
+		_param_rc_payload_th.set(on_threshold);
 		_param_rc_payload_th.commit();
+		_param_rc_payload_midth.set(mid_threshold);
+		_param_rc_payload_midth.commit();
 		_rc_update.updateParams();
 		EXPECT_EQ(_param_rc_map_pay_sw.get(), 1);
-		EXPECT_FLOAT_EQ(_param_rc_payload_th.get(), threshold);
+		EXPECT_FLOAT_EQ(_param_rc_payload_th.get(), on_threshold);
+		EXPECT_FLOAT_EQ(_param_rc_payload_midth.get(), mid_threshold);
 		// GIVEN: First channel has some value
 		_rc_update.setChannel(0, channel_value);
 
@@ -160,7 +163,8 @@ public:
 		(ParamFloat<px4::params::RC_RETURN_TH>) _param_rc_return_th,
 
 		(ParamInt<px4::params::RC_MAP_PAY_SW>) _param_rc_map_pay_sw,
-		(ParamFloat<px4::params::RC_PAYLOAD_TH>) _param_rc_payload_th
+		(ParamFloat<px4::params::RC_PAYLOAD_TH>) _param_rc_payload_th,
+		(ParamFloat<px4::params::RC_PAYLOAD_MIDTH>) _param_rc_payload_midth
 	)
 };
 
@@ -260,43 +264,65 @@ TEST_F(RCUpdateTest, ReturnSwitchNegativeThresholds)
 	checkReturnSwitch(-1.f, -.001f, 1); // Slightly below minimum threshold -> SWITCH_POS_OFF
 }
 
-TEST_F(RCUpdateTest, PayloadPowerSwitchPositiveThresholds)
+TEST_F(RCUpdateTest, PayloadPower2WaySwitchPositiveThresholds)
 {
-	checkPayloadPowerSwitch(-1.f, 0.5f, 3); // Below threshold -> SWITCH_POS_OFF
-	checkPayloadPowerSwitch(0.f, 0.5f, 3); // On threshold -> SWITCH_POS_OFF
-	checkPayloadPowerSwitch(.001f, 0.5f, 1); // Slightly above threshold -> SWITCH_POS_ON
-	checkPayloadPowerSwitch(1.f, 0.5f, 1); // Above threshold -> SWITCH_POS_ON
+	checkPayloadPowerSwitch(-1.f, 0.5f, 0.5f, 3); // Below threshold -> SWITCH_POS_OFF
+	checkPayloadPowerSwitch(0.f, 0.5f, 0.5f, 3); // On threshold -> SWITCH_POS_OFF
+	checkPayloadPowerSwitch(.001f, 0.5f, 0.5f, 1); // Slightly above threshold -> SWITCH_POS_ON
+	checkPayloadPowerSwitch(1.f, 0.5f, 0.5f, 1); // Above threshold -> SWITCH_POS_ON
 
-	checkPayloadPowerSwitch(-1.f, 0.75f, 3); // Below threshold -> SWITCH_POS_OFF
-	checkPayloadPowerSwitch(0.f, 0.75f, 3); // Below threshold -> SWITCH_POS_OFF
-	checkPayloadPowerSwitch(.5f, 0.75f, 3); // On threshold -> SWITCH_POS_OFF
-	checkPayloadPowerSwitch(.501f, 0.75f, 1); // Slightly above threshold -> SWITCH_POS_ON
-	checkPayloadPowerSwitch(1.f, 0.75f, 1); // Above threshold -> SWITCH_POS_ON
+	checkPayloadPowerSwitch(-1.f, 0.75f, 0.75f, 3); // Below threshold -> SWITCH_POS_OFF
+	checkPayloadPowerSwitch(0.f, 0.75f, 0.75f, 3); // Below threshold -> SWITCH_POS_OFF
+	checkPayloadPowerSwitch(.5f, 0.75f, 0.75f, 3); // On threshold -> SWITCH_POS_OFF
+	checkPayloadPowerSwitch(.501f, 0.75f, 0.75f, 1); // Slightly above threshold -> SWITCH_POS_ON
+	checkPayloadPowerSwitch(1.f, 0.75f, 0.75f, 1); // Above threshold -> SWITCH_POS_ON
 
-	checkPayloadPowerSwitch(-1.f, 0.f, 3); // On minimum threshold -> SWITCH_POS_OFF
-	checkPayloadPowerSwitch(-.999f, 0.f, 1); // Slightly above minimum threshold -> SWITCH_POS_ON
-	checkPayloadPowerSwitch(1.f, 0.f, 1); // Above minimum threshold -> SWITCH_POS_ON
+	checkPayloadPowerSwitch(-1.f, 0.f, 0.f, 3); // On minimum threshold -> SWITCH_POS_OFF
+	checkPayloadPowerSwitch(-.999f, 0.f, 0.f, 1); // Slightly above minimum threshold -> SWITCH_POS_ON
+	checkPayloadPowerSwitch(1.f, 0.f, 0.f, 1); // Above minimum threshold -> SWITCH_POS_ON
 
-	checkPayloadPowerSwitch(-1.f, 1.f, 3); // Below maximum threshold -> SWITCH_POS_OFF
-	checkPayloadPowerSwitch(1.f, 1.f, 3); // On maximum threshold -> SWITCH_POS_OFF
+	checkPayloadPowerSwitch(-1.f, 1.f, 1.f, 3); // Below maximum threshold -> SWITCH_POS_OFF
+	checkPayloadPowerSwitch(1.f, 1.f, 1.f, 3); // On maximum threshold -> SWITCH_POS_OFF
 }
 
-TEST_F(RCUpdateTest, PayloadPowerSwitchNegativeThresholds)
+TEST_F(RCUpdateTest, PayloadPower3WaySwitchPositiveThresholds)
 {
-	checkPayloadPowerSwitch(1.f, -0.5f, 3); // Above threshold -> SWITCH_POS_OFF
-	checkPayloadPowerSwitch(0.f, -0.5f, 3); // On threshold -> SWITCH_POS_OFF
-	checkPayloadPowerSwitch(-.001f, -0.5f, 1); // Slightly below threshold -> SWITCH_POS_ON
-	checkPayloadPowerSwitch(-1.f, -0.5f, 1); // Below threshold -> SWITCH_POS_ON
-
-	checkPayloadPowerSwitch(1.f, -0.75f, 3); // Above threshold -> SWITCH_POS_OFF
-	checkPayloadPowerSwitch(.5f, -0.75f, 3); // On threshold -> SWITCH_POS_OFF
-	checkPayloadPowerSwitch(.499f, -0.75f, 1); // Slightly below threshold -> SWITCH_POS_ON
-	checkPayloadPowerSwitch(-1.f, -0.75f, 1); // Below threshold -> SWITCH_POS_ON
-
-	checkPayloadPowerSwitch(1.f, -1.f, 3); // On maximum threshold -> SWITCH_POS_OFF
-	checkPayloadPowerSwitch(.999f, -1.f, 1); // Slighly below maximum threshold -> SWITCH_POS_ON
-	checkPayloadPowerSwitch(-1.f, -1.f, 1); // Below minimum threshold -> SWITCH_POS_ON
-
-	checkPayloadPowerSwitch(1.f, -.001f, 3); // Above minimum threshold -> SWITCH_POS_OFF
-	checkPayloadPowerSwitch(-1.f, -.001f, 1); // Slightly below minimum threshold -> SWITCH_POS_OFF
+	checkPayloadPowerSwitch(-1.f, 0.75f, 0.25f, 3); // Below mid_threshold -> SWITCH_POS_OFF
+	checkPayloadPowerSwitch(-0.5f, 0.75f, 0.25f, 3); // On mid_threshold -> SWITCH_POS_OFF
+	checkPayloadPowerSwitch(-0.449f, 0.75f, 0.25f, 2); // Slightly above mid_threshold -> SWITCH_POS_MIDDLE
+	checkPayloadPowerSwitch(0.f, 0.75f, 0.25f, 2); // Between mid_threshold and on_threshold -> SWITCH_POS_MIDDLE
+	checkPayloadPowerSwitch(0.5f, 0.75f, 0.25f, 2); // On on_threshold -> SWITCH_POS_MIDDLE
+	checkPayloadPowerSwitch(0.501f, 0.75f, 0.25f, 1); // Slightly above on_threshold -> SWITCH_POS_ON
+	checkPayloadPowerSwitch(1.f, 0.75f, 0.25f, 1); // Above on_threshold -> SWITCH_POS_ON
+}
+
+TEST_F(RCUpdateTest, PayloadPower2WaySwitchNegativeThresholds)
+{
+	checkPayloadPowerSwitch(1.f, -0.5f, -0.5f, 3); // Above threshold -> SWITCH_POS_OFF
+	checkPayloadPowerSwitch(0.f, -0.5f, -0.5f, 3); // On threshold -> SWITCH_POS_OFF
+	checkPayloadPowerSwitch(-.001f, -0.5f, -0.5f, 1); // Slightly below threshold -> SWITCH_POS_ON
+	checkPayloadPowerSwitch(-1.f, -0.5f, -0.5f, 1); // Below threshold -> SWITCH_POS_ON
+
+	checkPayloadPowerSwitch(1.f, -0.75f, -0.75f, 3); // Above threshold -> SWITCH_POS_OFF
+	checkPayloadPowerSwitch(.5f, -0.75f, -0.75f, 3); // On threshold -> SWITCH_POS_OFF
+	checkPayloadPowerSwitch(.499f, -0.75f, -0.75f, 1); // Slightly below threshold -> SWITCH_POS_ON
+	checkPayloadPowerSwitch(-1.f, -0.75f, -0.75f, 1); // Below threshold -> SWITCH_POS_ON
+
+	checkPayloadPowerSwitch(1.f, -1.f, -1.f, 3); // On maximum threshold -> SWITCH_POS_OFF
+	checkPayloadPowerSwitch(.999f, -1.f, -1.f, 1); // Slighly below maximum threshold -> SWITCH_POS_ON
+	checkPayloadPowerSwitch(-1.f, -1.f, -1.f, 1); // Below minimum threshold -> SWITCH_POS_ON
+
+	checkPayloadPowerSwitch(1.f, -.001f, -.001f, 3); // Above minimum threshold -> SWITCH_POS_OFF
+	checkPayloadPowerSwitch(-1.f, -.001f, -.001f, 1); // Slightly below minimum threshold -> SWITCH_POS_OFF
+}
+
+TEST_F(RCUpdateTest, PayloadPower3WaySwitchNegativeThresholds)
+{
+	checkPayloadPowerSwitch(1.f, -0.75f, -0.25f, 3); // Above on_threshold -> SWITCH_POS_OFF
+	checkPayloadPowerSwitch(0.5f, -0.75f, -0.25f, 3); // On on_threshold -> SWITCH_POS_OFF
+	checkPayloadPowerSwitch(0.449f, -0.75f, -0.25f, 2); // Slightly below on_threshold -> SWITCH_POS_MIDDLE
+	checkPayloadPowerSwitch(0.f, -0.75f, -0.25f, 2); // Between on_threshold and mid_threshold -> SWITCH_POS_MIDDLE
+	checkPayloadPowerSwitch(-0.5f, -0.75f, -0.25f, 2); // On mid_threshold -> SWITCH_POS_MIDDLE
+	checkPayloadPowerSwitch(-0.501f, -0.75f, -0.25f, 1); // Slightly above on_threshold -> SWITCH_POS_ON
+	checkPayloadPowerSwitch(-1.f, -0.75f, -0.25f, 1); // Above on_threshold -> SWITCH_POS_ON
 }

src/modules/rc_update/params.c

@@ -2061,7 +2061,7 @@ PARAM_DEFINE_FLOAT(RC_GEAR_TH, 0.75f);
 PARAM_DEFINE_FLOAT(RC_ENG_MOT_TH, 0.75f);
 
 /**
- * Threshold for selecting payload power switch
+ * Threshold for on position of payload power switch
  *
  * 0-1 indicate where in the full channel range the threshold sits
  * 		0 : min
@@ -2077,6 +2077,23 @@ PARAM_DEFINE_FLOAT(RC_ENG_MOT_TH, 0.75f);
  */
 PARAM_DEFINE_FLOAT(RC_PAYLOAD_TH, 0.75f);
 
+/**
+ * Threshold for mid position of payload power switch
+ *
+ * 0-1 indicate where in the full channel range the threshold sits
+ * 		0 : min
+ * 		1 : max
+ * sign indicates polarity of comparison
+ * 		positive : true when channel>th
+ * 		negative : true when channel<th
+ *
+ * @min -1
+ * @max 1
+ * @decimal 2
+ * @group Radio Switches
+ */
+PARAM_DEFINE_FLOAT(RC_PAYLOAD_MIDTH, 0.25f);
+
 /**
  * PWM input channel that provides RSSI.
  *

src/modules/rc_update/rc_update.cpp

@@ -544,6 +544,28 @@ switch_pos_t RCUpdate::getRCSwitchOnOffPosition(uint8_t function, float threshol
 	return manual_control_switches_s::SWITCH_POS_NONE;
 }
 
+switch_pos_t RCUpdate::getRCSwitch3WayPosition(uint8_t function, float on_threshold, float mid_threshold) const
+{
+	if (_rc.function[function] >= 0) {
+		const bool on_inverted = (on_threshold < 0.f);
+		const bool mid_inverted = (mid_threshold < 0.f);
+
+		const float value = 0.5f * _rc.channels[_rc.function[function]] + 0.5f;
+
+		if (on_inverted ? value < -mid_threshold : value > on_threshold) {
+			return manual_control_switches_s::SWITCH_POS_ON;
+
+		} else if (mid_inverted ? value < -on_threshold : value > mid_threshold) {
+			return manual_control_switches_s::SWITCH_POS_MIDDLE;
+
+		} else {
+			return manual_control_switches_s::SWITCH_POS_OFF;
+		}
+	}
+
+	return manual_control_switches_s::SWITCH_POS_NONE;
+}
+
 void RCUpdate::UpdateManualSwitches(const hrt_abstime &timestamp_sample)
 {
 	manual_control_switches_s switches{};
@@ -619,8 +641,8 @@ void RCUpdate::UpdateManualSwitches(const hrt_abstime &timestamp_sample)
 	switches.video_switch = getRCSwitchOnOffPosition(rc_channels_s::FUNCTION_AUX_4, 0.5f);
 #endif
 
-	switches.payload_power_switch = getRCSwitchOnOffPosition(rc_channels_s::FUNCTION_PAYLOAD_POWER,
-					_param_rc_payload_th.get());
+	switches.payload_power_switch = getRCSwitch3WayPosition(rc_channels_s::FUNCTION_PAYLOAD_POWER,
+					_param_rc_payload_th.get(), _param_rc_payload_midth.get());
 
 	// last 2 switch updates identical within 1 second (simple protection from bad RC data)
 	if ((switches == _manual_switches_previous)

src/modules/rc_update/rc_update.h

@@ -125,6 +125,15 @@ protected:
 	 */
 	switch_pos_t getRCSwitchOnOffPosition(uint8_t function, float threshold) const;
 
+	/**
+	 * Get 3-way switch position from the RC channel of the specified function
+	 *
+	 * @param function according to rc_channels_s::FUNCTION_XXX
+	 * @param on_threshold according to RC_XXX_TH parameters, negative means comparison is flipped
+	 * @param mid_threshold according to RC_XXX_MIDTH parameters, negative means comparison is flipped
+	 */
+	switch_pos_t getRCSwitch3WayPosition(uint8_t function, float on_threshold, float mid_threshold) const;
+
 	/**
 	 * Update parameters from RC channels if the functionality is activated and the
 	 * input has changed since the last update
@@ -241,6 +250,7 @@ protected:
 		(ParamFloat<px4::params::RC_RETURN_TH>) _param_rc_return_th,
 		(ParamFloat<px4::params::RC_ENG_MOT_TH>) _param_rc_eng_mot_th,
 		(ParamFloat<px4::params::RC_PAYLOAD_TH>) _param_rc_payload_th,
+		(ParamFloat<px4::params::RC_PAYLOAD_MIDTH>) _param_rc_payload_midth,
 
 		(ParamInt<px4::params::RC_CHAN_CNT>) _param_rc_chan_cnt
 	)