Skip to content

Commit

Permalink
RPM Filter Refactor (#194)
Browse files Browse the repository at this point in the history
  • Loading branch information
@rotorflight
rotorflight authored Jan 15, 2025
1 parent b533029 commit 245a556
Show file tree
Hide file tree
Showing 9 changed files with 359 additions and 214 deletions.
29 changes: 21 additions & 8 deletions src/main/blackbox/blackbox.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1681,14 +1681,27 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DSHOT_BIDIR, "%d", motorConfig()->dev.useDshotTelemetry);
#endif
#ifdef USE_RPM_FILTER
BLACKBOX_PRINT_HEADER_ARRAY("gyro_rpm_filter_bank_rpm_source", "%d",
RPM_FILTER_BANK_COUNT, rpmFilterConfig()->filter_bank_rpm_source);
BLACKBOX_PRINT_HEADER_ARRAY("gyro_rpm_filter_bank_rpm_ratio", "%d",
RPM_FILTER_BANK_COUNT, rpmFilterConfig()->filter_bank_rpm_ratio);
BLACKBOX_PRINT_HEADER_ARRAY("gyro_rpm_filter_bank_rpm_limit", "%d",
RPM_FILTER_BANK_COUNT, rpmFilterConfig()->filter_bank_rpm_limit);
BLACKBOX_PRINT_HEADER_ARRAY("gyro_rpm_filter_bank_notch_q", "%d",
RPM_FILTER_BANK_COUNT, rpmFilterConfig()->filter_bank_notch_q);
BLACKBOX_PRINT_HEADER_LINE("gyro_rpm_notch_preset", "%d", rpmFilterConfig()->preset);
BLACKBOX_PRINT_HEADER_LINE("gyro_rpm_notch_min_hz", "%d", rpmFilterConfig()->min_hz);

BLACKBOX_PRINT_HEADER_ARRAY("gyro_rpm_notch_source_pitch", "%d",
RPM_FILTER_NOTCH_COUNT, rpmFilterConfig()->custom.notch_source[FD_PITCH]);
BLACKBOX_PRINT_HEADER_ARRAY("gyro_rpm_notch_center_pitch", "%d",
RPM_FILTER_NOTCH_COUNT, rpmFilterConfig()->custom.notch_center[FD_PITCH]);
BLACKBOX_PRINT_HEADER_ARRAY("gyro_rpm_notch_q_pitch", "%d",
RPM_FILTER_NOTCH_COUNT, rpmFilterConfig()->custom.notch_q[FD_PITCH]);
BLACKBOX_PRINT_HEADER_ARRAY("gyro_rpm_notch_source_roll", "%d",
RPM_FILTER_NOTCH_COUNT, rpmFilterConfig()->custom.notch_source[FD_ROLL]);
BLACKBOX_PRINT_HEADER_ARRAY("gyro_rpm_notch_center_roll", "%d",
RPM_FILTER_NOTCH_COUNT, rpmFilterConfig()->custom.notch_center[FD_ROLL]);
BLACKBOX_PRINT_HEADER_ARRAY("gyro_rpm_notch_q_roll", "%d",
RPM_FILTER_NOTCH_COUNT, rpmFilterConfig()->custom.notch_q[FD_ROLL]);
BLACKBOX_PRINT_HEADER_ARRAY("gyro_rpm_notch_source_yaw", "%d",
RPM_FILTER_NOTCH_COUNT, rpmFilterConfig()->custom.notch_source[FD_YAW]);
BLACKBOX_PRINT_HEADER_ARRAY("gyro_rpm_notch_center_yaw", "%d",
RPM_FILTER_NOTCH_COUNT, rpmFilterConfig()->custom.notch_center[FD_YAW]);
BLACKBOX_PRINT_HEADER_ARRAY("gyro_rpm_notch_q_yaw", "%d",
RPM_FILTER_NOTCH_COUNT, rpmFilterConfig()->custom.notch_q[FD_YAW]);
#endif
#if defined(USE_ACC)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ACC_LPF_HZ, "%d", accelerometerConfig()->acc_lpf_hz * 100);
Expand Down
15 changes: 11 additions & 4 deletions src/main/cli/settings.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1694,10 +1694,17 @@ const clivalue_t valueTable[] = {
#endif

#ifdef USE_RPM_FILTER
{ "gyro_rpm_filter_bank_rpm_source", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = RPM_FILTER_BANK_COUNT, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, filter_bank_rpm_source) },
{ "gyro_rpm_filter_bank_rpm_ratio", VAR_UINT16 | MASTER_VALUE | MODE_ARRAY, .config.array.length = RPM_FILTER_BANK_COUNT, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, filter_bank_rpm_ratio) },
{ "gyro_rpm_filter_bank_rpm_limit", VAR_UINT16 | MASTER_VALUE | MODE_ARRAY, .config.array.length = RPM_FILTER_BANK_COUNT, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, filter_bank_rpm_limit) },
{ "gyro_rpm_filter_bank_notch_q", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = RPM_FILTER_BANK_COUNT, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, filter_bank_notch_q) },
{ "gyro_rpm_notch_preset", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 3 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, preset) },
{ "gyro_rpm_notch_min_hz", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 1, 100 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, min_hz) },
{ "gyro_rpm_notch_source_pitch", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = RPM_FILTER_NOTCH_COUNT, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, custom.notch_source[PITCH]) },
{ "gyro_rpm_notch_center_pitch", VAR_INT16 | MASTER_VALUE | MODE_ARRAY, .config.array.length = RPM_FILTER_NOTCH_COUNT, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, custom.notch_center[PITCH]) },
{ "gyro_rpm_notch_q_pitch", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = RPM_FILTER_NOTCH_COUNT, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, custom.notch_q[PITCH]) },
{ "gyro_rpm_notch_source_roll", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = RPM_FILTER_NOTCH_COUNT, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, custom.notch_source[ROLL]) },
{ "gyro_rpm_notch_center_roll", VAR_INT16 | MASTER_VALUE | MODE_ARRAY, .config.array.length = RPM_FILTER_NOTCH_COUNT, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, custom.notch_center[ROLL]) },
{ "gyro_rpm_notch_q_roll", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = RPM_FILTER_NOTCH_COUNT, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, custom.notch_q[ROLL]) },
{ "gyro_rpm_notch_source_yaw", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = RPM_FILTER_NOTCH_COUNT, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, custom.notch_source[YAW]) },
{ "gyro_rpm_notch_center_yaw", VAR_INT16 | MASTER_VALUE | MODE_ARRAY, .config.array.length = RPM_FILTER_NOTCH_COUNT, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, custom.notch_center[YAW]) },
{ "gyro_rpm_notch_q_yaw", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = RPM_FILTER_NOTCH_COUNT, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, custom.notch_q[YAW]) },
#endif

#ifdef USE_RX_FLYSKY
Expand Down
21 changes: 4 additions & 17 deletions src/main/config/config.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -428,6 +428,10 @@ static void validateAndFixConfig(void)
featureDisableImmediate(FEATURE_RSSI_ADC);
#endif

#ifdef USE_RPM_FILTER
validateAndFixRPMFilterConfig();
#endif

#if defined(USE_BEEPER)
#ifdef USE_TIMER
if (beeperDevConfig()->frequency && !timerGetConfiguredByTag(beeperDevConfig()->ioTag)) {
Expand Down Expand Up @@ -664,23 +668,6 @@ void validateAndFixGyroConfig(void)
if (gyroConfig()->gyro_soft_notch_cutoff_2 >= gyroConfig()->gyro_soft_notch_hz_2) {
gyroConfigMutable()->gyro_soft_notch_hz_2 = 0;
}

#ifdef USE_RPM_FILTER
if (featureIsConfigured(FEATURE_RPM_FILTER)) {
rpmFilterConfig_t *config = rpmFilterConfigMutable();
for (int index = 0; index < RPM_FILTER_BANK_COUNT; index++) {
if (config->filter_bank_rpm_source[index] == 0 ||
config->filter_bank_rpm_ratio[index] == 0 ||
config->filter_bank_notch_q[index] == 0)
{
config->filter_bank_rpm_source[index] = 0;
config->filter_bank_rpm_ratio[index] = 0;
config->filter_bank_rpm_limit[index] = 0;
config->filter_bank_notch_q[index] = 0;
}
}
}
#endif
}

#ifdef USE_BLACKBOX
Expand Down
Loading

0 comments on commit 245a556

Please sign in to comment.