From 3ee3b54ad6c837ec77603248fda5b10161a28f83 Mon Sep 17 00:00:00 2001 From: KarateBrot Date: Fri, 10 Sep 2021 13:21:51 +0200 Subject: [PATCH] Change RPM filter minHz limit - Changed lower limit of minHz from 50 to 30 - Added fadeRangeHz to blackbox header - Renamed variables to fit BF coding convention - Renamed CLI commands respectively --- src/main/blackbox/blackbox.c | 9 ++++---- src/main/cli/settings.c | 10 ++++----- src/main/flight/rpm_filter.c | 41 ++++++++++++++++++------------------ src/main/flight/rpm_filter.h | 15 +++++++------ src/main/msp/msp.c | 8 +++---- 5 files changed, 43 insertions(+), 40 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index c71e03df2..749aeecbd 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1430,10 +1430,11 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE("motor_poles", "%d", motorConfig()->motorPoleCount); #endif #ifdef USE_RPM_FILTER - BLACKBOX_PRINT_HEADER_LINE("gyro_rpm_notch_harmonics", "%d", rpmFilterConfig()->gyro_rpm_notch_harmonics); - BLACKBOX_PRINT_HEADER_LINE("gyro_rpm_notch_q", "%d", rpmFilterConfig()->gyro_rpm_notch_q); - BLACKBOX_PRINT_HEADER_LINE("gyro_rpm_notch_min", "%d", rpmFilterConfig()->gyro_rpm_notch_min); - BLACKBOX_PRINT_HEADER_LINE("rpm_notch_lpf", "%d", rpmFilterConfig()->rpm_lpf); + BLACKBOX_PRINT_HEADER_LINE("rpm_filter_harmonics", "%d", rpmFilterConfig()->rpm_filter_harmonics); + BLACKBOX_PRINT_HEADER_LINE("rpm_filter_q", "%d", rpmFilterConfig()->rpm_filter_q); + BLACKBOX_PRINT_HEADER_LINE("rpm_filter_min_hz", "%d", rpmFilterConfig()->rpm_filter_min_hz); + BLACKBOX_PRINT_HEADER_LINE("rpm_filter_fade_range_hz", "%d", rpmFilterConfig()->rpm_filter_fade_range_hz); + BLACKBOX_PRINT_HEADER_LINE("rpm_filter_lpf_hz", "%d", rpmFilterConfig()->rpm_filter_lpf_hz); #endif #if defined(USE_ACC) BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz", "%d", (int)(accelerometerConfig()->acc_lpf_hz * 100.0f)); diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index e61768a83..3e4e689fd 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1656,11 +1656,11 @@ const clivalue_t valueTable[] = { #endif #ifdef USE_RPM_FILTER - { "gyro_rpm_notch_harmonics", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 3 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, gyro_rpm_notch_harmonics) }, - { "gyro_rpm_notch_q", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 250, 3000 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, gyro_rpm_notch_q) }, - { "gyro_rpm_notch_min", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 50, 200 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, gyro_rpm_notch_min) }, - { "gyro_rpm_notch_fade_range_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, gyro_rpm_notch_fade_range_hz) }, - { "rpm_notch_lpf", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 100, 500 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_lpf) }, + { "rpm_filter_harmonics", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 3 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_filter_harmonics) }, + { "rpm_filter_q", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 250, 3000 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_filter_q) }, + { "rpm_filter_min_hz", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 30, 200 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_filter_min_hz) }, + { "rpm_filter_fade_range_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_filter_fade_range_hz) }, + { "rpm_filter_lpf_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 100, 500 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_filter_lpf_hz) }, #endif #ifdef USE_RX_FLYSKY diff --git a/src/main/flight/rpm_filter.c b/src/main/flight/rpm_filter.c index a93f8cc05..242cc0623 100644 --- a/src/main/flight/rpm_filter.c +++ b/src/main/flight/rpm_filter.c @@ -30,6 +30,7 @@ #include "common/filter.h" #include "common/maths.h" +#include "common/time.h" #include "drivers/dshot.h" @@ -59,7 +60,7 @@ typedef struct rpmNotchFilter_s { float maxHz; float fadeRangeHz; float q; - uint32_t looptime; + timeUs_t looptimeUs; biquadFilter_t notch[XYZ_AXIS_COUNT][MAX_SUPPORTED_MOTORS][RPM_FILTER_MAXHARMONICS]; @@ -87,28 +88,28 @@ PG_REGISTER_WITH_RESET_FN(rpmFilterConfig_t, rpmFilterConfig, PG_RPM_FILTER_CONF void pgResetFn_rpmFilterConfig(rpmFilterConfig_t *config) { - config->gyro_rpm_notch_harmonics = 3; - config->gyro_rpm_notch_min = 100; - config->gyro_rpm_notch_fade_range_hz = 50; - config->gyro_rpm_notch_q = 500; + config->rpm_filter_harmonics = 3; + config->rpm_filter_min_hz = 100; + config->rpm_filter_fade_range_hz = 50; + config->rpm_filter_q = 500; - config->rpm_lpf = 150; + config->rpm_filter_lpf_hz = 150; } -static void rpmNotchFilterInit(rpmNotchFilter_t *filter, const rpmFilterConfig_t *config, const uint32_t looptime) +static void rpmNotchFilterInit(rpmNotchFilter_t *filter, const rpmFilterConfig_t *config, const timeUs_t looptimeUs) { - filter->harmonics = config->gyro_rpm_notch_harmonics; - filter->minHz = config->gyro_rpm_notch_min; - filter->maxHz = 0.48f * 1e6f / looptime; // don't go quite to nyquist to avoid oscillations - filter->fadeRangeHz = config->gyro_rpm_notch_fade_range_hz; - filter->q = config->gyro_rpm_notch_q / 100.0f; - filter->looptime = looptime; + filter->harmonics = config->rpm_filter_harmonics; + filter->minHz = config->rpm_filter_min_hz; + filter->maxHz = 0.48f * 1e6f / looptimeUs; // don't go quite to nyquist to avoid oscillations + filter->fadeRangeHz = config->rpm_filter_fade_range_hz; + filter->q = config->rpm_filter_q / 100.0f; + filter->looptimeUs = looptimeUs; for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int motor = 0; motor < getMotorCount(); motor++) { for (int i = 0; i < filter->harmonics; i++) { biquadFilterInit( - &filter->notch[axis][motor][i], filter->minHz * i, filter->looptime, filter->q, FILTER_NOTCH, 0.0f); + &filter->notch[axis][motor][i], filter->minHz * i, filter->looptimeUs, filter->q, FILTER_NOTCH, 0.0f); } } } @@ -126,7 +127,7 @@ void rpmFilterInit(const rpmFilterConfig_t *config) } pidLooptime = gyro.targetLooptime; - if (config->gyro_rpm_notch_harmonics) { + if (config->rpm_filter_harmonics) { gyroFilter = &filters[numberRpmNotchFilters++]; rpmNotchFilterInit(gyroFilter, config, pidLooptime); } else { @@ -134,7 +135,7 @@ void rpmFilterInit(const rpmFilterConfig_t *config) } for (int i = 0; i < getMotorCount(); i++) { - pt1FilterInit(&rpmFilters[i], pt1FilterGain(config->rpm_lpf, pidLooptime * 1e-6f)); + pt1FilterInit(&rpmFilters[i], pt1FilterGain(config->rpm_filter_lpf_hz, pidLooptime * 1e-6f)); } erpmToHz = ERPM_PER_LSB / SECONDS_PER_MINUTE / (motorConfig()->motorPoleCount / 2.0f); @@ -163,7 +164,7 @@ float rpmFilterGyro(const int axis, float value) return applyFilter(gyroFilter, axis, value); } -FAST_CODE_NOINLINE void rpmFilterUpdate() +FAST_CODE_NOINLINE void rpmFilterUpdate(void) { if (gyroFilter == NULL) { return; @@ -195,7 +196,7 @@ FAST_CODE_NOINLINE void rpmFilterUpdate() } biquadFilterUpdate( - template, frequency, currentFilter->looptime, currentFilter->q, FILTER_NOTCH, weight); + template, frequency, currentFilter->looptimeUs, currentFilter->q, FILTER_NOTCH, weight); for (int axis = 1; axis < XYZ_AXIS_COUNT; axis++) { biquadFilter_t *clone = ¤tFilter->notch[axis][currentMotor][currentHarmonic]; @@ -223,10 +224,10 @@ FAST_CODE_NOINLINE void rpmFilterUpdate() bool isRpmFilterEnabled(void) { - return (motorConfig()->dev.useDshotTelemetry && rpmFilterConfig()->gyro_rpm_notch_harmonics); + return (motorConfig()->dev.useDshotTelemetry && rpmFilterConfig()->rpm_filter_harmonics); } -float rpmMinMotorFrequency() +float rpmMinMotorFrequency(void) { if (minMotorFrequency == 0.0f) { minMotorFrequency = 10000.0f; diff --git a/src/main/flight/rpm_filter.h b/src/main/flight/rpm_filter.h index 476172c8e..96ff05929 100644 --- a/src/main/flight/rpm_filter.h +++ b/src/main/flight/rpm_filter.h @@ -25,18 +25,19 @@ 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 - uint16_t gyro_rpm_notch_fade_range_hz; // range in which to gradually turn off notches down to minHz - uint16_t gyro_rpm_notch_q; // q of the notches + uint8_t rpm_filter_harmonics; // how many harmonics should be covered with notches? 0 means filter off + uint8_t rpm_filter_min_hz; // minimum frequency of the notches + uint16_t rpm_filter_fade_range_hz; // range in which to gradually turn off notches down to minHz + uint16_t rpm_filter_q; // q of the notches + + uint16_t rpm_filter_lpf_hz; // the cutoff of the lpf on reported motor rpm - uint16_t rpm_lpf; // the cutoff of the lpf on reported motor rpm } rpmFilterConfig_t; PG_DECLARE(rpmFilterConfig_t, rpmFilterConfig); void rpmFilterInit(const rpmFilterConfig_t *config); float rpmFilterGyro(const int axis, float value); -void rpmFilterUpdate(); +void rpmFilterUpdate(void); bool isRpmFilterEnabled(void); -float rpmMinMotorFrequency(); +float rpmMinMotorFrequency(void); diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index cd4f7c5e3..e77163884 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -1792,8 +1792,8 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst) sbufWriteU16(dst, 0); #endif #if defined(USE_RPM_FILTER) - sbufWriteU8(dst, rpmFilterConfig()->gyro_rpm_notch_harmonics); - sbufWriteU8(dst, rpmFilterConfig()->gyro_rpm_notch_min); + sbufWriteU8(dst, rpmFilterConfig()->rpm_filter_harmonics); + sbufWriteU8(dst, rpmFilterConfig()->rpm_filter_min_hz); #else sbufWriteU8(dst, 0); sbufWriteU8(dst, 0); @@ -2675,8 +2675,8 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbufReadU16(src); #endif #if defined(USE_RPM_FILTER) - rpmFilterConfigMutable()->gyro_rpm_notch_harmonics = sbufReadU8(src); - rpmFilterConfigMutable()->gyro_rpm_notch_min = sbufReadU8(src); + rpmFilterConfigMutable()->rpm_filter_harmonics = sbufReadU8(src); + rpmFilterConfigMutable()->rpm_filter_min_hz = sbufReadU8(src); #else sbufReadU8(src); sbufReadU8(src);