Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-20 06:45:16 +03:00
Code Issues Wiki Activity

Merge pull request #7350 from joelucid/rpm_filter_fixes

Browse source 
change q to same scale as dynamic and correct rpm pt1 filter gain
This commit is contained in:
Michael Keller 2019-01-10 08:45:14 +13:00 • committed by GitHub
commit a1eac6f0d1
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
3 changed files with 17 additions and 13 deletions
Download patch file Download diff file Expand all files Collapse all files

4
src/main/interface/settings.c
View file

@ -1308,10 +1308,10 @@ const clivalue_t valueTable[] = {
#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_q", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1, 1000 }, 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_q", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1, 1000 }, 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
};

18
src/main/sensors/rpm_filter.c
View file

@ -61,25 +61,24 @@ 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);
PG_REGISTER_WITH_RESET_FN(rpmFilterConfig_t, rpmFilterConfig, PG_RPM_FILTER_CONFIG, 2);
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->gyro_rpm_notch_q = 500;
config->dterm_rpm_notch_harmonics = 1;
config->dterm_rpm_notch_min = 100;
config->dterm_rpm_notch_q = 50;
config->dterm_rpm_notch_q = 500;
}
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->q = q / 100.0f;
filter->loopTime = looptime;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
@ -94,8 +93,13 @@ static void rpmNotchFilterInit(rpmNotchFilter_t* filter, int harmonics, int minH
void rpmFilterInit(const rpmFilterConfig_t *config)
{
pidLooptime = gyro.targetLooptime * pidConfig()->pid_process_denom;
numberRpmNotchFilters = 0;
if (!motorConfig()->dev.useDshotTelemetry) {
gyroFilter = dtermFilter = NULL;
return;
}
pidLooptime = gyro.targetLooptime * pidConfig()->pid_process_denom;
if (config->gyro_rpm_notch_harmonics) {
gyroFilter = &filters[numberRpmNotchFilters++];
rpmNotchFilterInit(gyroFilter, config->gyro_rpm_notch_harmonics,
@ -108,7 +112,7 @@ void rpmFilterInit(const rpmFilterConfig_t *config)
}
for (int i = 0; i < getMotorCount(); i++) {
pt1FilterInit(&rpmFilters[i], pt1FilterGain(RPM_MOTOR_FILTER_CUTOFF, gyro.targetLooptime));
pt1FilterInit(&rpmFilters[i], pt1FilterGain(RPM_MOTOR_FILTER_CUTOFF, pidLooptime));
}
erpmToHz = ERPM_PER_LSB / SECONDS_PER_MINUTE / (motorConfig()->motorPoleCount / 2.0f);

4
src/main/sensors/rpm_filter.h
View file

@ -27,11 +27,11 @@ 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
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
uint8_t dterm_rpm_notch_q; // q of the notches
uint16_t dterm_rpm_notch_q; // q of the notches
} rpmFilterConfig_t;