Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

rc_update: fix on-off-switch with negative threshold values #21796

Merged
merged 6 commits into from
Jul 11, 2023
Merged

rc_update: fix on-off-switch with negative threshold values #21796

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
415ddbf
rc_update: switch back to protected scope and use fixture for testing
MaEtUgR Jun 30, 2023
76289d9
rc_update: extend ModuleParams::updateParams() for local parameter up…
MaEtUgR Jun 30, 2023
1c41dc6
RCUpdateTest: fix unit test parameters + restoring state after test
MaEtUgR Jul 3, 2023
45f960c
RCUpdateTest: add unit test for return switch
MaEtUgR Jul 3, 2023
ad72d05
rc_update: fix on-off-switch with negative threshold values
MaEtUgR Jul 3, 2023
e1b8f4f
RCUpdateTest: RC switch correct corner case test value
MaEtUgR Jul 5, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
226 changes: 169 additions & 57 deletions src/modules/rc_update/RCUpdateTest.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,15 +38,120 @@

using namespace rc_update;

TEST(RCUpdateTest, ModeSlotUnassigned)
class TestRCUpdate : public RCUpdate
{
public:
void UpdateManualSwitches(const hrt_abstime &timestamp_sample) { RCUpdate::UpdateManualSwitches(timestamp_sample); }
void updateParams() { RCUpdate::updateParams(); }
void setChannel(size_t index, float channel_value) { _rc.channels[index] = channel_value; }
};
junwoo091400 marked this conversation as resolved.
Show resolved Hide resolved

class RCUpdateTest : public ::testing::Test, ModuleParams
{
public:
RCUpdateTest() : ModuleParams(nullptr)
{
// Disable autosaving parameters to avoid busy loop in param_set()
param_control_autosave(false);
}

void checkModeSlotSwitch(float channel_value, uint8_t expected_slot)
{
// GIVEN: First channel is configured as mode switch
_param_rc_map_fltmode.set(1);
_param_rc_map_fltmode.commit();
EXPECT_EQ(_param_rc_map_fltmode.get(), 1);
_rc_update.updateParams();
// GIVEN: First channel has some value
_rc_update.setChannel(0, channel_value);

// WHEN: we update the switches two times to pass the simple outlier protection
_rc_update.UpdateManualSwitches(0);
_rc_update.UpdateManualSwitches(0);

// THEN: we receive the expected mode slot
uORB::SubscriptionData<manual_control_switches_s> manual_control_switches_sub{ORB_ID(manual_control_switches)};
manual_control_switches_sub.update();

EXPECT_EQ(manual_control_switches_sub.get().mode_slot, expected_slot);
}

void checkModeSlotButton(uint8_t button_bitmask, uint8_t channel, float channel_value, uint8_t expected_slot)
{
// GIVEN: No mode switch is mapped
_param_rc_map_fltmode.set(0);
_param_rc_map_fltmode.commit();
EXPECT_EQ(_param_rc_map_fltmode.get(), 0);
// GIVEN: Buttons are configured
_param_rc_map_fltm_btn.set(button_bitmask);
_param_rc_map_fltm_btn.commit();
EXPECT_EQ(_param_rc_map_fltm_btn.get(), button_bitmask);
_rc_update.updateParams();
// GIVEN: First channel has some value
_rc_update.setChannel(channel - 1, channel_value);

// WHEN: we update the switches 4 times:
// - initiate the button press
// - keep the same button pressed
// - hold the button for 50ms
// - pass the simple outlier protection
_rc_update.UpdateManualSwitches(0);
_rc_update.UpdateManualSwitches(0);
_rc_update.UpdateManualSwitches(51_ms);
_rc_update.UpdateManualSwitches(51_ms);

// THEN: we receive the expected mode slot
uORB::SubscriptionData<manual_control_switches_s> manual_control_switches_sub{ORB_ID(manual_control_switches)};
manual_control_switches_sub.update();

EXPECT_EQ(manual_control_switches_sub.get().mode_slot, expected_slot);

// Reset channel value for the next test
_rc_update.setChannel(channel - 1, 0.f);
}

void checkReturnSwitch(float channel_value, float threshold, uint8_t expected_position)
{
// GIVEN: First channel is configured as return switch
_param_rc_map_return_sw.set(1);
_param_rc_map_return_sw.commit();
_param_rc_return_th.set(threshold);
_param_rc_return_th.commit();
_rc_update.updateParams();
EXPECT_EQ(_param_rc_map_return_sw.get(), 1);
EXPECT_FLOAT_EQ(_param_rc_return_th.get(), threshold);
// GIVEN: First channel has some value
_rc_update.setChannel(0, channel_value);

// WHEN: we update the switches two times to pass the simple outlier protection
_rc_update.UpdateManualSwitches(0);
_rc_update.UpdateManualSwitches(0);

// THEN: we receive the expected mode slot
uORB::SubscriptionData<manual_control_switches_s> manual_control_switches_sub{ORB_ID(manual_control_switches)};
manual_control_switches_sub.update();

EXPECT_EQ(manual_control_switches_sub.get().return_switch, expected_position);
}

TestRCUpdate _rc_update;

DEFINE_PARAMETERS(
(ParamInt<px4::params::RC_MAP_FLTMODE>) _param_rc_map_fltmode,
(ParamInt<px4::params::RC_MAP_FLTM_BTN>) _param_rc_map_fltm_btn,
(ParamInt<px4::params::RC_MAP_RETURN_SW>) _param_rc_map_return_sw,
(ParamFloat<px4::params::RC_RETURN_TH>) _param_rc_return_th
)
};

TEST_F(RCUpdateTest, ModeSlotUnassigned)
{
RCUpdate rc_update;
// GIVEN: Default configuration with no assigned mode switch
EXPECT_EQ(rc_update._param_rc_map_fltmode.get(), 0);
EXPECT_EQ(_param_rc_map_fltmode.get(), 0);

// WHEN: we update the switches two times to pass the simple outlier protection
rc_update.UpdateManualSwitches(0);
rc_update.UpdateManualSwitches(0);
_rc_update.UpdateManualSwitches(0);
_rc_update.UpdateManualSwitches(0);

// THEN: we receive no mode slot
uORB::SubscriptionData<manual_control_switches_s> manual_control_switches_sub{ORB_ID(manual_control_switches)};
Expand All @@ -55,28 +160,7 @@ TEST(RCUpdateTest, ModeSlotUnassigned)
EXPECT_EQ(manual_control_switches_sub.get().mode_slot, 0); // manual_control_switches_s::MODE_SLOT_NONE
}

void checkModeSlotSwitch(float channel_value, uint8_t expected_slot)
{
RCUpdate rc_update;

// GIVEN: First channel is configured as mode switch
rc_update._param_rc_map_fltmode.set(1);
EXPECT_EQ(rc_update._param_rc_map_fltmode.get(), 1);
// GIVEN: First channel has some value
rc_update._rc.channels[0] = channel_value;

// WHEN: we update the switches two times to pass the simple outlier protection
rc_update.UpdateManualSwitches(0);
rc_update.UpdateManualSwitches(0);

// THEN: we receive the expected mode slot
uORB::SubscriptionData<manual_control_switches_s> manual_control_switches_sub{ORB_ID(manual_control_switches)};
manual_control_switches_sub.update();

EXPECT_EQ(manual_control_switches_sub.get().mode_slot, expected_slot);
}

TEST(RCUpdateTest, ModeSlotSwitchAllValues)
TEST_F(RCUpdateTest, ModeSlotSwitchAllValues)
{
checkModeSlotSwitch(-1.f, 1); // manual_control_switches_s::MODE_SLOT_1
checkModeSlotSwitch(-.5f, 2); // manual_control_switches_s::MODE_SLOT_2
Expand All @@ -86,36 +170,7 @@ TEST(RCUpdateTest, ModeSlotSwitchAllValues)
checkModeSlotSwitch(1.f, 6); // manual_control_switches_s::MODE_SLOT_6
}

void checkModeSlotButton(uint8_t button_configuration, uint8_t channel, float channel_value, uint8_t expected_slot)
{
RCUpdate rc_update;

// GIVEN: Buttons are configured
rc_update._param_rc_map_fltm_btn.set(button_configuration);
EXPECT_EQ(rc_update._param_rc_map_fltm_btn.get(), button_configuration);
// GIVEN: buttons are mapped
rc_update.update_rc_functions();
// GIVEN: First channel has some value
rc_update._rc.channels[channel - 1] = channel_value;

// WHEN: we update the switches 4 times:
// - initiate the button press
// - keep the same button pressed
// - hold the button for 50ms
// - pass the simple outlier protection
rc_update.UpdateManualSwitches(0);
rc_update.UpdateManualSwitches(0);
rc_update.UpdateManualSwitches(51_ms);
rc_update.UpdateManualSwitches(51_ms);

// THEN: we receive the expected mode slot
uORB::SubscriptionData<manual_control_switches_s> manual_control_switches_sub{ORB_ID(manual_control_switches)};
manual_control_switches_sub.update();

EXPECT_EQ(manual_control_switches_sub.get().mode_slot, expected_slot);
}

TEST(RCUpdateTest, ModeSlotButtonAllValues)
TEST_F(RCUpdateTest, ModeSlotButtonAllValues)
{
checkModeSlotButton(1, 1, -1.f, 0); // button not pressed -> manual_control_switches_s::MODE_SLOT_NONE
checkModeSlotButton(1, 1, 0.f, 0); // button not pressed over threshold -> manual_control_switches_s::MODE_SLOT_NONE
Expand All @@ -131,3 +186,60 @@ TEST(RCUpdateTest, ModeSlotButtonAllValues)
checkModeSlotButton(31, 6, 1.f, 0); // button 6 pressed but not configured -> manual_control_switches_s::MODE_SLOT_NONE
checkModeSlotButton(63, 6, 1.f, 6); // button 6 pressed -> manual_control_switches_s::MODE_SLOT_6
}

TEST_F(RCUpdateTest, ReturnSwitchUnassigned)
{
// GIVEN: Default configuration with no assigned return switch
EXPECT_EQ(_param_rc_map_return_sw.get(), 0);

// WHEN: we update the switches two times to pass the simple outlier protection
_rc_update.UpdateManualSwitches(0);
_rc_update.UpdateManualSwitches(0);

// THEN: we receive an unmapped return switch state
uORB::SubscriptionData<manual_control_switches_s> manual_control_switches_sub{ORB_ID(manual_control_switches)};
manual_control_switches_sub.update();

EXPECT_EQ(manual_control_switches_sub.get().return_switch, 0); // manual_control_switches_s::SWITCH_POS_NONE
}

TEST_F(RCUpdateTest, ReturnSwitchPositiveThresholds)
{
checkReturnSwitch(-1.f, 0.5f, 3); // Below threshold -> SWITCH_POS_OFF
checkReturnSwitch(0.f, 0.5f, 3); // On threshold -> SWITCH_POS_OFF
checkReturnSwitch(.001f, 0.5f, 1); // Slightly above threshold -> SWITCH_POS_ON
checkReturnSwitch(1.f, 0.5f, 1); // Above threshold -> SWITCH_POS_ON

checkReturnSwitch(-1.f, 0.75f, 3); // Below threshold -> SWITCH_POS_OFF
checkReturnSwitch(0.f, 0.75f, 3); // Below threshold -> SWITCH_POS_OFF
checkReturnSwitch(.5f, 0.75f, 3); // On threshold -> SWITCH_POS_OFF
checkReturnSwitch(.501f, 0.75f, 1); // Slightly above threshold -> SWITCH_POS_ON
checkReturnSwitch(1.f, 0.75f, 1); // Above threshold -> SWITCH_POS_ON

checkReturnSwitch(-1.f, 0.f, 3); // On minimum threshold -> SWITCH_POS_OFF
checkReturnSwitch(-.999f, 0.f, 1); // Slightly above minimum threshold -> SWITCH_POS_ON
checkReturnSwitch(1.f, 0.f, 1); // Above minimum threshold -> SWITCH_POS_ON

checkReturnSwitch(-1.f, 1.f, 3); // Below maximum threshold -> SWITCH_POS_OFF
checkReturnSwitch(1.f, 1.f, 3); // On maximum threshold -> SWITCH_POS_OFF
}

TEST_F(RCUpdateTest, ReturnSwitchNegativeThresholds)
{
checkReturnSwitch(1.f, -0.5f, 3); // Above threshold -> SWITCH_POS_OFF
checkReturnSwitch(0.f, -0.5f, 3); // On threshold -> SWITCH_POS_OFF
checkReturnSwitch(-.001f, -0.5f, 1); // Slightly below threshold -> SWITCH_POS_ON
checkReturnSwitch(-1.f, -0.5f, 1); // Below threshold -> SWITCH_POS_ON

checkReturnSwitch(1.f, -0.75f, 3); // Above threshold -> SWITCH_POS_OFF
checkReturnSwitch(.5f, -0.75f, 3); // On threshold -> SWITCH_POS_OFF
checkReturnSwitch(.499f, -0.75f, 1); // Slightly below threshold -> SWITCH_POS_ON
checkReturnSwitch(-1.f, -0.75f, 1); // Below threshold -> SWITCH_POS_ON

checkReturnSwitch(1.f, -1.f, 3); // On maximum threshold -> SWITCH_POS_OFF
checkReturnSwitch(.999f, -1.f, 1); // Slighly below maximum threshold -> SWITCH_POS_ON
checkReturnSwitch(-1.f, -1.f, 1); // Below minimum threshold -> SWITCH_POS_ON

checkReturnSwitch(1.f, -.001f, 3); // Above minimum threshold -> SWITCH_POS_OFF
checkReturnSwitch(-1.f, -.001f, 1); // Slightly below minimum threshold -> SWITCH_POS_OFF
}
46 changes: 22 additions & 24 deletions src/modules/rc_update/rc_update.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -101,7 +101,7 @@ RCUpdate::RCUpdate() :
}

rc_parameter_map_poll(true /* forced */);
parameters_updated();
updateParams(); // Call is needed to populate the _rc.function array

_button_pressed_hysteresis.set_hysteresis_time_from(false, 50_ms);
}
Expand All @@ -123,8 +123,10 @@ bool RCUpdate::init()
return true;
}

void RCUpdate::parameters_updated()
MaEtUgR marked this conversation as resolved.
Show resolved Hide resolved
void RCUpdate::updateParams()
{
ModuleParams::updateParams();

// rc values
for (unsigned int i = 0; i < RC_MAX_CHAN_COUNT; i++) {
float min = 0.f;
Expand Down Expand Up @@ -388,7 +390,6 @@ void RCUpdate::Run()

// update parameters from storage
updateParams();
parameters_updated();
}

rc_parameter_map_poll();
Expand Down Expand Up @@ -560,19 +561,16 @@ void RCUpdate::Run()
perf_end(_loop_perf);
}

switch_pos_t RCUpdate::get_rc_sw2pos_position(uint8_t func, float on_th) const
switch_pos_t RCUpdate::getRCSwitchOnOffPosition(uint8_t function, float threshold) const
{
if (_rc.function[func] >= 0) {
const bool on_inv = (on_th < 0.f);

const float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f;
if (_rc.function[function] >= 0) {
float value = 0.5f * _rc.channels[_rc.function[function]] + 0.5f; // Rescale [-1,1] -> [0,1] range

if (on_inv ? value < on_th : value > on_th) {
return manual_control_switches_s::SWITCH_POS_ON;

} else {
return manual_control_switches_s::SWITCH_POS_OFF;
if (threshold < 0.f) {
value = -value;
}

return (value > threshold) ? manual_control_switches_s::SWITCH_POS_ON : manual_control_switches_s::SWITCH_POS_OFF;
junwoo091400 marked this conversation as resolved.
Show resolved Hide resolved
}

return manual_control_switches_s::SWITCH_POS_NONE;
Expand Down Expand Up @@ -639,18 +637,18 @@ void RCUpdate::UpdateManualSwitches(const hrt_abstime &timestamp_sample)
}
}

switches.return_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_RETURN, _param_rc_return_th.get());
switches.loiter_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_LOITER, _param_rc_loiter_th.get());
switches.offboard_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_OFFBOARD, _param_rc_offb_th.get());
switches.kill_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_KILLSWITCH, _param_rc_killswitch_th.get());
switches.arm_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_ARMSWITCH, _param_rc_armswitch_th.get());
switches.transition_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_TRANSITION, _param_rc_trans_th.get());
switches.gear_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_GEAR, _param_rc_gear_th.get());
switches.engage_main_motor_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_ENGAGE_MAIN_MOTOR,
_param_rc_eng_mot_th.get());
switches.return_switch = getRCSwitchOnOffPosition(rc_channels_s::FUNCTION_RETURN, _param_rc_return_th.get());
switches.loiter_switch = getRCSwitchOnOffPosition(rc_channels_s::FUNCTION_LOITER, _param_rc_loiter_th.get());
switches.offboard_switch = getRCSwitchOnOffPosition(rc_channels_s::FUNCTION_OFFBOARD, _param_rc_offb_th.get());
switches.kill_switch = getRCSwitchOnOffPosition(rc_channels_s::FUNCTION_KILLSWITCH, _param_rc_killswitch_th.get());
switches.arm_switch = getRCSwitchOnOffPosition(rc_channels_s::FUNCTION_ARMSWITCH, _param_rc_armswitch_th.get());
switches.transition_switch = getRCSwitchOnOffPosition(rc_channels_s::FUNCTION_TRANSITION, _param_rc_trans_th.get());
switches.gear_switch = getRCSwitchOnOffPosition(rc_channels_s::FUNCTION_GEAR, _param_rc_gear_th.get());
switches.engage_main_motor_switch =
getRCSwitchOnOffPosition(rc_channels_s::FUNCTION_ENGAGE_MAIN_MOTOR, _param_rc_eng_mot_th.get());
#if defined(ATL_MANTIS_RC_INPUT_HACKS)
switches.photo_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_AUX_3, 0.5f);
switches.video_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_AUX_4, 0.5f);
switches.photo_switch = getRCSwitchOnOffPosition(rc_channels_s::FUNCTION_AUX_3, 0.5f);
switches.video_switch = getRCSwitchOnOffPosition(rc_channels_s::FUNCTION_AUX_4, 0.5f);
#endif

// last 2 switch updates identical within 1 second (simple protection from bad RC data)
Expand Down
12 changes: 7 additions & 5 deletions src/modules/rc_update/rc_update.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -89,6 +89,7 @@ class RCUpdate : public ModuleBase<RCUpdate>, public ModuleParams, public px4::W

int print_status() override;

protected:
static constexpr uint64_t VALID_DATA_MIN_INTERVAL_US{1_s / 3}; // assume valid RC input is at least 3 Hz

void Run() override;
Expand All @@ -109,23 +110,24 @@ class RCUpdate : public ModuleBase<RCUpdate>, public ModuleParams, public px4::W
/**
* Update our local parameter cache.
*/
void parameters_updated();
void updateParams() override;

/**
* Get and limit value for specified RC function. Returns NAN if not mapped.
*/
float get_rc_value(uint8_t func, float min_value, float max_value) const;

/**
* Get switch position for specified function.
* Get on/off switch position from the RC channel of the specified function
*
* @param function according to rc_channels_s::FUNCTION_XXX
* @param threshold according to RC_XXX_TH parameters, negative means on and off are flipped
*/
switch_pos_t get_rc_sw2pos_position(uint8_t func, float on_th) const;
switch_pos_t getRCSwitchOnOffPosition(uint8_t function, float threshold) const;

/**
* Update parameters from RC channels if the functionality is activated and the
* input has changed since the last update
*
* @param
*/
void set_params_from_rc();

Expand Down