dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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

This commit is contained in:
Matthias Grob 2023-07-03 18:16:35 +02:00
parent 1fa5136e30
commit 42fa41e601
3 changed files with 43 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
src/modules/rc_update/RCUpdateTest.cpp
View File

@ -223,3 +223,23 @@ TEST_F(RCUpdateTest, ReturnSwitchPositiveThresholds)
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(-.001f, -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
}

39
src/modules/rc_update/rc_update.cpp
View File

@ -561,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);
if (_rc.function[function] >= 0) {
float value = 0.5f * _rc.channels[_rc.function[function]] + 0.5f; // Rescale [-1,1] -> [0,1] range
const float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f;
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;
}
return manual_control_switches_s::SWITCH_POS_NONE;
@ -640,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)

9
src/modules/rc_update/rc_update.h
View File

@ -118,15 +118,16 @@ protected:
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();