diff --git a/src/modules/sensors/parameters.cpp b/src/modules/sensors/parameters.cpp index 2fbcc6e8ed..8427d83a98 100644 --- a/src/modules/sensors/parameters.cpp +++ b/src/modules/sensors/parameters.cpp @@ -388,7 +388,8 @@ int update_parameters(const ParameterHandles ¶meter_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)); diff --git a/src/modules/sensors/rc_update.cpp b/src/modules/sensors/rc_update.cpp index 7f20d6cd3c..7c649b0311 100644 --- a/src/modules/sensors/rc_update.cpp +++ b/src/modules/sensors/rc_update.cpp @@ -47,17 +47,14 @@ #include #include -#define RC_SAMPLING_RATE 33.3f -#define RC_FILTER_LP_CUTOFF 5.0f - using namespace sensors; RCUpdate::RCUpdate(const Parameters ¶meters) : _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 ¶meter_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 ¶meter_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 */ diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 1c3bcd5527..2bea85bf1b 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -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);