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 cca59843cc
commit ecd1e9f730
2 changed files with 23 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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 &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

@ -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();