diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 2462ad4c56..d82e4b41e4 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -1307,10 +1307,10 @@ const clivalue_t valueTable[] = { #ifdef USE_RPM_FILTER { "gyro_rpm_notch_harmonics", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 3 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, gyro_rpm_notch_harmonics) }, - { "gyro_rpm_notch_q", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 100 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, gyro_rpm_notch_q) }, + { "gyro_rpm_notch_q", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1, 1000 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, gyro_rpm_notch_q) }, { "gyro_rpm_notch_min", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 50, 200 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, gyro_rpm_notch_min) }, { "dterm_rpm_notch_harmonics", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 3 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, dterm_rpm_notch_harmonics) }, - { "dterm_rpm_notch_q", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 100 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, dterm_rpm_notch_q) }, + { "dterm_rpm_notch_q", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1, 1000 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, dterm_rpm_notch_q) }, { "dterm_rpm_notch_min", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 50, 200 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, dterm_rpm_notch_min) }, #endif }; diff --git a/src/main/sensors/rpm_filter.c b/src/main/sensors/rpm_filter.c index 781b432786..b003199dba 100644 --- a/src/main/sensors/rpm_filter.c +++ b/src/main/sensors/rpm_filter.c @@ -61,8 +61,7 @@ static rpmNotchFilter_t filters[2]; static rpmNotchFilter_t* gyroFilter; static rpmNotchFilter_t* dtermFilter; - -PG_REGISTER_WITH_RESET_FN(rpmFilterConfig_t, rpmFilterConfig, PG_RPM_FILTER_CONFIG, 1); +PG_REGISTER_WITH_RESET_FN(rpmFilterConfig_t, rpmFilterConfig, PG_RPM_FILTER_CONFIG, 2); void pgResetFn_rpmFilterConfig(rpmFilterConfig_t *config) { @@ -79,7 +78,7 @@ static void rpmNotchFilterInit(rpmNotchFilter_t* filter, int harmonics, int minH { filter->harmonics = harmonics; filter->minHz = minHz; - filter->q = q / 10.0f; + filter->q = q / 100.0f; filter->loopTime = looptime; for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { @@ -94,8 +93,13 @@ static void rpmNotchFilterInit(rpmNotchFilter_t* filter, int harmonics, int minH void rpmFilterInit(const rpmFilterConfig_t *config) { - pidLooptime = gyro.targetLooptime * pidConfig()->pid_process_denom; numberRpmNotchFilters = 0; + if (!motorConfig()->dev.useDshotTelemetry) { + gyroFilter = dtermFilter = NULL; + return; + } + + pidLooptime = gyro.targetLooptime * pidConfig()->pid_process_denom; if (config->gyro_rpm_notch_harmonics) { gyroFilter = &filters[numberRpmNotchFilters++]; rpmNotchFilterInit(gyroFilter, config->gyro_rpm_notch_harmonics, @@ -108,7 +112,7 @@ void rpmFilterInit(const rpmFilterConfig_t *config) } for (int i = 0; i < getMotorCount(); i++) { - pt1FilterInit(&rpmFilters[i], pt1FilterGain(RPM_MOTOR_FILTER_CUTOFF, gyro.targetLooptime)); + pt1FilterInit(&rpmFilters[i], pt1FilterGain(RPM_MOTOR_FILTER_CUTOFF, pidLooptime)); } erpmToHz = ERPM_PER_LSB / SECONDS_PER_MINUTE / (motorConfig()->motorPoleCount / 2.0f); diff --git a/src/main/sensors/rpm_filter.h b/src/main/sensors/rpm_filter.h index cef92f2c78..5732e5552d 100644 --- a/src/main/sensors/rpm_filter.h +++ b/src/main/sensors/rpm_filter.h @@ -27,11 +27,11 @@ typedef struct rpmFilterConfig_s { uint8_t gyro_rpm_notch_harmonics; // how many harmonics should be covered with notches? 0 means filter off uint8_t gyro_rpm_notch_min; // minimum frequency of the notches - uint8_t gyro_rpm_notch_q; // q of the notches + uint16_t gyro_rpm_notch_q; // q of the notches - uint8_t dterm_rpm_notch_harmonics; // how many harmonics should be covered with notches? 0 means filter off - uint8_t dterm_rpm_notch_min; // minimum frequency of the notches - uint8_t dterm_rpm_notch_q; // q of the notches + uint8_t dterm_rpm_notch_harmonics; // how many harmonics should be covered with notches? 0 means filter off + uint8_t dterm_rpm_notch_min; // minimum frequency of the notches + uint16_t dterm_rpm_notch_q; // q of the notches } rpmFilterConfig_t;