diff --git a/src/modules/sensors/parameters.cpp b/src/modules/sensors/parameters.cpp index 1a4a358e1d..2fbcc6e8ed 100644 --- a/src/modules/sensors/parameters.cpp +++ b/src/modules/sensors/parameters.cpp @@ -125,6 +125,10 @@ int initialize_parameter_handles(ParameterHandles ¶meter_handles) parameter_handles.rc_trans_th = param_find("RC_TRANS_TH"); parameter_handles.rc_gear_th = param_find("RC_GEAR_TH"); + /* RC low pass filter configuration */ + parameter_handles.rc_flt_smp_rate = param_find("RC_FLT_SMP_RATE"); + parameter_handles.rc_flt_cutoff = param_find("RC_FLT_CUTOFF"); + /* Differential pressure offset */ parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF"); parameter_handles.diff_pres_analog_scale = param_find("SENS_DPRES_ANSC"); @@ -381,6 +385,11 @@ int update_parameters(const ParameterHandles ¶meter_handles, Parameters &par parameters.rc_gear_inv = (parameters.rc_gear_th < 0); parameters.rc_gear_th = fabs(parameters.rc_gear_th); + 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); + /* Airspeed offset */ param_get(parameter_handles.diff_pres_offset_pa, &(parameters.diff_pres_offset_pa)); param_get(parameter_handles.diff_pres_analog_scale, &(parameters.diff_pres_analog_scale)); diff --git a/src/modules/sensors/parameters.h b/src/modules/sensors/parameters.h index e43afdbc9d..b9bfd5d590 100644 --- a/src/modules/sensors/parameters.h +++ b/src/modules/sensors/parameters.h @@ -44,6 +44,7 @@ #include #include +#include #include @@ -125,6 +126,9 @@ struct Parameters { bool rc_trans_inv; bool rc_gear_inv; + float rc_flt_smp_rate; + float rc_flt_cutoff; + float battery_voltage_scaling; float battery_current_scaling; float battery_current_offset; @@ -196,6 +200,9 @@ struct ParameterHandles { param_t rc_trans_th; param_t rc_gear_th; + param_t rc_flt_smp_rate; + param_t rc_flt_cutoff; + param_t battery_voltage_scaling; param_t battery_current_scaling; param_t battery_current_offset; diff --git a/src/modules/sensors/rc_update.cpp b/src/modules/sensors/rc_update.cpp index fffc8ffa88..7f20d6cd3c 100644 --- a/src/modules/sensors/rc_update.cpp +++ b/src/modules/sensors/rc_update.cpp @@ -118,6 +118,12 @@ void RCUpdate::update_rc_functions() for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) { _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] = _parameters.rc_map_param[i] - 1; } + + /* update the RC low pass filter frequencies */ + _filter_roll.set_cutoff_frequency(_parameters.rc_flt_smp_rate, _parameters.rc_flt_cutoff); + _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); } void diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 9888c4c13e..1c3bcd5527 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -3343,3 +3343,23 @@ PARAM_DEFINE_FLOAT(THR_MDL_FAC, 0.0f); * @group PWM Outputs */ PARAM_DEFINE_FLOAT(MOT_SLEW_MAX, 0.0f); + +/** + * Sample rate of the remote control values for the low pass filter on roll,pitch, yaw and throttle + * + * Has an influence on the cutoff frequency precision. + * + * @min 1.0 + * @unit Hz + * @group Radio Calibration + */ +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 + * @unit Hz + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC_FLT_CUTOFF, 5.0f);