rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #9984 from mikeller/remove_dterm_rpm_filter 

Removed DTerm RPM filter.
This commit is contained in:
Michael Keller 2020-07-08 00:29:20 +12:00 committed by GitHub
parent 41fa8754bc 15d1df3c77
commit fe888ef98a
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 4 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/main/blackbox/blackbox.c
View File

@ -1430,9 +1430,6 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE("gyro_rpm_notch_harmonics", "%d", rpmFilterConfig()->gyro_rpm_notch_harmonics);
BLACKBOX_PRINT_HEADER_LINE("gyro_rpm_notch_q", "%d", rpmFilterConfig()->gyro_rpm_notch_q);
BLACKBOX_PRINT_HEADER_LINE("gyro_rpm_notch_min", "%d", rpmFilterConfig()->gyro_rpm_notch_min);
BLACKBOX_PRINT_HEADER_LINE("dterm_rpm_notch_harmonics", "%d", rpmFilterConfig()->dterm_rpm_notch_harmonics);
BLACKBOX_PRINT_HEADER_LINE("dterm_rpm_notch_q", "%d", rpmFilterConfig()->dterm_rpm_notch_q);
BLACKBOX_PRINT_HEADER_LINE("dterm_rpm_notch_min", "%d", rpmFilterConfig()->dterm_rpm_notch_min);
BLACKBOX_PRINT_HEADER_LINE("rpm_notch_lpf", "%d", rpmFilterConfig()->rpm_lpf);
#endif
#if defined(USE_ACC)

3
src/main/cli/settings.c
View File

@ -1617,9 +1617,6 @@ const clivalue_t valueTable[] = {
{ "gyro_rpm_notch_harmonics", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 3 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, gyro_rpm_notch_harmonics) },
{ "gyro_rpm_notch_q", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 250, 3000 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, gyro_rpm_notch_q) },
{ "gyro_rpm_notch_min", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 50, 200 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, gyro_rpm_notch_min) },
{ "dterm_rpm_notch_harmonics", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 3 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, dterm_rpm_notch_harmonics) },
{ "dterm_rpm_notch_q", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 250, 3000 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, dterm_rpm_notch_q) },
{ "dterm_rpm_notch_min", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 50, 200 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, dterm_rpm_notch_min) },
{ "rpm_notch_lpf", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 100, 500 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_lpf) },
#endif

1
src/main/fc/init.c
View File

@ -96,7 +96,6 @@
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/pid_init.h"
#include "flight/rpm_filter.h"
#include "flight/servos.h"
#include "io/asyncfatfs/asyncfatfs.h"

7
src/main/flight/pid.c
View File

@ -47,7 +47,6 @@
#include "flight/gps_rescue.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/rpm_filter.h"
#include "flight/interpolated_setpoint.h"
#include "io/gps.h"
@ -866,18 +865,12 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
DEBUG_SET(DEBUG_D_LPF, 1, lrintf(delta));
}
#ifdef USE_RPM_FILTER
gyroRateDterm[axis] = rpmFilterDterm(axis,gyroRateDterm[axis]);
#endif
gyroRateDterm[axis] = pidRuntime.dtermNotchApplyFn((filter_t *) &pidRuntime.dtermNotch[axis], gyroRateDterm[axis]);
gyroRateDterm[axis] = pidRuntime.dtermLowpassApplyFn((filter_t *) &pidRuntime.dtermLowpass[axis], gyroRateDterm[axis]);
gyroRateDterm[axis] = pidRuntime.dtermLowpass2ApplyFn((filter_t *) &pidRuntime.dtermLowpass2[axis], gyroRateDterm[axis]);
}
rotateItermAndAxisError();
#ifdef USE_RPM_FILTER
rpmFilterUpdate();
#endif
#ifdef USE_INTERPOLATED_SP
bool newRcFrame = false;

27
src/main/flight/rpm_filter.c
View File

@ -72,7 +72,6 @@ FAST_RAM_ZERO_INIT static uint8_t filterUpdatesPerIteration;
FAST_RAM_ZERO_INIT static float pidLooptime;
FAST_RAM_ZERO_INIT static rpmNotchFilter_t filters[2];
FAST_RAM_ZERO_INIT static rpmNotchFilter_t* gyroFilter;
FAST_RAM_ZERO_INIT static rpmNotchFilter_t* dtermFilter;
FAST_RAM_ZERO_INIT static uint8_t currentMotor;
FAST_RAM_ZERO_INIT static uint8_t currentHarmonic;
@ -81,7 +80,7 @@ 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, 4);
void pgResetFn_rpmFilterConfig(rpmFilterConfig_t *config)
{
@ -89,10 +88,6 @@ void pgResetFn_rpmFilterConfig(rpmFilterConfig_t *config)
config->gyro_rpm_notch_min = 100;
config->gyro_rpm_notch_q = 500;
config->dterm_rpm_notch_harmonics = 0;
config->dterm_rpm_notch_min = 100;
config->dterm_rpm_notch_q = 500;
config->rpm_lpf = 150;
}
@ -120,7 +115,7 @@ void rpmFilterInit(const rpmFilterConfig_t *config)
numberRpmNotchFilters = 0;
if (!motorConfig()->dev.useDshotTelemetry) {
gyroFilter = dtermFilter = NULL;
gyroFilter = NULL;
return;
}
@ -134,15 +129,6 @@ void rpmFilterInit(const rpmFilterConfig_t *config)
} else {
gyroFilter = NULL;
}
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);
// don't go quite to nyquist to avoid oscillations
dtermFilter->maxHz = 0.48f / (pidLooptime * 1e-6f);
} else {
dtermFilter = NULL;
}
for (int i = 0; i < getMotorCount(); i++) {
pt1FilterInit(&rpmFilters[i], pt1FilterGain(config->rpm_lpf, pidLooptime * 1e-6f));
@ -174,16 +160,11 @@ float rpmFilterGyro(int axis, float value)
return applyFilter(gyroFilter, axis, value);
}
float rpmFilterDterm(int axis, float value)
{
return applyFilter(dtermFilter, axis, value);
}
FAST_RAM_ZERO_INIT static float motorFrequency[MAX_SUPPORTED_MOTORS];
FAST_CODE_NOINLINE void rpmFilterUpdate()
{
if (gyroFilter == NULL && dtermFilter == NULL) {
if (gyroFilter == NULL) {
return;
}
@ -232,7 +213,7 @@ FAST_CODE_NOINLINE void rpmFilterUpdate()
bool isRpmFilterEnabled(void)
{
return (motorConfig()->dev.useDshotTelemetry && (rpmFilterConfig()->gyro_rpm_notch_harmonics || rpmFilterConfig()->dterm_rpm_notch_harmonics));
return (motorConfig()->dev.useDshotTelemetry && rpmFilterConfig()->gyro_rpm_notch_harmonics);
}
float rpmMinMotorFrequency()

4
src/main/flight/rpm_filter.h
View File

@ -29,10 +29,6 @@ typedef struct rpmFilterConfig_s
uint8_t gyro_rpm_notch_min; // minimum frequency of the notches
uint16_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
uint16_t dterm_rpm_notch_q; // q of the notches
uint16_t rpm_lpf; // the cutoff of the lpf on reported motor rpm
} rpmFilterConfig_t;