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

consistent gyro and dterm filter names

Browse source
This commit is contained in:
ctzsnooze 2021-09-27 09:12:00 +10:00
parent 8702bba3ec
commit 99a7479b8c
16 changed files with 219 additions and 219 deletions
Download patch file Download diff file Expand all files Collapse all files

24
src/main/blackbox/blackbox.c
View file

@ -1355,14 +1355,14 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE("d_min_gain", "%d", currentPidProfile->d_min_gain);
BLACKBOX_PRINT_HEADER_LINE("d_min_advance", "%d", currentPidProfile->d_min_advance);
#endif
BLACKBOX_PRINT_HEADER_LINE("dterm_filter_type", "%d", currentPidProfile->dterm_filter_type);
BLACKBOX_PRINT_HEADER_LINE("dterm_lowpass_hz", "%d", currentPidProfile->dterm_lowpass_hz);
BLACKBOX_PRINT_HEADER_LINE("dterm_lpf1_type", "%d", currentPidProfile->dterm_lpf1_type);
BLACKBOX_PRINT_HEADER_LINE("dterm_lpf1_static_hz", "%d", currentPidProfile->dterm_lpf1_static_hz);
#ifdef USE_DYN_LPF
BLACKBOX_PRINT_HEADER_LINE("dterm_lowpass_dyn_hz", "%d,%d", currentPidProfile->dyn_lpf_dterm_min_hz,
currentPidProfile->dyn_lpf_dterm_max_hz);
BLACKBOX_PRINT_HEADER_LINE("dterm_lpf1_dyn_hz", "%d,%d", currentPidProfile->dterm_lpf1_dyn_min_hz,
currentPidProfile->dterm_lpf1_dyn_max_hz);
#endif
BLACKBOX_PRINT_HEADER_LINE("dterm_filter2_type", "%d", currentPidProfile->dterm_filter2_type);
BLACKBOX_PRINT_HEADER_LINE("dterm_lowpass2_hz", "%d", currentPidProfile->dterm_lowpass2_hz);
BLACKBOX_PRINT_HEADER_LINE("dterm_lpf2_type", "%d", currentPidProfile->dterm_lpf2_type);
BLACKBOX_PRINT_HEADER_LINE("dterm_lpf2_static_hz", "%d", currentPidProfile->dterm_lpf2_static_hz);
BLACKBOX_PRINT_HEADER_LINE("yaw_lowpass_hz", "%d", currentPidProfile->yaw_lowpass_hz);
BLACKBOX_PRINT_HEADER_LINE("dterm_notch_hz", "%d", currentPidProfile->dterm_notch_hz);
BLACKBOX_PRINT_HEADER_LINE("dterm_notch_cutoff", "%d", currentPidProfile->dterm_notch_cutoff);
@ -1406,14 +1406,14 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE("yaw_deadband", "%d", rcControlsConfig()->yaw_deadband);
BLACKBOX_PRINT_HEADER_LINE("gyro_hardware_lpf", "%d", gyroConfig()->gyro_hardware_lpf);
BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_type", "%d", gyroConfig()->gyro_lowpass_type);
BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz", "%d", gyroConfig()->gyro_lowpass_hz);
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf1_type", "%d", gyroConfig()->gyro_lpf1_type);
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf1_static_hz", "%d", gyroConfig()->gyro_lpf1_static_hz);
#ifdef USE_DYN_LPF
BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_dyn_hz", "%d,%d", gyroConfig()->dyn_lpf_gyro_min_hz,
gyroConfig()->dyn_lpf_gyro_max_hz);
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf1_dyn_hz", "%d,%d", gyroConfig()->gyro_lpf1_dyn_min_hz,
gyroConfig()->gyro_lpf1_dyn_max_hz);
#endif
BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass2_type", "%d", gyroConfig()->gyro_lowpass2_type);
BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass2_hz", "%d", gyroConfig()->gyro_lowpass2_hz);
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf2_type", "%d", gyroConfig()->gyro_lpf2_type);
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf2_static_hz", "%d", gyroConfig()->gyro_lpf2_static_hz);
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz", "%d,%d", gyroConfig()->gyro_soft_notch_hz_1,
gyroConfig()->gyro_soft_notch_hz_2);
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff", "%d,%d", gyroConfig()->gyro_soft_notch_cutoff_1,

40
src/main/cli/settings.c
View file

@ -636,16 +636,16 @@ const clivalue_t valueTable[] = {
{ "gyro_high_range", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_high_fsr) },
#endif
{ "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LOWPASS_TYPE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass_type) },
{ "gyro_lowpass_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, FILTER_FREQUENCY_MAX }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass_hz) },
{ "gyro_lpf1_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO_LPF_TYPE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lpf1_type) },
{ "gyro_lpf1_static_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, LPF_MAX_HZ }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lpf1_static_hz) },
{ "gyro_lowpass2_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LOWPASS_TYPE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass2_type) },
{ "gyro_lowpass2_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, FILTER_FREQUENCY_MAX }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass2_hz) },
{ "gyro_lpf2_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO_LPF_TYPE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lpf2_type) },
{ "gyro_lpf2_static_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, LPF_MAX_HZ }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lpf2_static_hz) },
{ "gyro_notch1_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, FILTER_FREQUENCY_MAX }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_hz_1) },
{ "gyro_notch1_cutoff", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, FILTER_FREQUENCY_MAX }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_cutoff_1) },
{ "gyro_notch2_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, FILTER_FREQUENCY_MAX }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_hz_2) },
{ "gyro_notch2_cutoff", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, FILTER_FREQUENCY_MAX }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_cutoff_2) },
{ "gyro_notch1_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, LPF_MAX_HZ }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_hz_1) },
{ "gyro_notch1_cutoff", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, LPF_MAX_HZ }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_cutoff_1) },
{ "gyro_notch2_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, LPF_MAX_HZ }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_hz_2) },
{ "gyro_notch2_cutoff", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, LPF_MAX_HZ }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_cutoff_2) },
{ "gyro_calib_duration", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 50, 3000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyroCalibrationDuration) },
{ "gyro_calib_noise_limit", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyroMovementCalibrationThreshold) },
@ -668,9 +668,9 @@ const clivalue_t valueTable[] = {
{ "dyn_notch_max_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 200, 1000 }, PG_DYN_NOTCH_CONFIG, offsetof(dynNotchConfig_t, dyn_notch_max_hz) },
#endif
#ifdef USE_DYN_LPF
{ "dyn_lpf_gyro_min_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, DYN_LPF_FILTER_FREQUENCY_MAX }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_lpf_gyro_min_hz) },
{ "dyn_lpf_gyro_max_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, DYN_LPF_FILTER_FREQUENCY_MAX }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_lpf_gyro_max_hz) },
{ "dyn_lpf_gyro_curve_expo", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 10 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_lpf_curve_expo) },
{ "gyro_lpf1_dyn_min_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, DYN_LPF_MAX_HZ }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lpf1_dyn_min_hz) },
{ "gyro_lpf1_dyn_max_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, DYN_LPF_MAX_HZ }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lpf1_dyn_max_hz) },
{ "gyro_lpf1_dyn_expo", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 10 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lpf1_dyn_expo) },
#endif
{ "gyro_filter_debug_axis", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO_FILTER_DEBUG }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_filter_debug_axis) },
@ -1035,16 +1035,16 @@ const clivalue_t valueTable[] = {
{ "profile_name", VAR_UINT8 | PROFILE_VALUE | MODE_STRING, .config.string = { 1, MAX_PROFILE_NAME_LENGTH, STRING_FLAGS_NONE }, PG_PID_PROFILE, offsetof(pidProfile_t, profileName) },
#endif
#ifdef USE_DYN_LPF
{ "dyn_lpf_dterm_min_hz", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, DYN_LPF_FILTER_FREQUENCY_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, dyn_lpf_dterm_min_hz) },
{ "dyn_lpf_dterm_max_hz", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, DYN_LPF_FILTER_FREQUENCY_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, dyn_lpf_dterm_max_hz) },
{ "dyn_lpf_dterm_curve_expo", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 10 }, PG_PID_PROFILE, offsetof(pidProfile_t, dyn_lpf_curve_expo) },
{ "dterm_lpf1_dyn_min_hz", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, DYN_LPF_MAX_HZ }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_lpf1_dyn_min_hz) },
{ "dterm_lpf1_dyn_max_hz", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, DYN_LPF_MAX_HZ }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_lpf1_dyn_max_hz) },
{ "dterm_lpf1_dyn_expo", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 10 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_lpf1_dyn_expo) },
#endif
{ "dterm_lowpass_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DTERM_LOWPASS_TYPE }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_filter_type) },
{ "dterm_lowpass_hz", VAR_INT16 | PROFILE_VALUE, .config.minmax = { 0, FILTER_FREQUENCY_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_lowpass_hz) },
{ "dterm_lowpass2_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DTERM_LOWPASS_TYPE }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_filter2_type) },
{ "dterm_lowpass2_hz", VAR_INT16 | PROFILE_VALUE, .config.minmax = { 0, FILTER_FREQUENCY_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_lowpass2_hz) },
{ "dterm_notch_hz", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, FILTER_FREQUENCY_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_notch_hz) },
{ "dterm_notch_cutoff", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, FILTER_FREQUENCY_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_notch_cutoff) },
{ "dterm_lpf1_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DTERM_LPF_TYPE }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_lpf1_type) },
{ "dterm_lpf1_static_hz", VAR_INT16 | PROFILE_VALUE, .config.minmax = { 0, LPF_MAX_HZ }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_lpf1_static_hz) },
{ "dterm_lpf2_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DTERM_LPF_TYPE }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_lpf2_type) },
{ "dterm_lpf2_static_hz", VAR_INT16 | PROFILE_VALUE, .config.minmax = { 0, LPF_MAX_HZ }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_lpf2_static_hz) },
{ "dterm_notch_hz", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, LPF_MAX_HZ }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_notch_hz) },
{ "dterm_notch_cutoff", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, LPF_MAX_HZ }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_notch_cutoff) },
#if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
{ "vbat_sag_compensation", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 150 }, PG_PID_PROFILE, offsetof(pidProfile_t, vbat_sag_compensation) },
#endif

4
src/main/cli/settings.h
View file

@ -64,8 +64,8 @@ typedef enum {
#endif
TABLE_DEBUG,
TABLE_MOTOR_PWM_PROTOCOL,
TABLE_LOWPASS_TYPE,
TABLE_DTERM_LOWPASS_TYPE,
TABLE_GYRO_LPF_TYPE,
TABLE_DTERM_LPF_TYPE,
TABLE_ANTI_GRAVITY_MODE,
TABLE_FAILSAFE,
TABLE_FAILSAFE_SWITCH_MODE,

84
src/main/cms/cms_menu_imu.c
View file

@ -699,8 +699,8 @@ static CMS_Menu cmsx_menuProfileOther = {
};
static uint16_t gyroConfig_gyro_lowpass_hz;
static uint16_t gyroConfig_gyro_lowpass2_hz;
static uint16_t gyroConfig_gyro_lpf1_static_hz;
static uint16_t gyroConfig_gyro_lpf2_static_hz;
static uint16_t gyroConfig_gyro_soft_notch_hz_1;
static uint16_t gyroConfig_gyro_soft_notch_cutoff_1;
static uint16_t gyroConfig_gyro_soft_notch_hz_2;
@ -711,8 +711,8 @@ static const void *cmsx_menuGyro_onEnter(displayPort_t *pDisp)
{
UNUSED(pDisp);
gyroConfig_gyro_lowpass_hz = gyroConfig()->gyro_lowpass_hz;
gyroConfig_gyro_lowpass2_hz = gyroConfig()->gyro_lowpass2_hz;
gyroConfig_gyro_lpf1_static_hz = gyroConfig()->gyro_lpf1_static_hz;
gyroConfig_gyro_lpf2_static_hz = gyroConfig()->gyro_lpf2_static_hz;
gyroConfig_gyro_soft_notch_hz_1 = gyroConfig()->gyro_soft_notch_hz_1;
gyroConfig_gyro_soft_notch_cutoff_1 = gyroConfig()->gyro_soft_notch_cutoff_1;
gyroConfig_gyro_soft_notch_hz_2 = gyroConfig()->gyro_soft_notch_hz_2;
@ -727,8 +727,8 @@ static const void *cmsx_menuGyro_onExit(displayPort_t *pDisp, const OSD_Entry *s
UNUSED(pDisp);
UNUSED(self);
gyroConfigMutable()->gyro_lowpass_hz = gyroConfig_gyro_lowpass_hz;
gyroConfigMutable()->gyro_lowpass2_hz = gyroConfig_gyro_lowpass2_hz;
gyroConfigMutable()->gyro_lpf1_static_hz = gyroConfig_gyro_lpf1_static_hz;
gyroConfigMutable()->gyro_lpf2_static_hz = gyroConfig_gyro_lpf2_static_hz;
gyroConfigMutable()->gyro_soft_notch_hz_1 = gyroConfig_gyro_soft_notch_hz_1;
gyroConfigMutable()->gyro_soft_notch_cutoff_1 = gyroConfig_gyro_soft_notch_cutoff_1;
gyroConfigMutable()->gyro_soft_notch_hz_2 = gyroConfig_gyro_soft_notch_hz_2;
@ -742,9 +742,9 @@ static const OSD_Entry cmsx_menuFilterGlobalEntries[] =
{
{ "-- FILTER GLB --", OME_Label, NULL, NULL, 0 },
{ "GYRO LPF", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_lowpass_hz, 0, FILTER_FREQUENCY_MAX, 1 }, 0 },
{ "GYRO LPF1", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_lpf1_static_hz, 0, LPF_MAX_HZ, 1 }, 0 },
#ifdef USE_GYRO_LPF2
{ "GYRO LPF2", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_lowpass2_hz, 0, FILTER_FREQUENCY_MAX, 1 }, 0 },
{ "GYRO LPF2", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_lpf2_static_hz, 0, LPF_MAX_HZ, 1 }, 0 },
#endif
{ "GYRO NF1", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_hz_1, 0, 500, 1 }, 0 },
{ "GYRO NF1C", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_cutoff_1, 0, 500, 1 }, 0 },
@ -778,12 +778,12 @@ static uint16_t dynFiltNotchQ;
static uint16_t dynFiltNotchMinHz;
#endif
#ifdef USE_DYN_LPF
static uint16_t dynFiltGyroMin;
static uint16_t dynFiltGyroMax;
static uint8_t dynFiltGyroExpo;
static uint16_t dynFiltDtermMin;
static uint16_t dynFiltDtermMax;
static uint8_t dynFiltDtermExpo;
static uint16_t gyroLpfDynMin;
static uint16_t gyroLpfDynMax;
static uint8_t gyroLpfDynExpo;
static uint16_t dtermLpfDynMin;
static uint16_t dtermLpfDynMax;
static uint8_t dtermLpfDynExpo;
#endif
static const void *cmsx_menuDynFilt_onEnter(displayPort_t *pDisp)
@ -798,12 +798,12 @@ static const void *cmsx_menuDynFilt_onEnter(displayPort_t *pDisp)
#endif
#ifdef USE_DYN_LPF
const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);
dynFiltGyroMin = gyroConfig()->dyn_lpf_gyro_min_hz;
dynFiltGyroMax = gyroConfig()->dyn_lpf_gyro_max_hz;
dynFiltGyroExpo = gyroConfig()->dyn_lpf_curve_expo;
dynFiltDtermMin = pidProfile->dyn_lpf_dterm_min_hz;
dynFiltDtermMax = pidProfile->dyn_lpf_dterm_max_hz;
dynFiltDtermExpo = pidProfile->dyn_lpf_curve_expo;
gyroLpfDynMin = gyroConfig()->gyro_lpf1_dyn_min_hz;
gyroLpfDynMax = gyroConfig()->gyro_lpf1_dyn_max_hz;
gyroLpfDynExpo = gyroConfig()->gyro_lpf1_dyn_expo;
dtermLpfDynMin = pidProfile->dterm_lpf1_dyn_min_hz;
dtermLpfDynMax = pidProfile->dterm_lpf1_dyn_max_hz;
dtermLpfDynExpo = pidProfile->dterm_lpf1_dyn_expo;
#endif
return NULL;
@ -822,12 +822,12 @@ static const void *cmsx_menuDynFilt_onExit(displayPort_t *pDisp, const OSD_Entry
#endif
#ifdef USE_DYN_LPF
pidProfile_t *pidProfile = currentPidProfile;
gyroConfigMutable()->dyn_lpf_gyro_min_hz = dynFiltGyroMin;
gyroConfigMutable()->dyn_lpf_gyro_max_hz = dynFiltGyroMax;
gyroConfigMutable()->dyn_lpf_curve_expo = dynFiltGyroExpo;
pidProfile->dyn_lpf_dterm_min_hz = dynFiltDtermMin;
pidProfile->dyn_lpf_dterm_max_hz = dynFiltDtermMax;
pidProfile->dyn_lpf_curve_expo = dynFiltDtermExpo;
gyroConfigMutable()->gyro_lpf1_dyn_min_hz = gyroLpfDynMin;
gyroConfigMutable()->gyro_lpf1_dyn_max_hz = gyroLpfDynMax;
gyroConfigMutable()->gyro_lpf1_dyn_expo = gyroLpfDynExpo;
pidProfile->dterm_lpf1_dyn_min_hz = dtermLpfDynMin;
pidProfile->dterm_lpf1_dyn_max_hz = dtermLpfDynMax;
pidProfile->dterm_lpf1_dyn_expo = dtermLpfDynExpo;
#endif
return NULL;
@ -845,12 +845,12 @@ static const OSD_Entry cmsx_menuDynFiltEntries[] =
#endif
#ifdef USE_DYN_LPF
{ "LPF GYRO MIN", OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltGyroMin, 0, 1000, 1 }, 0 },
{ "LPF GYRO MAX", OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltGyroMax, 0, 1000, 1 }, 0 },
{ "GYRO DLPF EXPO", OME_UINT8, NULL, &(OSD_UINT8_t) { &dynFiltGyroExpo, 0, 10, 1 }, 0 },
{ "DTERM DLPF MIN", OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltDtermMin, 0, 1000, 1 }, 0 },
{ "DTERM DLPF MAX", OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltDtermMax, 0, 1000, 1 }, 0 },
{ "DTERM DLPF EXPO", OME_UINT8, NULL, &(OSD_UINT8_t) { &dynFiltDtermExpo, 0, 10, 1 }, 0 },
{ "GYRO DLPF MIN", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroLpfDynMin, 0, 1000, 1 }, 0 },
{ "GYRO DLPF MAX", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroLpfDynMax, 0, 1000, 1 }, 0 },
{ "GYRO DLPF EXPO", OME_UINT8, NULL, &(OSD_UINT8_t) { &gyroLpfDynExpo, 0, 10, 1 }, 0 },
{ "DTERM DLPF MIN", OME_UINT16, NULL, &(OSD_UINT16_t) { &dtermLpfDynMin, 0, 1000, 1 }, 0 },
{ "DTERM DLPF MAX", OME_UINT16, NULL, &(OSD_UINT16_t) { &dtermLpfDynMax, 0, 1000, 1 }, 0 },
{ "DTERM DLPF EXPO", OME_UINT8, NULL, &(OSD_UINT8_t) { &dtermLpfDynExpo, 0, 10, 1 }, 0 },
#endif
{ "BACK", OME_Back, NULL, NULL, 0 },
@ -870,8 +870,8 @@ static CMS_Menu cmsx_menuDynFilt = {
#endif
static uint16_t cmsx_dterm_lowpass_hz;
static uint16_t cmsx_dterm_lowpass2_hz;
static uint16_t cmsx_dterm_lpf1_static_hz;
static uint16_t cmsx_dterm_lpf2_static_hz;
static uint16_t cmsx_dterm_notch_hz;
static uint16_t cmsx_dterm_notch_cutoff;
static uint16_t cmsx_yaw_lowpass_hz;
@ -882,8 +882,8 @@ static const void *cmsx_FilterPerProfileRead(displayPort_t *pDisp)
const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);
cmsx_dterm_lowpass_hz = pidProfile->dterm_lowpass_hz;
cmsx_dterm_lowpass2_hz = pidProfile->dterm_lowpass2_hz;
cmsx_dterm_lpf1_static_hz = pidProfile->dterm_lpf1_static_hz;
cmsx_dterm_lpf2_static_hz = pidProfile->dterm_lpf2_static_hz;
cmsx_dterm_notch_hz = pidProfile->dterm_notch_hz;
cmsx_dterm_notch_cutoff = pidProfile->dterm_notch_cutoff;
cmsx_yaw_lowpass_hz = pidProfile->yaw_lowpass_hz;
@ -898,8 +898,8 @@ static const void *cmsx_FilterPerProfileWriteback(displayPort_t *pDisp, const OS
pidProfile_t *pidProfile = currentPidProfile;
pidProfile->dterm_lowpass_hz = cmsx_dterm_lowpass_hz;
pidProfile->dterm_lowpass2_hz = cmsx_dterm_lowpass2_hz;
pidProfile->dterm_lpf1_static_hz = cmsx_dterm_lpf1_static_hz;
pidProfile->dterm_lpf2_static_hz = cmsx_dterm_lpf2_static_hz;
pidProfile->dterm_notch_hz = cmsx_dterm_notch_hz;
pidProfile->dterm_notch_cutoff = cmsx_dterm_notch_cutoff;
pidProfile->yaw_lowpass_hz = cmsx_yaw_lowpass_hz;
@ -911,10 +911,10 @@ static const OSD_Entry cmsx_menuFilterPerProfileEntries[] =
{
{ "-- FILTER PP --", OME_Label, NULL, NULL, 0 },
{ "DTERM LPF", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_lowpass_hz, 0, FILTER_FREQUENCY_MAX, 1 }, 0 },
{ "DTERM LPF2", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_lowpass2_hz, 0, FILTER_FREQUENCY_MAX, 1 }, 0 },
{ "DTERM NF", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_notch_hz, 0, FILTER_FREQUENCY_MAX, 1 }, 0 },
{ "DTERM NFCO", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_notch_cutoff, 0, FILTER_FREQUENCY_MAX, 1 }, 0 },
{ "DTERM LPF1", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_lpf1_static_hz, 0, LPF_MAX_HZ, 1 }, 0 },
{ "DTERM LPF2", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_lpf2_static_hz, 0, LPF_MAX_HZ, 1 }, 0 },
{ "DTERM NF", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_notch_hz, 0, LPF_MAX_HZ, 1 }, 0 },
{ "DTERM NFCO", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_notch_cutoff, 0, LPF_MAX_HZ, 1 }, 0 },
{ "YAW LPF", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_yaw_lowpass_hz, 0, 500, 1 }, 0 },
{ "BACK", OME_Back, NULL, NULL, 0 },

24
src/main/config/config.c
View file

@ -188,7 +188,7 @@ static void activateConfig(void)
static void adjustFilterLimit(uint16_t *parm, uint16_t resetValue)
{
if (*parm > FILTER_FREQUENCY_MAX) {
if (*parm > LPF_MAX_HZ) {
*parm = resetValue;
}
}
@ -245,9 +245,9 @@ static void validateAndFixConfig(void)
for (unsigned i = 0; i < PID_PROFILE_COUNT; i++) {
// Fix filter settings to handle cases where an older configurator was used that
// allowed higher cutoff limits from previous firmware versions.
adjustFilterLimit(&pidProfilesMutable(i)->dterm_lowpass_hz, FILTER_FREQUENCY_MAX);
adjustFilterLimit(&pidProfilesMutable(i)->dterm_lowpass2_hz, FILTER_FREQUENCY_MAX);
adjustFilterLimit(&pidProfilesMutable(i)->dterm_notch_hz, FILTER_FREQUENCY_MAX);
adjustFilterLimit(&pidProfilesMutable(i)->dterm_lpf1_static_hz, LPF_MAX_HZ);
adjustFilterLimit(&pidProfilesMutable(i)->dterm_lpf2_static_hz, LPF_MAX_HZ);
adjustFilterLimit(&pidProfilesMutable(i)->dterm_notch_hz, LPF_MAX_HZ);
adjustFilterLimit(&pidProfilesMutable(i)->dterm_notch_cutoff, 0);
// Prevent invalid notch cutoff
@ -257,8 +257,8 @@ static void validateAndFixConfig(void)
#ifdef USE_DYN_LPF
//Prevent invalid dynamic lowpass
if (pidProfilesMutable(i)->dyn_lpf_dterm_min_hz > pidProfilesMutable(i)->dyn_lpf_dterm_max_hz) {
pidProfilesMutable(i)->dyn_lpf_dterm_min_hz = 0;
if (pidProfilesMutable(i)->dterm_lpf1_dyn_min_hz > pidProfilesMutable(i)->dterm_lpf1_dyn_max_hz) {
pidProfilesMutable(i)->dterm_lpf1_dyn_min_hz = 0;
}
#endif
@ -608,11 +608,11 @@ void validateAndFixGyroConfig(void)
{
// Fix gyro filter settings to handle cases where an older configurator was used that
// allowed higher cutoff limits from previous firmware versions.
adjustFilterLimit(&gyroConfigMutable()->gyro_lowpass_hz, FILTER_FREQUENCY_MAX);
adjustFilterLimit(&gyroConfigMutable()->gyro_lowpass2_hz, FILTER_FREQUENCY_MAX);
adjustFilterLimit(&gyroConfigMutable()->gyro_soft_notch_hz_1, FILTER_FREQUENCY_MAX);
adjustFilterLimit(&gyroConfigMutable()->gyro_lpf1_static_hz, LPF_MAX_HZ);
adjustFilterLimit(&gyroConfigMutable()->gyro_lpf2_static_hz, LPF_MAX_HZ);
adjustFilterLimit(&gyroConfigMutable()->gyro_soft_notch_hz_1, LPF_MAX_HZ);
adjustFilterLimit(&gyroConfigMutable()->gyro_soft_notch_cutoff_1, 0);
adjustFilterLimit(&gyroConfigMutable()->gyro_soft_notch_hz_2, FILTER_FREQUENCY_MAX);
adjustFilterLimit(&gyroConfigMutable()->gyro_soft_notch_hz_2, LPF_MAX_HZ);
adjustFilterLimit(&gyroConfigMutable()->gyro_soft_notch_cutoff_2, 0);
// Prevent invalid notch cutoff
@ -624,8 +624,8 @@ void validateAndFixGyroConfig(void)
}
#ifdef USE_DYN_LPF
//Prevent invalid dynamic lowpass filter
if (gyroConfig()->dyn_lpf_gyro_min_hz > gyroConfig()->dyn_lpf_gyro_max_hz) {
gyroConfigMutable()->dyn_lpf_gyro_min_hz = 0;
if (gyroConfig()->gyro_lpf1_dyn_min_hz > gyroConfig()->gyro_lpf1_dyn_max_hz) {
gyroConfigMutable()->gyro_lpf1_dyn_min_hz = 0;
}
#endif

20
src/main/config/simplified_tuning.c
View file

@ -65,20 +65,20 @@ static void calculateNewPidValues(pidProfile_t *pidProfile)
static void calculateNewDTermFilterValues(pidProfile_t *pidProfile)
{
pidProfile->dyn_lpf_dterm_min_hz = constrain(DYN_LPF_DTERM_MIN_HZ_DEFAULT * pidProfile->simplified_dterm_filter_multiplier / 100, 0, DYN_LPF_FILTER_FREQUENCY_MAX);
pidProfile->dyn_lpf_dterm_max_hz = constrain(DYN_LPF_DTERM_MAX_HZ_DEFAULT * pidProfile->simplified_dterm_filter_multiplier / 100, 0, DYN_LPF_FILTER_FREQUENCY_MAX);
pidProfile->dterm_lowpass2_hz = constrain(DTERM_LOWPASS_2_HZ_DEFAULT * pidProfile->simplified_dterm_filter_multiplier / 100, 0, FILTER_FREQUENCY_MAX);
pidProfile->dterm_filter_type = FILTER_PT1;
pidProfile->dterm_filter2_type = FILTER_PT1;
pidProfile->dterm_lpf1_dyn_min_hz = constrain(DTERM_LPF1_DYN_MIN_HZ_DEFAULT * pidProfile->simplified_dterm_filter_multiplier / 100, 0, DYN_LPF_MAX_HZ);
pidProfile->dterm_lpf1_dyn_max_hz = constrain(DTERM_LPF1_DYN_MAX_HZ_DEFAULT * pidProfile->simplified_dterm_filter_multiplier / 100, 0, DYN_LPF_MAX_HZ);
pidProfile->dterm_lpf2_static_hz = constrain(DTERM_LPF2_HZ_DEFAULT * pidProfile->simplified_dterm_filter_multiplier / 100, 0, LPF_MAX_HZ);
pidProfile->dterm_lpf1_type = FILTER_PT1;
pidProfile->dterm_lpf2_type = FILTER_PT1;
}
static void calculateNewGyroFilterValues()
{
gyroConfigMutable()->dyn_lpf_gyro_min_hz = constrain(DYN_LPF_GYRO_MIN_HZ_DEFAULT * gyroConfig()->simplified_gyro_filter_multiplier / 100, 0, DYN_LPF_FILTER_FREQUENCY_MAX);
gyroConfigMutable()->dyn_lpf_gyro_max_hz = constrain(DYN_LPF_GYRO_MAX_HZ_DEFAULT * gyroConfig()->simplified_gyro_filter_multiplier / 100, 0, DYN_LPF_FILTER_FREQUENCY_MAX);
gyroConfigMutable()->gyro_lowpass2_hz = constrain(GYRO_LOWPASS_2_HZ_DEFAULT * gyroConfig()->simplified_gyro_filter_multiplier / 100, 0, FILTER_FREQUENCY_MAX);
gyroConfigMutable()->gyro_lowpass_type = FILTER_PT1;
gyroConfigMutable()->gyro_lowpass2_type = FILTER_PT1;
gyroConfigMutable()->gyro_lpf1_dyn_min_hz = constrain(GYRO_LPF1_DYN_MIN_HZ_DEFAULT * gyroConfig()->simplified_gyro_filter_multiplier / 100, 0, DYN_LPF_MAX_HZ);
gyroConfigMutable()->gyro_lpf1_dyn_max_hz = constrain(GYRO_LPF1_DYN_MAX_HZ_DEFAULT * gyroConfig()->simplified_gyro_filter_multiplier / 100, 0, DYN_LPF_MAX_HZ);
gyroConfigMutable()->gyro_lpf2_static_hz = constrain(GYRO_LPF2_HZ_DEFAULT * gyroConfig()->simplified_gyro_filter_multiplier / 100, 0, LPF_MAX_HZ);
gyroConfigMutable()->gyro_lpf1_type = FILTER_PT1;
gyroConfigMutable()->gyro_lpf2_type = FILTER_PT1;
}
void applySimplifiedTuning(pidProfile_t *pidProfile)

14
src/main/flight/pid.c
View file

@ -173,15 +173,15 @@ void resetPidProfile(pidProfile_t *pidProfile)
.abs_control_error_limit = 20,
.abs_control_cutoff = 11,
.antiGravityMode = ANTI_GRAVITY_SMOOTH,
.dterm_lowpass_hz = 150, // NOTE: dynamic lpf is enabled by default so this setting is actually
.dterm_lpf1_static_hz = 150, // NOTE: dynamic lpf is enabled by default so this setting is actually
// overridden and the static lowpass 1 is disabled. We can't set this
// value to 0 otherwise Configurator versions 10.4 and earlier will also
// reset the lowpass filter type to PT1 overriding the desired BIQUAD setting.
.dterm_lowpass2_hz = DTERM_LOWPASS_2_HZ_DEFAULT, // second Dterm LPF ON by default
.dterm_filter_type = FILTER_PT1,
.dterm_filter2_type = FILTER_PT1,
.dyn_lpf_dterm_min_hz = DYN_LPF_DTERM_MIN_HZ_DEFAULT,
.dyn_lpf_dterm_max_hz = DYN_LPF_DTERM_MAX_HZ_DEFAULT,
.dterm_lpf2_static_hz = DTERM_LPF2_HZ_DEFAULT, // second Dterm LPF ON by default
.dterm_lpf1_type = FILTER_PT1,
.dterm_lpf2_type = FILTER_PT1,
.dterm_lpf1_dyn_min_hz = DTERM_LPF1_DYN_MIN_HZ_DEFAULT,
.dterm_lpf1_dyn_max_hz = DTERM_LPF1_DYN_MAX_HZ_DEFAULT,
.launchControlMode = LAUNCH_CONTROL_MODE_NORMAL,
.launchControlThrottlePercent = 20,
.launchControlAngleLimit = 0,
@ -207,7 +207,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
.feedforward_smooth_factor = 25,
.feedforward_jitter_factor = 7,
.feedforward_boost = 15,
.dyn_lpf_curve_expo = 5,
.dterm_lpf1_dyn_expo = 5,
.level_race_mode = false,
.vbat_sag_compensation = 0,
.simplified_pids_mode = PID_SIMPLIFIED_TUNING_RPY,

22
src/main/flight/pid.h
View file

@ -37,7 +37,7 @@
#define PIDSUM_LIMIT_MAX 1000
#define PID_GAIN_MAX 250
#define F_GAIN_MAX 2000
#define F_GAIN_MAX 1000
#define D_MIN_GAIN_MAX 250
// Scaling factors for Pids for better tunable range in configurator for betaflight pid controller. The scaling is based on legacy pid controller or previous float
@ -63,9 +63,9 @@
#define PID_YAW_DEFAULT { 45, 80, 0, 120 }
#define D_MIN_DEFAULT { 30, 30, 0 }
#define DYN_LPF_DTERM_MIN_HZ_DEFAULT 75
#define DYN_LPF_DTERM_MAX_HZ_DEFAULT 150
#define DTERM_LOWPASS_2_HZ_DEFAULT 150
#define DTERM_LPF1_DYN_MIN_HZ_DEFAULT 75
#define DTERM_LPF1_DYN_MAX_HZ_DEFAULT 150
#define DTERM_LPF2_HZ_DEFAULT 150
typedef enum {
PID_ROLL,
@ -132,13 +132,13 @@ typedef enum feedforwardAveraging_e {
typedef struct pidProfile_s {
uint16_t yaw_lowpass_hz; // Additional yaw filter when yaw axis too noisy
uint16_t dterm_lowpass_hz; // Delta Filter in hz
uint16_t dterm_lpf1_static_hz; // Static Dterm lowpass 1 filter cutoff value in hz
uint16_t dterm_notch_hz; // Biquad dterm notch hz
uint16_t dterm_notch_cutoff; // Biquad dterm notch low cutoff
pidf_t pid[PID_ITEM_COUNT];
uint8_t dterm_filter_type; // Filter selection for dterm
uint8_t dterm_lpf1_type; // Filter type for dterm lowpass 1
uint8_t itermWindupPointPercent; // iterm windup threshold, percent motor saturation
uint16_t pidSumLimit;
uint16_t pidSumLimitYaw;
@ -163,7 +163,7 @@ typedef struct pidProfile_s {
uint8_t crash_recovery_rate; // degree/second
uint16_t crash_limit_yaw; // limits yaw errorRate, so crashes don't cause huge throttle increase
uint16_t itermLimit;
uint16_t dterm_lowpass2_hz; // Extra PT1 Filter on D in hz
uint16_t dterm_lpf2_static_hz; // Static Dterm lowpass 2 filter cutoff value in hz
uint8_t crash_recovery; // off, on, on and beeps when it is in crash recovery mode
uint8_t throttle_boost; // how much should throttle be boosted during transient changes 0-100, 100 adds 10x hpf filtered throttle
uint8_t throttle_boost_cutoff; // Which cutoff frequency to use for throttle boost. higher cutoffs keep the boost on for shorter. Specified in hz.
@ -179,9 +179,9 @@ typedef struct pidProfile_s {
uint8_t abs_control_limit; // Limit to the correction
uint8_t abs_control_error_limit; // Limit to the accumulated error
uint8_t abs_control_cutoff; // Cutoff frequency for path estimation in abs control
uint8_t dterm_filter2_type; // Filter selection for 2nd dterm
uint16_t dyn_lpf_dterm_min_hz;
uint16_t dyn_lpf_dterm_max_hz;
uint8_t dterm_lpf2_type; // Filter type for 2nd dterm lowpass
uint16_t dterm_lpf1_dyn_min_hz; // Dterm lowpass filter 1 min hz when in dynamic mode
uint16_t dterm_lpf1_dyn_max_hz; // Dterm lowpass filter 1 max hz when in dynamic mode
uint8_t launchControlMode; // Whether launch control is limited to pitch only (launch stand or top-mount) or all axes (on battery)
uint8_t launchControlThrottlePercent; // Throttle percentage to trigger launch for launch control
uint8_t launchControlAngleLimit; // Optional launch control angle limit (requires ACC)
@ -211,7 +211,7 @@ typedef struct pidProfile_s {
uint8_t feedforward_boost; // amount of setpoint acceleration to add to feedforward, 10 means 100% added
uint8_t feedforward_max_rate_limit; // Maximum setpoint rate percentage for feedforward
uint8_t dyn_lpf_curve_expo; // set the curve for dynamic dterm lowpass filter
uint8_t dterm_lpf1_dyn_expo; // set the curve for dynamic dterm lowpass filter
uint8_t level_race_mode; // NFE race mode - when true pitch setpoint calculation is gyro based in level mode
uint8_t vbat_sag_compensation; // Reduce motor output by this percentage of the maximum compensation amount

44
src/main/flight/pid_init.c
View file

@ -102,31 +102,31 @@ void pidInitFilters(const pidProfile_t *pidProfile)
}
//1st Dterm Lowpass Filter
uint16_t dterm_lowpass_hz = pidProfile->dterm_lowpass_hz;
uint16_t dterm_lpf1_init_hz = pidProfile->dterm_lpf1_static_hz;
#ifdef USE_DYN_LPF
if (pidProfile->dyn_lpf_dterm_min_hz) {
dterm_lowpass_hz = pidProfile->dyn_lpf_dterm_min_hz;
if (pidProfile->dterm_lpf1_dyn_min_hz) {
dterm_lpf1_init_hz = pidProfile->dterm_lpf1_dyn_min_hz;
}
#endif
if (dterm_lowpass_hz > 0) {
switch (pidProfile->dterm_filter_type) {
if (dterm_lpf1_init_hz > 0) {
switch (pidProfile->dterm_lpf1_type) {
case FILTER_PT1:
pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply;
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
pt1FilterInit(&pidRuntime.dtermLowpass[axis].pt1Filter, pt1FilterGain(dterm_lowpass_hz, pidRuntime.dT));
pt1FilterInit(&pidRuntime.dtermLowpass[axis].pt1Filter, pt1FilterGain(dterm_lpf1_init_hz, pidRuntime.dT));
}
break;
case FILTER_BIQUAD:
if (pidProfile->dterm_lowpass_hz < pidFrequencyNyquist) {
if (pidProfile->dterm_lpf1_static_hz < pidFrequencyNyquist) {
#ifdef USE_DYN_LPF
pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1;
#else
pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApply;
#endif
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
biquadFilterInitLPF(&pidRuntime.dtermLowpass[axis].biquadFilter, dterm_lowpass_hz, targetPidLooptime);
biquadFilterInitLPF(&pidRuntime.dtermLowpass[axis].biquadFilter, dterm_lpf1_init_hz, targetPidLooptime);
}
} else {
pidRuntime.dtermLowpassApplyFn = nullFilterApply;
@ -135,13 +135,13 @@ void pidInitFilters(const pidProfile_t *pidProfile)
case FILTER_PT2:
pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)pt2FilterApply;
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
pt2FilterInit(&pidRuntime.dtermLowpass[axis].pt2Filter, pt2FilterGain(dterm_lowpass_hz, pidRuntime.dT));
pt2FilterInit(&pidRuntime.dtermLowpass[axis].pt2Filter, pt2FilterGain(dterm_lpf1_init_hz, pidRuntime.dT));
}
break;
case FILTER_PT3:
pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)pt3FilterApply;
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
pt3FilterInit(&pidRuntime.dtermLowpass[axis].pt3Filter, pt3FilterGain(dterm_lowpass_hz, pidRuntime.dT));
pt3FilterInit(&pidRuntime.dtermLowpass[axis].pt3Filter, pt3FilterGain(dterm_lpf1_init_hz, pidRuntime.dT));
}
break;
default:
@ -153,19 +153,19 @@ void pidInitFilters(const pidProfile_t *pidProfile)
}
//2nd Dterm Lowpass Filter
if (pidProfile->dterm_lowpass2_hz > 0) {
switch (pidProfile->dterm_filter2_type) {
if (pidProfile->dterm_lpf2_static_hz > 0) {
switch (pidProfile->dterm_lpf2_type) {
case FILTER_PT1:
pidRuntime.dtermLowpass2ApplyFn = (filterApplyFnPtr)pt1FilterApply;
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
pt1FilterInit(&pidRuntime.dtermLowpass2[axis].pt1Filter, pt1FilterGain(pidProfile->dterm_lowpass2_hz, pidRuntime.dT));
pt1FilterInit(&pidRuntime.dtermLowpass2[axis].pt1Filter, pt1FilterGain(pidProfile->dterm_lpf2_static_hz, pidRuntime.dT));
}
break;
case FILTER_BIQUAD:
if (pidProfile->dterm_lowpass2_hz < pidFrequencyNyquist) {
if (pidProfile->dterm_lpf2_static_hz < pidFrequencyNyquist) {
pidRuntime.dtermLowpass2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
biquadFilterInitLPF(&pidRuntime.dtermLowpass2[axis].biquadFilter, pidProfile->dterm_lowpass2_hz, targetPidLooptime);
biquadFilterInitLPF(&pidRuntime.dtermLowpass2[axis].biquadFilter, pidProfile->dterm_lpf2_static_hz, targetPidLooptime);
}
} else {
pidRuntime.dtermLowpassApplyFn = nullFilterApply;
@ -174,13 +174,13 @@ void pidInitFilters(const pidProfile_t *pidProfile)
case FILTER_PT2:
pidRuntime.dtermLowpass2ApplyFn = (filterApplyFnPtr)pt2FilterApply;
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
pt2FilterInit(&pidRuntime.dtermLowpass2[axis].pt2Filter, pt2FilterGain(pidProfile->dterm_lowpass2_hz, pidRuntime.dT));
pt2FilterInit(&pidRuntime.dtermLowpass2[axis].pt2Filter, pt2FilterGain(pidProfile->dterm_lpf2_static_hz, pidRuntime.dT));
}
break;
case FILTER_PT3:
pidRuntime.dtermLowpass2ApplyFn = (filterApplyFnPtr)pt3FilterApply;
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
pt3FilterInit(&pidRuntime.dtermLowpass2[axis].pt3Filter, pt3FilterGain(pidProfile->dterm_lowpass2_hz, pidRuntime.dT));
pt3FilterInit(&pidRuntime.dtermLowpass2[axis].pt3Filter, pt3FilterGain(pidProfile->dterm_lpf2_static_hz, pidRuntime.dT));
}
break;
default:
@ -350,8 +350,8 @@ void pidInitConfig(const pidProfile_t *pidProfile)
#endif
#ifdef USE_DYN_LPF
if (pidProfile->dyn_lpf_dterm_min_hz > 0) {
switch (pidProfile->dterm_filter_type) {
if (pidProfile->dterm_lpf1_dyn_min_hz > 0) {
switch (pidProfile->dterm_lpf1_type) {
case FILTER_PT1:
pidRuntime.dynLpfFilter = DYN_LPF_PT1;
break;
@ -371,9 +371,9 @@ void pidInitConfig(const pidProfile_t *pidProfile)
} else {
pidRuntime.dynLpfFilter = DYN_LPF_NONE;
}
pidRuntime.dynLpfMin = pidProfile->dyn_lpf_dterm_min_hz;
pidRuntime.dynLpfMax = pidProfile->dyn_lpf_dterm_max_hz;
pidRuntime.dynLpfCurveExpo = pidProfile->dyn_lpf_curve_expo;
pidRuntime.dynLpfMin = pidProfile->dterm_lpf1_dyn_min_hz;
pidRuntime.dynLpfMax = pidProfile->dterm_lpf1_dyn_max_hz;
pidRuntime.dynLpfCurveExpo = pidProfile->dterm_lpf1_dyn_expo;
#endif
#ifdef USE_LAUNCH_CONTROL

56
src/main/msp/msp.c
View file

@ -1749,8 +1749,8 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
break;
case MSP_FILTER_CONFIG :
sbufWriteU8(dst, gyroConfig()->gyro_lowpass_hz);
sbufWriteU16(dst, currentPidProfile->dterm_lowpass_hz);
sbufWriteU8(dst, gyroConfig()->gyro_lpf1_static_hz);
sbufWriteU16(dst, currentPidProfile->dterm_lpf1_static_hz);
sbufWriteU16(dst, currentPidProfile->yaw_lowpass_hz);
sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_hz_1);
sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_cutoff_1);
@ -1758,21 +1758,21 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
sbufWriteU16(dst, currentPidProfile->dterm_notch_cutoff);
sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_hz_2);
sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_cutoff_2);
sbufWriteU8(dst, currentPidProfile->dterm_filter_type);
sbufWriteU8(dst, currentPidProfile->dterm_lpf1_type);
sbufWriteU8(dst, gyroConfig()->gyro_hardware_lpf);
sbufWriteU8(dst, 0); // DEPRECATED: gyro_32khz_hardware_lpf
sbufWriteU16(dst, gyroConfig()->gyro_lowpass_hz);
sbufWriteU16(dst, gyroConfig()->gyro_lowpass2_hz);
sbufWriteU8(dst, gyroConfig()->gyro_lowpass_type);
sbufWriteU8(dst, gyroConfig()->gyro_lowpass2_type);
sbufWriteU16(dst, currentPidProfile->dterm_lowpass2_hz);
sbufWriteU16(dst, gyroConfig()->gyro_lpf1_static_hz);
sbufWriteU16(dst, gyroConfig()->gyro_lpf2_static_hz);
sbufWriteU8(dst, gyroConfig()->gyro_lpf1_type);
sbufWriteU8(dst, gyroConfig()->gyro_lpf2_type);
sbufWriteU16(dst, currentPidProfile->dterm_lpf2_static_hz);
// Added in MSP API 1.41
sbufWriteU8(dst, currentPidProfile->dterm_filter2_type);
sbufWriteU8(dst, currentPidProfile->dterm_lpf2_type);
#if defined(USE_DYN_LPF)
sbufWriteU16(dst, gyroConfig()->dyn_lpf_gyro_min_hz);
sbufWriteU16(dst, gyroConfig()->dyn_lpf_gyro_max_hz);
sbufWriteU16(dst, currentPidProfile->dyn_lpf_dterm_min_hz);
sbufWriteU16(dst, currentPidProfile->dyn_lpf_dterm_max_hz);
sbufWriteU16(dst, gyroConfig()->gyro_lpf1_dyn_min_hz);
sbufWriteU16(dst, gyroConfig()->gyro_lpf1_dyn_max_hz);
sbufWriteU16(dst, currentPidProfile->dterm_lpf1_dyn_min_hz);
sbufWriteU16(dst, currentPidProfile->dterm_lpf1_dyn_max_hz);
#else
sbufWriteU16(dst, 0);
sbufWriteU16(dst, 0);
@ -1806,7 +1806,7 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
#endif
#if defined(USE_DYN_LPF)
// Added in MSP API 1.44
sbufWriteU8(dst, currentPidProfile->dyn_lpf_curve_expo);
sbufWriteU8(dst, currentPidProfile->dterm_lpf1_dyn_expo);
#else
sbufWriteU8(dst, 0);
#endif
@ -2622,8 +2622,8 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
break;
case MSP_SET_FILTER_CONFIG:
gyroConfigMutable()->gyro_lowpass_hz = sbufReadU8(src);
currentPidProfile->dterm_lowpass_hz = sbufReadU16(src);
gyroConfigMutable()->gyro_lpf1_static_hz = sbufReadU8(src);
currentPidProfile->dterm_lpf1_static_hz = sbufReadU16(src);
currentPidProfile->yaw_lowpass_hz = sbufReadU16(src);
if (sbufBytesRemaining(src) >= 8) {
gyroConfigMutable()->gyro_soft_notch_hz_1 = sbufReadU16(src);
@ -2636,25 +2636,25 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
gyroConfigMutable()->gyro_soft_notch_cutoff_2 = sbufReadU16(src);
}
if (sbufBytesRemaining(src) >= 1) {
currentPidProfile->dterm_filter_type = sbufReadU8(src);
currentPidProfile->dterm_lpf1_type = sbufReadU8(src);
}
if (sbufBytesRemaining(src) >= 10) {
gyroConfigMutable()->gyro_hardware_lpf = sbufReadU8(src);
sbufReadU8(src); // DEPRECATED: gyro_32khz_hardware_lpf
gyroConfigMutable()->gyro_lowpass_hz = sbufReadU16(src);
gyroConfigMutable()->gyro_lowpass2_hz = sbufReadU16(src);
gyroConfigMutable()->gyro_lowpass_type = sbufReadU8(src);
gyroConfigMutable()->gyro_lowpass2_type = sbufReadU8(src);
currentPidProfile->dterm_lowpass2_hz = sbufReadU16(src);
gyroConfigMutable()->gyro_lpf1_static_hz = sbufReadU16(src);
gyroConfigMutable()->gyro_lpf2_static_hz = sbufReadU16(src);
gyroConfigMutable()->gyro_lpf1_type = sbufReadU8(src);
gyroConfigMutable()->gyro_lpf2_type = sbufReadU8(src);
currentPidProfile->dterm_lpf2_static_hz = sbufReadU16(src);
}
if (sbufBytesRemaining(src) >= 9) {
// Added in MSP API 1.41
currentPidProfile->dterm_filter2_type = sbufReadU8(src);
currentPidProfile->dterm_lpf2_type = sbufReadU8(src);
#if defined(USE_DYN_LPF)
gyroConfigMutable()->dyn_lpf_gyro_min_hz = sbufReadU16(src);
gyroConfigMutable()->dyn_lpf_gyro_max_hz = sbufReadU16(src);
currentPidProfile->dyn_lpf_dterm_min_hz = sbufReadU16(src);
currentPidProfile->dyn_lpf_dterm_max_hz = sbufReadU16(src);
gyroConfigMutable()->gyro_lpf1_dyn_min_hz = sbufReadU16(src);
gyroConfigMutable()->gyro_lpf1_dyn_max_hz = sbufReadU16(src);
currentPidProfile->dterm_lpf1_dyn_min_hz = sbufReadU16(src);
currentPidProfile->dterm_lpf1_dyn_max_hz = sbufReadU16(src);
#else
sbufReadU16(src);
sbufReadU16(src);
@ -2694,7 +2694,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
if (sbufBytesRemaining(src) >= 2) {
// Added in MSP API 1.44
#if defined(USE_DYN_LPF)
currentPidProfile->dyn_lpf_curve_expo = sbufReadU8(src);
currentPidProfile->dterm_lpf1_dyn_expo = sbufReadU8(src);
#else
sbufReadU8(src);
#endif

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

@ -109,13 +109,13 @@ void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig)
gyroConfig->gyroCalibrationDuration = 125; // 1.25 seconds
gyroConfig->gyroMovementCalibrationThreshold = 48;
gyroConfig->gyro_hardware_lpf = GYRO_HARDWARE_LPF_NORMAL;
gyroConfig->gyro_lowpass_type = FILTER_PT1;
gyroConfig->gyro_lowpass_hz = 250; // NOTE: dynamic lpf is enabled by default so this setting is actually
gyroConfig->gyro_lpf1_type = FILTER_PT1;
gyroConfig->gyro_lpf1_static_hz = 250; // NOTE: dynamic lpf is enabled by default so this setting is actually
// overridden and the static lowpass 1 is disabled. We can't set this
// value to 0 otherwise Configurator versions 10.4 and earlier will also
// reset the lowpass filter type to PT1 overriding the desired BIQUAD setting.
gyroConfig->gyro_lowpass2_type = FILTER_PT1;
gyroConfig->gyro_lowpass2_hz = GYRO_LOWPASS_2_HZ_DEFAULT;
gyroConfig->gyro_lpf2_type = FILTER_PT1;
gyroConfig->gyro_lpf2_static_hz = GYRO_LPF2_HZ_DEFAULT;
gyroConfig->gyro_high_fsr = false;
gyroConfig->gyro_to_use = GYRO_CONFIG_USE_GYRO_DEFAULT;
gyroConfig->gyro_soft_notch_hz_1 = 0;
@ -126,10 +126,10 @@ void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig)
gyroConfig->gyro_offset_yaw = 0;
gyroConfig->yaw_spin_recovery = YAW_SPIN_RECOVERY_AUTO;
gyroConfig->yaw_spin_threshold = 1950;
gyroConfig->dyn_lpf_gyro_min_hz = DYN_LPF_GYRO_MIN_HZ_DEFAULT;
gyroConfig->dyn_lpf_gyro_max_hz = DYN_LPF_GYRO_MAX_HZ_DEFAULT;
gyroConfig->gyro_lpf1_dyn_min_hz = GYRO_LPF1_DYN_MIN_HZ_DEFAULT;
gyroConfig->gyro_lpf1_dyn_max_hz = GYRO_LPF1_DYN_MAX_HZ_DEFAULT;
gyroConfig->gyro_filter_debug_axis = FD_ROLL;
gyroConfig->dyn_lpf_curve_expo = 5;
gyroConfig->gyro_lpf1_dyn_expo = 5;
gyroConfig->simplified_gyro_filter = true;
gyroConfig->simplified_gyro_filter_multiplier = SIMPLIFIED_TUNING_DEFAULT;
}

28
src/main/sensors/gyro.h
View file

@ -36,12 +36,12 @@
#include "pg/pg.h"
#define FILTER_FREQUENCY_MAX 1000 // so little filtering above 1000hz that if the user wants less delay, they must disable the filter
#define DYN_LPF_FILTER_FREQUENCY_MAX 1000
#define LPF_MAX_HZ 1000 // so little filtering above 1000hz that if the user wants less delay, they must disable the filter
#define DYN_LPF_MAX_HZ 1000
#define DYN_LPF_GYRO_MIN_HZ_DEFAULT 250
#define DYN_LPF_GYRO_MAX_HZ_DEFAULT 500
#define GYRO_LOWPASS_2_HZ_DEFAULT 500
#define GYRO_LPF1_DYN_MIN_HZ_DEFAULT 250
#define GYRO_LPF1_DYN_MAX_HZ_DEFAULT 500
#define GYRO_LPF2_HZ_DEFAULT 500
#ifdef USE_YAW_SPIN_RECOVERY
#define YAW_SPIN_RECOVERY_THRESHOLD_MIN 500
@ -157,8 +157,8 @@ typedef enum {
#define GYRO_CONFIG_USE_GYRO_BOTH 2
enum {
FILTER_LOWPASS = 0,
FILTER_LOWPASS2
FILTER_LPF1 = 0,
FILTER_LPF2
};
typedef struct gyroConfig_s {
@ -168,8 +168,8 @@ typedef struct gyroConfig_s {
uint8_t gyro_high_fsr;
uint8_t gyro_to_use;
uint16_t gyro_lowpass_hz;
uint16_t gyro_lowpass2_hz;
uint16_t gyro_lpf1_static_hz;
uint16_t gyro_lpf2_static_hz;
uint16_t gyro_soft_notch_hz_1;
uint16_t gyro_soft_notch_cutoff_1;
@ -179,21 +179,21 @@ typedef struct gyroConfig_s {
uint8_t checkOverflow;
// Lowpass primary/secondary
uint8_t gyro_lowpass_type;
uint8_t gyro_lowpass2_type;
uint8_t gyro_lpf1_type;
uint8_t gyro_lpf2_type;
uint8_t yaw_spin_recovery;
int16_t yaw_spin_threshold;
uint16_t gyroCalibrationDuration; // Gyro calibration duration in 1/100 second
uint16_t dyn_lpf_gyro_min_hz;
uint16_t dyn_lpf_gyro_max_hz;
uint16_t gyro_lpf1_dyn_min_hz;
uint16_t gyro_lpf1_dyn_max_hz;
uint8_t gyro_filter_debug_axis;
uint8_t gyrosDetected; // What gyros should detection be attempted for on startup. Automatically set on first startup.
uint8_t dyn_lpf_curve_expo; // set the curve for dynamic gyro lowpass filter
uint8_t gyro_lpf1_dyn_expo; // set the curve for dynamic gyro lowpass filter
uint8_t simplified_gyro_filter;
uint8_t simplified_gyro_filter_multiplier;
} gyroConfig_t;

32
src/main/sensors/gyro_init.c
View file

@ -130,12 +130,12 @@ static bool gyroInitLowpassFilterLpf(int slot, int type, uint16_t lpfHz, uint32_
gyroLowpassFilter_t *lowpassFilter = NULL;
switch (slot) {
case FILTER_LOWPASS:
case FILTER_LPF1:
lowpassFilterApplyFn = &gyro.lowpassFilterApplyFn;
lowpassFilter = gyro.lowpassFilter;
break;
case FILTER_LOWPASS2:
case FILTER_LPF2:
lowpassFilterApplyFn = &gyro.lowpass2FilterApplyFn;
lowpassFilter = gyro.lowpass2Filter;
break;
@ -202,8 +202,8 @@ static bool gyroInitLowpassFilterLpf(int slot, int type, uint16_t lpfHz, uint32_
#ifdef USE_DYN_LPF
static void dynLpfFilterInit()
{
if (gyroConfig()->dyn_lpf_gyro_min_hz > 0) {
switch (gyroConfig()->gyro_lowpass_type) {
if (gyroConfig()->gyro_lpf1_dyn_min_hz > 0) {
switch (gyroConfig()->gyro_lpf1_type) {
case FILTER_PT1:
gyro.dynLpfFilter = DYN_LPF_PT1;
break;
@ -223,33 +223,33 @@ static void dynLpfFilterInit()
} else {
gyro.dynLpfFilter = DYN_LPF_NONE;
}
gyro.dynLpfMin = gyroConfig()->dyn_lpf_gyro_min_hz;
gyro.dynLpfMax = gyroConfig()->dyn_lpf_gyro_max_hz;
gyro.dynLpfCurveExpo = gyroConfig()->dyn_lpf_curve_expo;
gyro.dynLpfMin = gyroConfig()->gyro_lpf1_dyn_min_hz;
gyro.dynLpfMax = gyroConfig()->gyro_lpf1_dyn_max_hz;
gyro.dynLpfCurveExpo = gyroConfig()->gyro_lpf1_dyn_expo;
}
#endif
void gyroInitFilters(void)
{
uint16_t gyro_lowpass_hz = gyroConfig()->gyro_lowpass_hz;
uint16_t gyro_lpf1_init_hz = gyroConfig()->gyro_lpf1_static_hz;
#ifdef USE_DYN_LPF
if (gyroConfig()->dyn_lpf_gyro_min_hz > 0) {
gyro_lowpass_hz = gyroConfig()->dyn_lpf_gyro_min_hz;
if (gyroConfig()->gyro_lpf1_dyn_min_hz > 0) {
gyro_lpf1_init_hz = gyroConfig()->gyro_lpf1_dyn_min_hz;
}
#endif
gyroInitLowpassFilterLpf(
FILTER_LOWPASS,
gyroConfig()->gyro_lowpass_type,
gyro_lowpass_hz,
FILTER_LPF1,
gyroConfig()->gyro_lpf1_type,
gyro_lpf1_init_hz,
gyro.targetLooptime
);
gyro.downsampleFilterEnabled = gyroInitLowpassFilterLpf(
FILTER_LOWPASS2,
gyroConfig()->gyro_lowpass2_type,
gyroConfig()->gyro_lowpass2_hz,
FILTER_LPF2,
gyroConfig()->gyro_lpf2_type,
gyroConfig()->gyro_lpf2_static_hz,
gyro.sampleLooptime
);

2
src/main/target/ALIENWHOOP/config.c
View file

@ -127,7 +127,7 @@ void targetConfiguration(void)
pidProfile->pid[PID_LEVEL].D = 55;
/* Setpoints */
pidProfile->dterm_filter_type = FILTER_BIQUAD;
pidProfile->dterm_lpf1_type = FILTER_BIQUAD;
pidProfile->dterm_notch_hz = 0;
pidProfile->pid[PID_PITCH].F = 100;
pidProfile->pid[PID_ROLL].F = 100;

6
src/test/unit/pid_unittest.cc
View file

@ -134,11 +134,11 @@ void setDefaultTestSettings(void) {
pidProfile->pidSumLimit = PIDSUM_LIMIT;
pidProfile->pidSumLimitYaw = PIDSUM_LIMIT_YAW;
pidProfile->yaw_lowpass_hz = 0;
pidProfile->dterm_lowpass_hz = 100;
pidProfile->dterm_lowpass2_hz = 0;
pidProfile->dterm_lpf1_static_hz = 100;
pidProfile->dterm_lpf2_static_hz = 0;
pidProfile->dterm_notch_hz = 260;
pidProfile->dterm_notch_cutoff = 160;
pidProfile->dterm_filter_type = FILTER_BIQUAD;
pidProfile->dterm_lpf1_type = FILTER_BIQUAD;
pidProfile->itermWindupPointPercent = 50;
pidProfile->pidAtMinThrottle = PID_STABILISATION_ON;
pidProfile->levelAngleLimit = 55;

4
src/test/unit/sensor_gyro_unittest.cc
View file

@ -116,8 +116,8 @@ TEST(SensorGyro, Update)
{
pgResetAll();
// turn off filters
gyroConfigMutable()->gyro_lowpass_hz = 0;
gyroConfigMutable()->gyro_lowpass2_hz = 0;
gyroConfigMutable()->gyro_lpf1_static_hz = 0;
gyroConfigMutable()->gyro_lpf2_static_hz = 0;
gyroConfigMutable()->gyro_soft_notch_hz_1 = 0;
gyroConfigMutable()->gyro_soft_notch_hz_2 = 0;
gyroInit();