Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-26 01:35:41 +03:00
Code Issues Wiki Activity

Allow rpm filter to be reinitialized with different settings

Browse source
This commit is contained in:
Thorsten Laux 2019-09-05 09:49:09 +02:00 • committed by mikeller
parent 44f0fb577c
commit 3d9541eea0
1 changed files with 25 additions and 16 deletions
Download patch file Download diff file Expand all files Collapse all files

41
src/main/flight/rpm_filter.c
View file

@ -74,6 +74,13 @@ FAST_RAM_ZERO_INIT static rpmNotchFilter_t filters[2];
FAST_RAM_ZERO_INIT static rpmNotchFilter_t* gyroFilter; FAST_RAM_ZERO_INIT static rpmNotchFilter_t* gyroFilter;
FAST_RAM_ZERO_INIT static rpmNotchFilter_t* dtermFilter; FAST_RAM_ZERO_INIT static rpmNotchFilter_t* dtermFilter;
FAST_RAM_ZERO_INIT static uint8_t currentMotor;
FAST_RAM_ZERO_INIT static uint8_t currentHarmonic;
FAST_RAM_ZERO_INIT static uint8_t currentFilterNumber;
FAST_RAM static rpmNotchFilter_t* currentFilter = &filters[0];
PG_REGISTER_WITH_RESET_FN(rpmFilterConfig_t, rpmFilterConfig, PG_RPM_FILTER_CONFIG, 3); PG_REGISTER_WITH_RESET_FN(rpmFilterConfig_t, rpmFilterConfig, PG_RPM_FILTER_CONFIG, 3);
void pgResetFn_rpmFilterConfig(rpmFilterConfig_t *config) void pgResetFn_rpmFilterConfig(rpmFilterConfig_t *config)
@ -108,6 +115,9 @@ static void rpmNotchFilterInit(rpmNotchFilter_t* filter, int harmonics, int minH
void rpmFilterInit(const rpmFilterConfig_t *config) void rpmFilterInit(const rpmFilterConfig_t *config)
{ {
currentFilter = &filters[0];
currentMotor = currentHarmonic = currentFilterNumber = 0;
numberRpmNotchFilters = 0; numberRpmNotchFilters = 0;
if (!motorConfig()->dev.useDshotTelemetry) { if (!motorConfig()->dev.useDshotTelemetry) {
gyroFilter = dtermFilter = NULL; gyroFilter = dtermFilter = NULL;
@ -121,6 +131,8 @@ void rpmFilterInit(const rpmFilterConfig_t *config)
config->gyro_rpm_notch_min, config->gyro_rpm_notch_q, gyro.targetLooptime); config->gyro_rpm_notch_min, config->gyro_rpm_notch_q, gyro.targetLooptime);
// don't go quite to nyquist to avoid oscillations // don't go quite to nyquist to avoid oscillations
gyroFilter->maxHz = 0.48f / (gyro.targetLooptime * 1e-6f); gyroFilter->maxHz = 0.48f / (gyro.targetLooptime * 1e-6f);
} else {
gyroFilter = NULL;
} }
if (config->dterm_rpm_notch_harmonics) { if (config->dterm_rpm_notch_harmonics) {
dtermFilter = &filters[numberRpmNotchFilters++]; dtermFilter = &filters[numberRpmNotchFilters++];
@ -128,6 +140,8 @@ void rpmFilterInit(const rpmFilterConfig_t *config)
config->dterm_rpm_notch_min, config->dterm_rpm_notch_q, pidLooptime); config->dterm_rpm_notch_min, config->dterm_rpm_notch_q, pidLooptime);
// don't go quite to nyquist to avoid oscillations // don't go quite to nyquist to avoid oscillations
dtermFilter->maxHz = 0.48f / (pidLooptime * 1e-6f); dtermFilter->maxHz = 0.48f / (pidLooptime * 1e-6f);
} else {
dtermFilter = NULL;
} }
for (int i = 0; i < getMotorCount(); i++) { for (int i = 0; i < getMotorCount(); i++) {
@ -173,11 +187,6 @@ FAST_CODE_NOINLINE void rpmFilterUpdate()
return; return;
} }
FAST_RAM_ZERO_INIT static uint8_t motor;
FAST_RAM_ZERO_INIT static uint8_t harmonic;
FAST_RAM_ZERO_INIT static uint8_t filter;
FAST_RAM static rpmNotchFilter_t* currentFilter = &filters[0];
for (int motor = 0; motor < getMotorCount(); motor++) { for (int motor = 0; motor < getMotorCount(); motor++) {
filteredMotorErpm[motor] = pt1FilterApply(&rpmFilters[motor], getDshotTelemetry(motor)); filteredMotorErpm[motor] = pt1FilterApply(&rpmFilters[motor], getDshotTelemetry(motor));
if (motor < 4) { if (motor < 4) {
@ -187,8 +196,8 @@ FAST_CODE_NOINLINE void rpmFilterUpdate()
for (int i = 0; i < filterUpdatesPerIteration; i++) { for (int i = 0; i < filterUpdatesPerIteration; i++) {
float frequency = constrainf( float frequency = constrainf(
(harmonic + 1) * motorFrequency[motor], currentFilter->minHz, currentFilter->maxHz); (currentHarmonic + 1) * motorFrequency[currentMotor], currentFilter->minHz, currentFilter->maxHz);
biquadFilter_t* template = &currentFilter->notch[0][motor][harmonic]; biquadFilter_t* template = &currentFilter->notch[0][currentMotor][currentHarmonic];
// uncomment below to debug filter stepping. Need to also comment out motor rpm DEBUG_SET above // 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, 0, harmonic); */
/* DEBUG_SET(DEBUG_RPM_FILTER, 1, motor); */ /* DEBUG_SET(DEBUG_RPM_FILTER, 1, motor); */
@ -197,7 +206,7 @@ FAST_CODE_NOINLINE void rpmFilterUpdate()
biquadFilterUpdate( biquadFilterUpdate(
template, frequency, currentFilter->loopTime, currentFilter->q, FILTER_NOTCH); template, frequency, currentFilter->loopTime, currentFilter->q, FILTER_NOTCH);
for (int axis = 1; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 1; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilter_t* clone = &currentFilter->notch[axis][motor][harmonic]; biquadFilter_t* clone = &currentFilter->notch[axis][currentMotor][currentHarmonic];
clone->b0 = template->b0; clone->b0 = template->b0;
clone->b1 = template->b1; clone->b1 = template->b1;
clone->b2 = template->b2; clone->b2 = template->b2;
@ -205,17 +214,17 @@ FAST_CODE_NOINLINE void rpmFilterUpdate()
clone->a2 = template->a2; clone->a2 = template->a2;
} }
if (++harmonic == currentFilter->harmonics) { if (++currentHarmonic == currentFilter->harmonics) {
harmonic = 0; currentHarmonic = 0;
if (++filter == numberRpmNotchFilters) { if (++currentFilterNumber == numberRpmNotchFilters) {
filter = 0; currentFilterNumber = 0;
if (++motor == getMotorCount()) { if (++currentMotor == getMotorCount()) {
motor = 0; currentMotor = 0;
} }
motorFrequency[motor] = erpmToHz * filteredMotorErpm[motor]; motorFrequency[currentMotor] = erpmToHz * filteredMotorErpm[currentMotor];
minMotorFrequency = 0.0f; minMotorFrequency = 0.0f;
} }
currentFilter = &filters[filter]; currentFilter = &filters[currentFilterNumber];
} }
} }