From ecd1e9f730d492f7b3df88f0b335c93f4174fa33 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 3 Jul 2023 18:16:35 +0200 Subject: [PATCH] rc_update: fix on-off-switch with negative threshold values --- src/modules/rc_update/rc_update.cpp | 39 +++++++++++++---------------- src/modules/rc_update/rc_update.h | 9 ++++--- 2 files changed, 23 insertions(+), 25 deletions(-) diff --git a/src/modules/rc_update/rc_update.cpp b/src/modules/rc_update/rc_update.cpp index bb764901d3..6a706035c9 100644 --- a/src/modules/rc_update/rc_update.cpp +++ b/src/modules/rc_update/rc_update.cpp @@ -555,19 +555,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; @@ -634,18 +631,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 309c67af06..f660bfa38c 100644 --- a/src/modules/rc_update/rc_update.h +++ b/src/modules/rc_update/rc_update.h @@ -117,15 +117,16 @@ public: 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();