From 27a4ce26918a12ab8fce570303728051592c2010 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 26 Jan 2017 08:07:10 +0100 Subject: [PATCH] sensors: rc added lowpass filters to the 4 main channels without useful samplerate and cutoff frequency yet --- src/modules/sensors/rc_update.cpp | 14 +++++++++----- src/modules/sensors/rc_update.h | 6 ++++++ 2 files changed, 15 insertions(+), 5 deletions(-) diff --git a/src/modules/sensors/rc_update.cpp b/src/modules/sensors/rc_update.cpp index 430206d740..bf14ea7e55 100644 --- a/src/modules/sensors/rc_update.cpp +++ b/src/modules/sensors/rc_update.cpp @@ -50,7 +50,11 @@ using namespace sensors; RCUpdate::RCUpdate(const Parameters ¶meters) - : _parameters(parameters) + : _parameters(parameters), + _filter_roll(0, 0), + _filter_pitch(0, 0), + _filter_yaw(0, 0), + _filter_throttle(0, 0) { memset(&_rc, 0, sizeof(_rc)); memset(&_rc_parameter_map, 0, sizeof(_rc_parameter_map)); @@ -352,10 +356,10 @@ RCUpdate::rc_poll(const ParameterHandles ¶meter_handles) manual.data_source = manual_control_setpoint_s::SOURCE_RC; /* limit controls */ - manual.y = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_ROLL, -1.0, 1.0); - manual.x = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_PITCH, -1.0, 1.0); - manual.r = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_YAW, -1.0, 1.0); - manual.z = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE, 0.0, 1.0); + manual.y = _filter_roll.apply(get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_ROLL, -1.0, 1.0)); + manual.x = _filter_pitch.apply(get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_PITCH, -1.0, 1.0)); + manual.r = _filter_yaw.apply(get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_YAW, -1.0, 1.0)); + manual.z = _filter_throttle.apply(get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE, 0.0, 1.0)); manual.flaps = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS, -1.0, 1.0); manual.aux1 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1, -1.0, 1.0); manual.aux2 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2, -1.0, 1.0); diff --git a/src/modules/sensors/rc_update.h b/src/modules/sensors/rc_update.h index bcf2adbf64..aeade4c833 100644 --- a/src/modules/sensors/rc_update.h +++ b/src/modules/sensors/rc_update.h @@ -43,6 +43,7 @@ #include #include +#include namespace sensors { @@ -125,6 +126,11 @@ private: hrt_abstime _last_rc_to_param_map_time = 0; + math::LowPassFilter2p _filter_roll; /**< filters for the main 4 stick inputs */ + math::LowPassFilter2p _filter_pitch; /** we want smooth setpoints as inputs to the controllers */ + math::LowPassFilter2p _filter_yaw; + math::LowPassFilter2p _filter_throttle; + };