mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-25 01:05:27 +03:00
Refactor RPM Filter (#11765)
* Refactor RPM Filter * Refactor RPM Filter parameter group
This commit is contained in:
Jan Post
GitHub
parent
9024bfcfd9
commit
931c982a3b
8 changed files with 194 additions and 171 deletions
|
|
@ -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 {
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -19,18 +19,18 @@
|
|||
*/
|
||||
|
||||
|
||||
#include <float.h>
|
||||
#include <math.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
|
||||
#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
|
||||
|
|
|
|||
|
|
@ -20,24 +20,14 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include "common/axis.h"
|
||||
#include "pg/pg.h"
|
||||
#include <stdbool.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
|
||||
#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);
|
||||
|
|
|
|||
40
src/main/pg/rpm_filter.c
Normal file
40
src/main/pg/rpm_filter.c
Normal file
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#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
|
||||
38
src/main/pg/rpm_filter.h
Normal file
38
src/main/pg/rpm_filter.h
Normal file
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#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);
|
||||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue