diff --git a/src/modules/rc_update/RCUpdateTest.cpp b/src/modules/rc_update/RCUpdateTest.cpp index a58a01d3dc..6a21a469a4 100644 --- a/src/modules/rc_update/RCUpdateTest.cpp +++ b/src/modules/rc_update/RCUpdateTest.cpp @@ -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 +} diff --git a/src/modules/rc_update/rc_update.cpp b/src/modules/rc_update/rc_update.cpp index 24d4ca51b0..6aa4ffd20b 100644 --- a/src/modules/rc_update/rc_update.cpp +++ b/src/modules/rc_update/rc_update.cpp @@ -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 ×tamp_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) diff --git a/src/modules/rc_update/rc_update.h b/src/modules/rc_update/rc_update.h index 16a6ed76ed..edd9e18dfc 100644 --- a/src/modules/rc_update/rc_update.h +++ b/src/modules/rc_update/rc_update.h @@ -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();