fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-27 10:50:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
to support 3 different power states for the payload.
This commit is contained in:
Matthias Grob
2025-05-06 17:06:05 +02:00
parent 1bccd5557a
commit daa491dc19
6 changed files with 118 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/manual_control/ManualControl.cpp
+2 -1
View File
@@ -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
+62 -36
View File
@@ -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
+18 -1
View File
@@ -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
+24 -2
View File
@@ -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
+10
View File
@@ -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
)