From 931c982a3bdd67714911cad295bd6d0f83446997 Mon Sep 17 00:00:00 2001 From: Jan Post Date: Sat, 12 Nov 2022 10:39:21 +0100 Subject: [PATCH] Refactor RPM Filter (#11765) * Refactor RPM Filter * Refactor RPM Filter parameter group --- src/main/fc/core.c | 2 +- src/main/flight/mixer.c | 4 +- src/main/flight/pid_init.c | 2 +- src/main/flight/rpm_filter.c | 253 ++++++++++++---------------- src/main/flight/rpm_filter.h | 24 +-- src/main/pg/rpm_filter.c | 40 +++++ src/main/pg/rpm_filter.h | 38 +++++ src/main/sensors/gyro_filter_impl.c | 2 +- 8 files changed, 194 insertions(+), 171 deletions(-) create mode 100644 src/main/pg/rpm_filter.c create mode 100644 src/main/pg/rpm_filter.h diff --git a/src/main/fc/core.c b/src/main/fc/core.c index e4b7b5ddd9..dcd9eef608 100644 --- a/src/main/fc/core.c +++ b/src/main/fc/core.c @@ -361,7 +361,7 @@ void updateArmingStatus(void) #ifdef USE_RPM_FILTER // USE_RPM_FILTER will only be defined if USE_DSHOT and USE_DSHOT_TELEMETRY are defined - // If the RPM filter is anabled and any motor isn't providing telemetry, then disable arming + // If the RPM filter is enabled and any motor isn't providing telemetry, then disable arming if (isRpmFilterEnabled() && !isDshotTelemetryActive()) { setArmingDisabled(ARMING_DISABLED_RPMFILTER); } else { diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index f2a6ea3497..a02f78ee7b 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -222,8 +222,8 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs) #ifdef USE_DYN_IDLE if (mixerRuntime.dynIdleMinRps > 0.0f) { const float maxIncrease = isAirmodeActivated() ? mixerRuntime.dynIdleMaxIncrease : 0.05f; - float minRps = rpmMinMotorFrequency(); - DEBUG_SET(DEBUG_DYN_IDLE, 3, lrintf(minRps * 10)); + float minRps = getMinMotorFrequency(); + DEBUG_SET(DEBUG_DYN_IDLE, 3, lrintf(minRps * 10.0f)); float rpsError = mixerRuntime.dynIdleMinRps - minRps; // PT1 type lowpass delay and smoothing for D minRps = mixerRuntime.prevMinRps + mixerRuntime.minRpsDelayK * (minRps - mixerRuntime.prevMinRps); diff --git a/src/main/flight/pid_init.c b/src/main/flight/pid_init.c index 658cb9f7b1..ca85fa613a 100644 --- a/src/main/flight/pid_init.c +++ b/src/main/flight/pid_init.c @@ -251,7 +251,7 @@ void pidInit(const pidProfile_t *pidProfile) pidInitFilters(pidProfile); pidInitConfig(pidProfile); #ifdef USE_RPM_FILTER - rpmFilterInit(rpmFilterConfig()); + rpmFilterInit(rpmFilterConfig(), gyro.targetLooptime); #endif } diff --git a/src/main/flight/rpm_filter.c b/src/main/flight/rpm_filter.c index 48b227e32a..3c9c70b034 100644 --- a/src/main/flight/rpm_filter.c +++ b/src/main/flight/rpm_filter.c @@ -19,18 +19,18 @@ */ +#include #include -#include +#include #include "platform.h" -#if defined(USE_RPM_FILTER) +#ifdef USE_RPM_FILTER #include "build/debug.h" #include "common/filter.h" #include "common/maths.h" -#include "common/time.h" #include "drivers/dshot.h" @@ -45,200 +45,155 @@ #include "rpm_filter.h" -#define RPM_FILTER_MAXHARMONICS 3 -#define SECONDS_PER_MINUTE 60.0f -#define ERPM_PER_LSB 100.0f -#define MIN_UPDATE_T 0.001f +#define RPM_FILTER_HARMONICS_MAX 3 +#define RPM_FILTER_DURATION_S 0.001f // Maximum duration allowed to update all RPM notches once +#define SECONDS_PER_MINUTE 60.0f +#define ERPM_PER_LSB 100.0f -static pt1Filter_t rpmFilters[MAX_SUPPORTED_MOTORS]; +typedef struct rpmFilter_s { -typedef struct rpmNotchFilter_s { + int numHarmonics; + float minHz; + float maxHz; + float fadeRangeHz; + float q; - uint8_t harmonics; - float minHz; - float maxHz; - float fadeRangeHz; - float q; timeUs_t looptimeUs; + biquadFilter_t notch[XYZ_AXIS_COUNT][MAX_SUPPORTED_MOTORS][RPM_FILTER_HARMONICS_MAX]; - biquadFilter_t notch[XYZ_AXIS_COUNT][MAX_SUPPORTED_MOTORS][RPM_FILTER_MAXHARMONICS]; +} rpmFilter_t; -} rpmNotchFilter_t; +// Singleton +FAST_DATA_ZERO_INIT static rpmFilter_t rpmFilter; -FAST_DATA_ZERO_INIT static float erpmToHz; -FAST_DATA_ZERO_INIT static float filteredMotorErpm[MAX_SUPPORTED_MOTORS]; -FAST_DATA_ZERO_INIT static float motorFrequency[MAX_SUPPORTED_MOTORS]; -FAST_DATA_ZERO_INIT static float minMotorFrequency; -FAST_DATA_ZERO_INIT static uint8_t numberFilters; -FAST_DATA_ZERO_INIT static uint8_t numberRpmNotchFilters; -FAST_DATA_ZERO_INIT static uint8_t filterUpdatesPerIteration; -FAST_DATA_ZERO_INIT static float pidLooptime; -FAST_DATA_ZERO_INIT static rpmNotchFilter_t filters[2]; -FAST_DATA_ZERO_INIT static rpmNotchFilter_t *gyroFilter; +FAST_DATA_ZERO_INIT static pt1Filter_t motorFreqLpf[MAX_SUPPORTED_MOTORS]; +FAST_DATA_ZERO_INIT static float motorFrequencyHz[MAX_SUPPORTED_MOTORS]; +FAST_DATA_ZERO_INIT static float minMotorFrequencyHz; +FAST_DATA_ZERO_INIT static float erpmToHz; -FAST_DATA_ZERO_INIT static uint8_t currentMotor; -FAST_DATA_ZERO_INIT static uint8_t currentHarmonic; -FAST_DATA_ZERO_INIT static uint8_t currentFilterNumber; -FAST_DATA static rpmNotchFilter_t *currentFilter = &filters[0]; +// batch processing of RPM notches +FAST_DATA_ZERO_INIT static int notchUpdatesPerIteration; +FAST_DATA_ZERO_INIT static int motorIndex; +FAST_DATA_ZERO_INIT static int harmonicIndex; - -PG_REGISTER_WITH_RESET_FN(rpmFilterConfig_t, rpmFilterConfig, PG_RPM_FILTER_CONFIG, 5); - -void pgResetFn_rpmFilterConfig(rpmFilterConfig_t *config) +void rpmFilterInit(const rpmFilterConfig_t *config, const timeUs_t looptimeUs) { - config->rpm_filter_harmonics = 3; - config->rpm_filter_min_hz = 100; - config->rpm_filter_fade_range_hz = 50; - config->rpm_filter_q = 500; + motorIndex = 0; + harmonicIndex = 0; + minMotorFrequencyHz = 0; + rpmFilter.numHarmonics = 0; // disable RPM Filtering - config->rpm_filter_lpf_hz = 150; -} - -static void rpmNotchFilterInit(rpmNotchFilter_t *filter, const rpmFilterConfig_t *config, const timeUs_t looptimeUs) -{ - 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->looptimeUs, filter->q, FILTER_NOTCH, 0.0f); - } - } - } -} - -void rpmFilterInit(const rpmFilterConfig_t *config) -{ - currentFilter = &filters[0]; - currentMotor = currentHarmonic = currentFilterNumber = 0; - - numberRpmNotchFilters = 0; + // if bidirectional DShot is not available if (!motorConfig()->dev.useDshotTelemetry) { - gyroFilter = NULL; return; } - pidLooptime = gyro.targetLooptime; - if (config->rpm_filter_harmonics) { - gyroFilter = &filters[numberRpmNotchFilters++]; - rpmNotchFilterInit(gyroFilter, config, pidLooptime); - } else { - gyroFilter = NULL; + // init LPFs for RPM data + for (int i = 0; i < getMotorCount(); i++) { + pt1FilterInit(&motorFreqLpf[i], pt1FilterGain(config->rpm_filter_lpf_hz, looptimeUs * 1e-6f)); } - for (int i = 0; i < getMotorCount(); i++) { - pt1FilterInit(&rpmFilters[i], pt1FilterGain(config->rpm_filter_lpf_hz, pidLooptime * 1e-6f)); + // if RPM Filtering is configured to be off + if (!config->rpm_filter_harmonics) { + return; + } + + // if we get to this point, enable and init RPM filtering + rpmFilter.numHarmonics = config->rpm_filter_harmonics; + rpmFilter.minHz = config->rpm_filter_min_hz; + rpmFilter.maxHz = 0.48f * 1e6f / looptimeUs; // don't go quite to nyquist to avoid oscillations + rpmFilter.fadeRangeHz = config->rpm_filter_fade_range_hz; + rpmFilter.q = config->rpm_filter_q / 100.0f; + rpmFilter.looptimeUs = looptimeUs; + + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + for (int motor = 0; motor < getMotorCount(); motor++) { + for (int i = 0; i < rpmFilter.numHarmonics; i++) { + biquadFilterInit(&rpmFilter.notch[axis][motor][i], rpmFilter.minHz * i, rpmFilter.looptimeUs, rpmFilter.q, FILTER_NOTCH, 0.0f); + } + } } erpmToHz = ERPM_PER_LSB / SECONDS_PER_MINUTE / (motorConfig()->motorPoleCount / 2.0f); - const float loopIterationsPerUpdate = MIN_UPDATE_T / (pidLooptime * 1e-6f); - numberFilters = getMotorCount() * (filters[0].harmonics + filters[1].harmonics); - const float filtersPerLoopIteration = numberFilters / loopIterationsPerUpdate; - filterUpdatesPerIteration = lrintf(filtersPerLoopIteration + 0.49f); -} - -static float applyFilter(rpmNotchFilter_t *filter, const int axis, float value) -{ - if (filter == NULL) { - return value; - } - for (int motor = 0; motor < getMotorCount(); motor++) { - for (int i = 0; i < filter->harmonics; i++) { - value = biquadFilterApplyDF1Weighted(&filter->notch[axis][motor][i], value); - } - } - return value; -} - -float rpmFilterGyro(const int axis, float value) -{ - return applyFilter(gyroFilter, axis, value); + const float loopIterationsPerUpdate = RPM_FILTER_DURATION_S / (looptimeUs * 1e-6f); + const float numNotchesPerAxis = getMotorCount() * rpmFilter.numHarmonics; + notchUpdatesPerIteration = ceilf(numNotchesPerAxis / loopIterationsPerUpdate); // round to ceiling } FAST_CODE_NOINLINE void rpmFilterUpdate(void) { - for (int motor = 0; motor < getMotorCount(); motor++) { - filteredMotorErpm[motor] = pt1FilterApply(&rpmFilters[motor], getDshotTelemetry(motor)); - if (motor < 4) { - DEBUG_SET(DEBUG_RPM_FILTER, motor, motorFrequency[motor]); - } - motorFrequency[motor] = erpmToHz * filteredMotorErpm[motor]; - } - - if (gyroFilter == NULL) { - minMotorFrequency = 0.0f; + if (!motorConfig()->dev.useDshotTelemetry) { return; } - for (int i = 0; i < filterUpdatesPerIteration; i++) { + // update motor RPM data + minMotorFrequencyHz = FLT_MAX; + for (int motor = 0; motor < getMotorCount(); motor++) { + motorFrequencyHz[motor] = pt1FilterApply(&motorFreqLpf[motor], erpmToHz * getDshotTelemetry(motor)); + minMotorFrequencyHz = MIN(minMotorFrequencyHz, motorFrequencyHz[motor]); + if (motor < 4) { + DEBUG_SET(DEBUG_RPM_FILTER, motor, motorFrequencyHz[motor]); + } + } - float frequency = constrainf( - (currentHarmonic + 1) * motorFrequency[currentMotor], currentFilter->minHz, currentFilter->maxHz); - biquadFilter_t *template = ¤tFilter->notch[0][currentMotor][currentHarmonic]; - // uncomment below to debug filter stepping. Need to also comment out motor rpm DEBUG_SET above - /* DEBUG_SET(DEBUG_RPM_FILTER, 0, harmonic); */ - /* DEBUG_SET(DEBUG_RPM_FILTER, 1, motor); */ - /* DEBUG_SET(DEBUG_RPM_FILTER, 2, currentFilter == &gyroFilter); */ - /* DEBUG_SET(DEBUG_RPM_FILTER, 3, frequency) */ + if (!isRpmFilterEnabled()) { + return; + } + + // update RPM notches + for (int i = 0; i < notchUpdatesPerIteration; i++) { + + // select current notch on ROLL + biquadFilter_t *template = &rpmFilter.notch[0][motorIndex][harmonicIndex]; + + const float frequencyHz = constrainf((harmonicIndex + 1) * motorFrequencyHz[motorIndex], rpmFilter.minHz, rpmFilter.maxHz); + const float marginHz = frequencyHz - rpmFilter.minHz; // fade out notch when approaching minHz (turn it off) float weight = 1.0f; - if (frequency < currentFilter->minHz + currentFilter->fadeRangeHz) { - weight = (frequency - currentFilter->minHz) / currentFilter->fadeRangeHz; + if (marginHz < rpmFilter.fadeRangeHz) { + weight = marginHz / rpmFilter.fadeRangeHz; } - biquadFilterUpdate( - template, frequency, currentFilter->looptimeUs, currentFilter->q, FILTER_NOTCH, weight); + // update notch + biquadFilterUpdate(template, frequencyHz, rpmFilter.looptimeUs, rpmFilter.q, FILTER_NOTCH, weight); + // copy notch properties to corresponding notches (clones) on PITCH and YAW for (int axis = 1; axis < XYZ_AXIS_COUNT; axis++) { - biquadFilter_t *clone = ¤tFilter->notch[axis][currentMotor][currentHarmonic]; - clone->b0 = template->b0; - clone->b1 = template->b1; - clone->b2 = template->b2; - clone->a1 = template->a1; - clone->a2 = template->a2; - clone->weight = template->weight; + biquadFilter_t *clone = &rpmFilter.notch[axis][motorIndex][harmonicIndex]; + memcpy(clone, template, sizeof(biquadFilter_t)); } - if (++currentHarmonic == currentFilter->harmonics) { - currentHarmonic = 0; - if (++currentFilterNumber == numberRpmNotchFilters) { - currentFilterNumber = 0; - if (++currentMotor == getMotorCount()) { - currentMotor = 0; - } - minMotorFrequency = 0.0f; - } - currentFilter = &filters[currentFilterNumber]; + // cycle through all notches on ROLL (takes RPM_FILTER_DURATION_S at max.) + harmonicIndex = (harmonicIndex + 1) % rpmFilter.numHarmonics; + if (harmonicIndex == 0) { + motorIndex = (motorIndex + 1) % getMotorCount(); } } } +FAST_CODE float rpmFilterApply(const int axis, float value) +{ + for (int i = 0; i < rpmFilter.numHarmonics; i++) { + for (int motor = 0; motor < getMotorCount(); motor++) { + value = biquadFilterApplyDF1Weighted(&rpmFilter.notch[axis][motor][i], value); + } + } + + return value; +} + bool isRpmFilterEnabled(void) { - return (motorConfig()->dev.useDshotTelemetry && rpmFilterConfig()->rpm_filter_harmonics); + return rpmFilter.numHarmonics > 0; } -float rpmMinMotorFrequency(void) +float getMinMotorFrequency(void) { - if (minMotorFrequency == 0.0f) { - minMotorFrequency = 10000.0f; // max RPM reported in Hz = 600,000RPM - for (int i = getMotorCount(); i--;) { - if (motorFrequency[i] < minMotorFrequency) { - minMotorFrequency = motorFrequency[i]; - } - } - } - return minMotorFrequency; + return minMotorFrequencyHz; } -#endif +#endif // USE_RPM_FILTER diff --git a/src/main/flight/rpm_filter.h b/src/main/flight/rpm_filter.h index 96ff059297..dbf17f00a9 100644 --- a/src/main/flight/rpm_filter.h +++ b/src/main/flight/rpm_filter.h @@ -20,24 +20,14 @@ #pragma once -#include "common/axis.h" -#include "pg/pg.h" +#include -typedef struct rpmFilterConfig_s -{ - 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 +#include "common/time.h" - uint16_t rpm_filter_lpf_hz; // the cutoff of the lpf on reported motor rpm +#include "pg/rpm_filter.h" -} rpmFilterConfig_t; - -PG_DECLARE(rpmFilterConfig_t, rpmFilterConfig); - -void rpmFilterInit(const rpmFilterConfig_t *config); -float rpmFilterGyro(const int axis, float value); -void rpmFilterUpdate(void); +void rpmFilterInit(const rpmFilterConfig_t *config, const timeUs_t looptimeUs); +void rpmFilterUpdate(void); +float rpmFilterApply(const int axis, float value); bool isRpmFilterEnabled(void); -float rpmMinMotorFrequency(void); +float getMinMotorFrequency(void); diff --git a/src/main/pg/rpm_filter.c b/src/main/pg/rpm_filter.c new file mode 100644 index 0000000000..15dd1753e1 --- /dev/null +++ b/src/main/pg/rpm_filter.c @@ -0,0 +1,40 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include "platform.h" + +#ifdef USE_RPM_FILTER + +#include "pg/pg.h" +#include "pg/pg_ids.h" + +#include "rpm_filter.h" + +PG_REGISTER_WITH_RESET_TEMPLATE(rpmFilterConfig_t, rpmFilterConfig, PG_RPM_FILTER_CONFIG, 5); + +PG_RESET_TEMPLATE(rpmFilterConfig_t, rpmFilterConfig, + .rpm_filter_harmonics = 3, + .rpm_filter_min_hz = 100, + .rpm_filter_fade_range_hz = 50, + .rpm_filter_q = 500, + .rpm_filter_lpf_hz = 150 +); + +#endif // USE_RPM_FILTER diff --git a/src/main/pg/rpm_filter.h b/src/main/pg/rpm_filter.h new file mode 100644 index 0000000000..cd3f5008c4 --- /dev/null +++ b/src/main/pg/rpm_filter.h @@ -0,0 +1,38 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#pragma once + +#include + +#include "pg/pg.h" + +typedef struct rpmFilterConfig_s +{ + 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 + +} rpmFilterConfig_t; + +PG_DECLARE(rpmFilterConfig_t, rpmFilterConfig); diff --git a/src/main/sensors/gyro_filter_impl.c b/src/main/sensors/gyro_filter_impl.c index 58d7006ba2..f5992e0110 100644 --- a/src/main/sensors/gyro_filter_impl.c +++ b/src/main/sensors/gyro_filter_impl.c @@ -50,7 +50,7 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(void) GYRO_FILTER_AXIS_DEBUG_SET(axis, DEBUG_GYRO_SAMPLE, 1, lrintf(gyroADCf)); #ifdef USE_RPM_FILTER - gyroADCf = rpmFilterGyro(axis, gyroADCf); + gyroADCf = rpmFilterApply(axis, gyroADCf); #endif // DEBUG_GYRO_SAMPLE(2) Record the post-RPM Filter value for the selected debug axis