diff --git a/make/source.mk b/make/source.mk
index 354249bb66..9e1452511b 100644
--- a/make/source.mk
+++ b/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) \
diff --git a/src/main/build/debug.c b/src/main/build/debug.c
index e7177cdfe8..6acb6813f5 100644
--- a/src/main/build/debug.c
+++ b/src/main/build/debug.c
@@ -79,4 +79,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
"DYN_LPF",
"RX_SPEKTRUM_SPI",
"DSHOT_TELEMETRY",
+ "RPM_FILTER",
};
diff --git a/src/main/build/debug.h b/src/main/build/debug.h
index 291cef84f0..3159e7a187 100644
--- a/src/main/build/debug.h
+++ b/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;
diff --git a/src/main/drivers/pwm_output_dshot.c b/src/main/drivers/pwm_output_dshot.c
index bba1ff6b0a..77deca03c9 100644
--- a/src/main/drivers/pwm_output_dshot.c
+++ b/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(
- dmaMotors[i].dmaBuffer ) :
- decodeDshotPacket(
- dmaMotors[i].dmaBuffer );
+ decodeProshotPacket(dmaMotors[i].dmaBuffer) :
+ decodeDshotPacket(dmaMotors[i].dmaBuffer);
if (value != 0xffff) {
dmaMotors[i].dshotTelemetryValue = value;
if (i < 4) {
diff --git a/src/main/fc/init.c b/src/main/fc/init.c
index 1541a745d8..c950af6ead 100644
--- a/src/main/fc/init.c
+++ b/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"
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 9e2c496138..7c5146919d 100644
--- a/src/main/flight/pid.c
+++ b/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) {
diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c
index d898ec1c4f..b8672d70da 100644
--- a/src/main/interface/settings.c
+++ b/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);
diff --git a/src/main/pg/pg_ids.h b/src/main/pg/pg_ids.h
index 0560778f8d..dcb12a80cd 100644
--- a/src/main/pg/pg_ids.h
+++ b/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)
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index ecd542df70..93e5708d40 100644
--- a/src/main/sensors/gyro.c
+++ b/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)))
diff --git a/src/main/sensors/gyro_filter_impl.h b/src/main/sensors/gyro_filter_impl.h
index 69111f5d48..977eb5f304 100644
--- a/src/main/sensors/gyro_filter_impl.h
+++ b/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);
diff --git a/src/main/sensors/rpm_filter.c b/src/main/sensors/rpm_filter.c
new file mode 100644
index 0000000000..64b0ba011c
--- /dev/null
+++ b/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 .
+ */
+
+
+#include
+#include
+#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 = ¤tFilter->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 = ¤tFilter->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
diff --git a/src/main/sensors/rpm_filter.h b/src/main/sensors/rpm_filter.h
new file mode 100644
index 0000000000..ede732f306
--- /dev/null
+++ b/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 .
+ */
+
+#pragma once
+#include
+
+#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();
diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h
index c2aa322b3f..cfd404f434 100644
--- a/src/main/target/common_post.h
+++ b/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
diff --git a/src/main/target/common_pre.h b/src/main/target/common_pre.h
index 0de3130d79..a2ebc886f4 100644
--- a/src/main/target/common_pre.h
+++ b/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