diff --git a/msg/ManualControlSwitches.msg b/msg/ManualControlSwitches.msg index 39b6efe71f..afd9fcb347 100644 --- a/msg/ManualControlSwitches.msg +++ b/msg/ManualControlSwitches.msg @@ -29,8 +29,8 @@ uint8 transition_switch # VTOL transition switch: _HOVER, FORWARD_FLIGH uint8 photo_switch # Photo trigger switch uint8 video_switch # Photo trigger switch -uint8 payload_power_switch # Payload power switch - uint8 engage_main_motor_switch # Engage the main motor (for helicopters) +uint8 payload_power_switch # Payload power switch + uint32 switch_changes # number of switch changes diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp index a927035f93..dc94487218 100644 --- a/src/modules/manual_control/ManualControl.cpp +++ b/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); } } diff --git a/src/modules/rc_update/RCUpdateTest.cpp b/src/modules/rc_update/RCUpdateTest.cpp index 8925b24d89..b6defca603 100644 --- a/src/modules/rc_update/RCUpdateTest.cpp +++ b/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) _param_rc_return_th, (ParamInt) _param_rc_map_pay_sw, - (ParamFloat) _param_rc_payload_th + (ParamFloat) _param_rc_payload_th, + (ParamFloat) _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 } diff --git a/src/modules/rc_update/params.c b/src/modules/rc_update/params.c index df3e95850d..eb734de1dc 100644 --- a/src/modules/rc_update/params.c +++ b/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= 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 ×tamp_sample) { manual_control_switches_s switches{}; @@ -619,8 +641,8 @@ void RCUpdate::UpdateManualSwitches(const hrt_abstime ×tamp_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) diff --git a/src/modules/rc_update/rc_update.h b/src/modules/rc_update/rc_update.h index 6c44b4cbcf..518c7a31fd 100644 --- a/src/modules/rc_update/rc_update.h +++ b/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) _param_rc_return_th, (ParamFloat) _param_rc_eng_mot_th, (ParamFloat) _param_rc_payload_th, + (ParamFloat) _param_rc_payload_midth, (ParamInt) _param_rc_chan_cnt )