rpm telemetry based notch filter

make/source.mk

@@ -113,6 +113,7 @@ COMMON_SRC = \
             sensors/compass.c \
             sensors/gyro.c \
             sensors/gyroanalyse.c \
+            sensors/rpm_filter.c \
             sensors/initialisation.c \
             blackbox/blackbox.c \
             blackbox/blackbox_encoding.c \
@@ -239,6 +240,7 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \
             sensors/boardalignment.c \
             sensors/gyro.c \
             sensors/gyroanalyse.c \
+            sensors/rpm_filter.c \
             $(CMSIS_SRC) \
             $(DEVICE_STDPERIPH_SRC) \
 

src/main/build/debug.c

@@ -79,4 +79,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
     "DYN_LPF",
     "RX_SPEKTRUM_SPI",
     "DSHOT_TELEMETRY",
+    "RPM_FILTER",
 };

src/main/build/debug.h

@@ -97,6 +97,7 @@ typedef enum {
     DEBUG_DYN_LPF,
     DEBUG_RX_SPEKTRUM_SPI,
     DEBUG_RPM_TELEMETRY,
+    DEBUG_RPM_FILTER,
     DEBUG_COUNT
 } debugType_e;
 

src/main/drivers/pwm_output_dshot.c

@@ -51,7 +51,6 @@ static motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS];
 static motorDmaOutput_t dmaMotors[MAX_SUPPORTED_MOTORS];
 
 #ifdef USE_DSHOT_TELEMETRY
-//FAST_RAM_ZERO_INIT bool dshotTelemetryEnabled;
 uint32_t readDoneCount;
 
 // TODO remove once debugging no longer needed
@@ -150,14 +149,14 @@ static uint16_t decodeProshotPacket(uint32_t buffer[])
     for (int i = 1; i < PROSHOT_TELEMETRY_INPUT_LEN; i += 2) {
         const int proshotModulo = MOTOR_NIBBLE_LENGTH_PROSHOT;
         int diff = ((buffer[i] + proshotModulo - buffer[i-1]) % proshotModulo) - PROSHOT_BASE_SYMBOL;
-        int nible;
+        int nibble;
         if (diff < 0) {
-            nible = 0;
+            nibble = 0;
         } else {
-            nible = (diff + PROSHOT_BIT_WIDTH / 2) / PROSHOT_BIT_WIDTH;
+            nibble = (diff + PROSHOT_BIT_WIDTH / 2) / PROSHOT_BIT_WIDTH;
         }
         value <<= 4;
-        value |= nible;
+        value |= (nibble & 0xf);
     }
 
     uint32_t csum = value;
@@ -263,10 +262,8 @@ void pwmStartDshotMotorUpdate(uint8_t motorCount)
         for (int i = 0; i < motorCount; i++) {
             if (dmaMotors[i].hasTelemetry) {
                 uint16_t value = dmaMotors[i].useProshot ?
-                    decodeProshotPacket(
+                    decodeProshotPacket(dmaMotors[i].dmaBuffer) :
-                        dmaMotors[i].dmaBuffer ) :
+                    decodeDshotPacket(dmaMotors[i].dmaBuffer);
-                    decodeDshotPacket(
-                        dmaMotors[i].dmaBuffer );
                 if (value != 0xffff) {
                     dmaMotors[i].dshotTelemetryValue = value;
                     if (i < 4) {

src/main/fc/init.c

@@ -150,6 +150,7 @@
 #include "sensors/esc_sensor.h"
 #include "sensors/gyro.h"
 #include "sensors/initialisation.h"
+#include "sensors/rpm_filter.h"
 #include "sensors/sensors.h"
 
 #include "telemetry/telemetry.h"

src/main/flight/pid.c

@@ -56,6 +56,9 @@
 #include "sensors/gyro.h"
 #include "sensors/acceleration.h"
 
+#ifdef USE_RPM_FILTER
+#include "sensors/rpm_filter.h"
+#endif
 
 const char pidNames[] =
     "ROLL;"
@@ -604,6 +607,9 @@ void pidInit(const pidProfile_t *pidProfile)
     pidSetTargetLooptime(gyro.targetLooptime * pidConfig()->pid_process_denom); // Initialize pid looptime
     pidInitFilters(pidProfile);
     pidInitConfig(pidProfile);
+#ifdef USE_RPM_FILTER
+    rpmFilterInit(rpmFilterConfig());
+#endif
 }
 
 #ifdef USE_ACRO_TRAINER
@@ -1103,12 +1109,20 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
     // Precalculate gyro deta for D-term here, this allows loop unrolling
     float gyroRateDterm[XYZ_AXIS_COUNT];
     for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
-        gyroRateDterm[axis] = dtermNotchApplyFn((filter_t *) &dtermNotch[axis], gyro.gyroADCf[axis]);
+        gyroRateDterm[axis] = gyro.gyroADCf[axis];
+#ifdef USE_RPM_FILTER
+        gyroRateDterm[axis] = rpmFilterDterm(axis,gyroRateDterm[axis]);
+#endif
+        gyroRateDterm[axis] = dtermNotchApplyFn((filter_t *) &dtermNotch[axis], gyroRateDterm[axis]);
         gyroRateDterm[axis] = dtermLowpassApplyFn((filter_t *) &dtermLowpass[axis], gyroRateDterm[axis]);
         gyroRateDterm[axis] = dtermLowpass2ApplyFn((filter_t *) &dtermLowpass2[axis], gyroRateDterm[axis]);
     }
 
     rotateItermAndAxisError();
+#ifdef USE_RPM_FILTER
+    rpmFilterUpdate();
+#endif
+
 
     // ----------PID controller----------
     for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {

src/main/interface/settings.c

@@ -102,6 +102,7 @@
 #include "sensors/esc_sensor.h"
 #include "sensors/gyro.h"
 #include "sensors/rangefinder.h"
+#include "sensors/rpm_filter.h"
 
 #include "telemetry/frsky_hub.h"
 #include "telemetry/ibus_shared.h"
@@ -1299,6 +1300,15 @@ const clivalue_t valueTable[] = {
 #ifdef USE_RTC_TIME
     { "timezone_offset_minutes",  VAR_INT16 | MASTER_VALUE, .config.minmax = { TIMEZONE_OFFSET_MINUTES_MIN, TIMEZONE_OFFSET_MINUTES_MAX }, PG_TIME_CONFIG, offsetof(timeConfig_t, tz_offsetMinutes) },
 #endif
+
+#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_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_min",  VAR_UINT8 | MASTER_VALUE, .config.minmax = { 50, 200 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, dterm_rpm_notch_min) },
+#endif
 };
 
 const uint16_t valueTableEntryCount = ARRAYLEN(valueTable);

src/main/pg/pg_ids.h

@@ -138,7 +138,8 @@
 #define PG_MCO_CONFIG 541
 #define PG_RX_SPEKTRUM_SPI_CONFIG 542
 #define PG_SERIAL_UART_CONFIG 543
-#define PG_BETAFLIGHT_END 543
+#define PG_RPM_FILTER_CONFIG 544
+#define PG_BETAFLIGHT_END 544
 
 
 // OSD configuration (subject to change)

src/main/sensors/gyro.c

@@ -76,6 +76,9 @@
 #ifdef USE_GYRO_DATA_ANALYSE
 #include "sensors/gyroanalyse.h"
 #endif
+#ifdef USE_RPM_FILTER
+#include "sensors/rpm_filter.h"
+#endif
 #include "sensors/sensors.h"
 
 #if ((FLASH_SIZE > 128) && (defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_MPU6500)))

src/main/sensors/gyro_filter_impl.h

@@ -17,6 +17,11 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor)
         }
 #endif
 
+#ifdef USE_RPM_FILTER
+        gyroADCf = rpmFilterGyro(axis, gyroADCf);
+#endif
+
+
         // apply static notch filters and software lowpass filters
         gyroADCf = gyroSensor->notchFilter1ApplyFn((filter_t *)&gyroSensor->notchFilter1[axis], gyroADCf);
         gyroADCf = gyroSensor->notchFilter2ApplyFn((filter_t *)&gyroSensor->notchFilter2[axis], gyroADCf);

src/main/sensors/rpm_filter.c

@@ -0,0 +1,204 @@
+/*
+ * 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 <http://www.gnu.org/licenses/>.
+ */
+
+
+#include <stdint.h>
+#include <math.h>
+#include "drivers/pwm_output_counts.h"
+#include "sensors/rpm_filter.h"
+#include "common/filter.h"
+#include "sensors/gyro.h"
+#include "build/debug.h"
+#include "flight/mixer.h"
+#include "flight/pid.h"
+
+#define RPM_FILTER_MAXHARMONICS 3
+#define RPM_MOTOR_FILTER_CUTOFF 150
+
+#if defined(USE_RPM_FILTER)
+
+static pt1Filter_t rpmFilters[MAX_SUPPORTED_MOTORS];
+
+typedef struct rpmNotchFilter_s
+{
+    uint8_t harmonics;
+    uint8_t minHz;
+    float   q;
+    float   loopTime;
+
+    biquadFilter_t notch[XYZ_AXIS_COUNT][MAX_SUPPORTED_MOTORS][RPM_FILTER_MAXHARMONICS];
+} rpmNotchFilter_t;
+
+static float   erpmToHz;
+static float   filteredMotorErpm[MAX_SUPPORTED_MOTORS];
+static uint8_t numberFilters;
+static uint8_t numberRpmNotchFilters;
+static uint8_t filterUpdatesPerIteration;
+static float   pidLooptime;
+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);
+
+void pgResetFn_rpmFilterConfig(rpmFilterConfig_t *config)
+{
+    config->gyro_rpm_notch_harmonics = 3;
+    config->gyro_rpm_notch_min = 100;
+    config->gyro_rpm_notch_q = 50;
+
+    config->dterm_rpm_notch_harmonics = 1;
+    config->dterm_rpm_notch_min = 100;
+    config->dterm_rpm_notch_q = 50;
+}
+
+static void rpmNotchFilterInit(rpmNotchFilter_t* filter, int harmonics, int minHz, int q, float looptime)
+{
+    filter->harmonics = harmonics;
+    filter->minHz = minHz;
+    filter->q = q / 10.0f;
+    filter->loopTime = looptime;
+
+    for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+        for (int motor = 0; motor < getMotorCount(); motor++) {
+            for (int i = 0; i < harmonics; i++) {
+                biquadFilterInit(
+                    &filter->notch[axis][motor][i], minHz * i, looptime, filter->q, FILTER_NOTCH);
+            }
+        }
+    }
+}
+
+void rpmFilterInit(const rpmFilterConfig_t *config)
+{
+    pidLooptime = gyro.targetLooptime * pidConfig()->pid_process_denom;
+    numberRpmNotchFilters = 0;
+    if (config->gyro_rpm_notch_harmonics) {
+        gyroFilter = &filters[numberRpmNotchFilters++];
+        rpmNotchFilterInit(gyroFilter, config->gyro_rpm_notch_harmonics,
+                           config->gyro_rpm_notch_min, config->gyro_rpm_notch_q, gyro.targetLooptime);
+    }
+    if (config->dterm_rpm_notch_harmonics) {
+        dtermFilter = &filters[numberRpmNotchFilters++];
+        rpmNotchFilterInit(dtermFilter, config->dterm_rpm_notch_harmonics,
+                           config->dterm_rpm_notch_min, config->dterm_rpm_notch_q, pidLooptime);
+    }
+
+    for (int i = 0; i < getMotorCount(); i++) {
+        pt1FilterInit(&rpmFilters[i], pt1FilterGain(RPM_MOTOR_FILTER_CUTOFF, gyro.targetLooptime));
+    }
+
+    const float secondsPerMinute = 60.0f;
+    const float erpmPerLsb = 100.0f;
+    erpmToHz = erpmPerLsb / secondsPerMinute  / (motorConfig()->motorPoleCount / 2.0f);
+
+    const float minUpdateT = 0.001f;
+    const float loopIterationsPerUpdate = minUpdateT / (pidLooptime * 1e-6f);
+    numberFilters = getMotorCount() * (filters[0].harmonics + filters[1].harmonics);
+    const float filtersPerLoopIteration = numberFilters / loopIterationsPerUpdate;
+    filterUpdatesPerIteration = rintf(filtersPerLoopIteration + 0.49f);
+}
+
+static float applyFilter(rpmNotchFilter_t* filter, 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 = biquadFilterApplyDF1(&filter->notch[axis][motor][i], value);
+        }
+    }
+    return value;
+}
+
+float rpmFilterGyro(int axis, float value)
+{
+    return applyFilter(gyroFilter, axis, value);
+}
+
+float rpmFilterDterm(int axis, float value)
+{
+    return applyFilter(dtermFilter, axis, value);
+}
+
+static float motorFrequency[MAX_SUPPORTED_MOTORS];
+
+void rpmFilterUpdate()
+{
+    if (gyroFilter == NULL && dtermFilter == NULL) {
+        return;
+    }
+
+    for (int motor = 0; motor < getMotorCount(); motor++) {
+        filteredMotorErpm[motor] = pt1FilterApply(&rpmFilters[motor], getDshotTelemetry(motor));
+        DEBUG_SET(DEBUG_RPM_FILTER, motor, motorFrequency[motor]);
+    }
+
+    static uint8_t motor;
+    static uint8_t harmonic;
+    static uint8_t filter;
+    static rpmNotchFilter_t* currentFilter = &filters[0];
+
+    for (int i = 0; i < filterUpdatesPerIteration; i++) {
+
+        float frequency = (harmonic + 1) * motorFrequency[motor];
+        const float deactivateFreq = 1000.0f;
+        if (frequency < currentFilter->minHz) {
+            if (frequency < 0.5f * currentFilter->minHz) {
+                frequency = deactivateFreq;
+            } else {
+                frequency = currentFilter->minHz;
+            }
+        }
+
+        biquadFilter_t* template = &currentFilter->notch[0][motor][harmonic];
+        /* 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) */
+        biquadFilterUpdate(
+            template, frequency, currentFilter->loopTime, currentFilter->q, FILTER_NOTCH);
+        for (int axis = 1; axis < XYZ_AXIS_COUNT; axis++) {
+            biquadFilter_t* clone = &currentFilter->notch[axis][motor][harmonic];
+            clone->b0 = template->b0;
+            clone->b1 = template->b1;
+            clone->b2 = template->b2;
+            clone->a1 = template->a1;
+            clone->a2 = template->a2;
+        }
+
+        if (++harmonic == currentFilter->harmonics) {
+            harmonic = 0;
+            if (++filter == numberRpmNotchFilters) {
+                filter = 0;
+                if (++motor == getMotorCount()) {
+                    motor = 0;
+                }
+                motorFrequency[motor] = erpmToHz * filteredMotorErpm[motor];
+            }
+            currentFilter = &filters[filter];
+        }
+
+    }
+}
+
+#endif

src/main/sensors/rpm_filter.h

@@ -0,0 +1,44 @@
+/*
+ * 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 <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+#include <common/axis.h>
+
+#include "pg/pg.h"
+#include "pg/pg_ids.h"
+
+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
+
+    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
+
+} rpmFilterConfig_t;
+
+PG_DECLARE(rpmFilterConfig_t, rpmFilterConfig);
+
+void  rpmFilterInit(const rpmFilterConfig_t *config);
+float rpmFilterGyro(int axis, float values);
+float rpmFilterDterm(int axis, float values);
+void  rpmFilterUpdate();

src/main/target/common_post.h

@@ -238,3 +238,8 @@
 // This feature uses 'arm_math.h', which does not exist for x86.
 #undef USE_GYRO_DATA_ANALYSE
 #endif
+
+#ifndef USE_DSHOT
+#undef USE_DSHOT_TELEMETRY
+#undef USE_RPM_FILTER
+#endif

src/main/target/common_pre.h

@@ -54,6 +54,7 @@
 #endif
 #define USE_DSHOT
 #define USE_DSHOT_TELEMETRY
+#define USE_RPM_FILTER
 #define I2C3_OVERCLOCK true
 #define USE_GYRO_DATA_ANALYSE
 #define USE_ADC