Merge pull request #9984 from mikeller/remove_dterm_rpm_filter
Removed DTerm RPM filter.
This commit is contained in:
commit
fe888ef98a
|
@ -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)
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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"
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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()
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
Loading…
Reference in New Issue