sensors: rc added lowpass filters to the 4 main channels without usef…

…ul samplerate and cutoff frequency yet
PX4 · Jan 26, 2017 · 6dc1988 · 6dc1988
1 parent 7eada73
commit 6dc1988
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 5 deletions.
diff --git 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 &parameters)
-	: _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 &parameter_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
@@ -43,6 +43,7 @@
 
 #include <drivers/drv_hrt.h>
 #include <mathlib/mathlib.h>
+#include <mathlib/math/filter/LowPassFilter2p.hpp>
 
 namespace sensors
 {
@@ -125,6 +126,11 @@ class RCUpdate
 
 	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;
+
 };