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

change q to same scale as dynamic and correct rpm pt1 filter gain

Browse source
This commit is contained in:
Thorsten Laux 2019-01-08 21:47:31 +01:00
parent 297eebca74
commit 9b05a99c94
3 changed files with 15 additions and 11 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1307,10 +1307,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
};

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

@ -61,8 +61,7 @@ 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)
{
@ -79,7 +78,7 @@ static void rpmNotchFilterInit(rpmNotchFilter_t* filter, int harmonics, int minH
{
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);

8
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
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
} rpmFilterConfig_t;