fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-18 01:07:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensors: rc filter no unstable cutoff, better initialisation, reset filter on change, constrain output

Browse Source 
Filter gets unstable if cutoff is above sample rate/2.
Filter initial frequencies do not matter a lot because they get replaced by parameters anyways.
Filter delay values get reset to 0 when the filter is reconfigured otherwise there can be some weird spikes in the output.
Filter output gets constrained to the range again because of possible overshoot.
This commit is contained in:
Matthias Grob
2017-01-30 16:47:56 +01:00
committed by Lorenz Meier
parent 81dcba3a2a
commit 9e80a6c9d6
3 changed files with 24 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/parameters.cpp
+2 -1
View File
@@ -388,7 +388,8 @@ int update_parameters(const ParameterHandles &parameter_handles, Parameters &par
param_get(parameter_handles.rc_flt_smp_rate, &(parameters.rc_flt_smp_rate));
parameters.rc_flt_smp_rate = math::max(1.0f, parameters.rc_flt_smp_rate);
param_get(parameter_handles.rc_flt_cutoff, &(parameters.rc_flt_cutoff));
parameters.rc_flt_cutoff = math::max(1.0f, parameters.rc_flt_cutoff);
/* make sure the filter is in its stable region -> fc < fs/2 */
parameters.rc_flt_cutoff = math::constrain(parameters.rc_flt_cutoff, 0.1f, (parameters.rc_flt_smp_rate / 2) - 1.f);
/* Airspeed offset */
param_get(parameter_handles.diff_pres_offset_pa, &(parameters.diff_pres_offset_pa));
src/modules/sensors/rc_update.cpp
+18 -11
View File
@@ -47,17 +47,14 @@
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/manual_control_setpoint.h>
#define RC_SAMPLING_RATE 33.3f
#define RC_FILTER_LP_CUTOFF 5.0f
using namespace sensors;
RCUpdate::RCUpdate(const Parameters &parameters)
: _parameters(parameters),
_filter_roll(RC_SAMPLING_RATE, RC_FILTER_LP_CUTOFF),
_filter_pitch(RC_SAMPLING_RATE, RC_FILTER_LP_CUTOFF),
_filter_yaw(RC_SAMPLING_RATE, RC_FILTER_LP_CUTOFF),
_filter_throttle(RC_SAMPLING_RATE, RC_FILTER_LP_CUTOFF)
_filter_roll(50.0f, 10.f), /* get replaced by parameter */
_filter_pitch(50.0f, 10.f),
_filter_yaw(50.0f, 10.f),
_filter_throttle(50.0f, 10.f)
{
memset(&_rc, 0, sizeof(_rc));
memset(&_rc_parameter_map, 0, sizeof(_rc_parameter_map));
@@ -124,6 +121,10 @@ void RCUpdate::update_rc_functions()
_filter_pitch.set_cutoff_frequency(_parameters.rc_flt_smp_rate, _parameters.rc_flt_cutoff);
_filter_yaw.set_cutoff_frequency(_parameters.rc_flt_smp_rate, _parameters.rc_flt_cutoff);
_filter_throttle.set_cutoff_frequency(_parameters.rc_flt_smp_rate, _parameters.rc_flt_cutoff);
_filter_roll.reset(0.f);
_filter_pitch.reset(0.f);
_filter_yaw.reset(0.f);
_filter_throttle.reset(0.f);
}
void
@@ -365,10 +366,10 @@ RCUpdate::rc_poll(const ParameterHandles &parameter_handles)
manual.data_source = manual_control_setpoint_s::SOURCE_RC;
/* limit controls */
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.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.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);
@@ -376,6 +377,12 @@ RCUpdate::rc_poll(const ParameterHandles &parameter_handles)
manual.aux4 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4, -1.0, 1.0);
manual.aux5 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5, -1.0, 1.0);
/* filter controls */
manual.y = math::constrain(_filter_roll.apply(manual.y), -1.f, 1.f);
manual.x = math::constrain(_filter_pitch.apply(manual.x), -1.f, 1.f);
manual.r = math::constrain(_filter_yaw.apply(manual.r), -1.f, 1.f);
manual.z = math::constrain(_filter_throttle.apply(manual.z), 0.f, 1.f);
if (_parameters.rc_map_flightmode > 0) {
/* the number of valid slots equals the index of the max marker minus one */
src/modules/sensors/sensor_params.c
+4 -2
View File
@@ -3358,8 +3358,10 @@ PARAM_DEFINE_FLOAT(RC_FLT_SMP_RATE, 50.0f);
/**
* Cutoff frequency for the low pass filter on roll,pitch, yaw and throttle
*
* @min 1.0
* Does not get set unless below RC_FLT_SMP_RATE/2 because of filter instability characteristics.
*
* @min 0.1
* @unit Hz
* @group Radio Calibration
*/
PARAM_DEFINE_FLOAT(RC_FLT_CUTOFF, 5.0f);
PARAM_DEFINE_FLOAT(RC_FLT_CUTOFF, 10.0f);