diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index baf7e9b597..2fb23d1499 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1268,8 +1268,8 @@ static bool blackboxWriteSysinfo(void) currentPidProfile->pid[PID_VEL].I, currentPidProfile->pid[PID_VEL].D); BLACKBOX_PRINT_HEADER_LINE("dterm_filter_type", "%d", currentPidProfile->dterm_filter_type); - BLACKBOX_PRINT_HEADER_LINE("dterm_lpf_hz", "%d", currentPidProfile->dterm_lpf_hz); - BLACKBOX_PRINT_HEADER_LINE("yaw_lpf_hz", "%d", currentPidProfile->yaw_lpf_hz); + BLACKBOX_PRINT_HEADER_LINE("dterm_lowpass_hz", "%d", currentPidProfile->dterm_lowpass_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); BLACKBOX_PRINT_HEADER_LINE("iterm_windup", "%d", currentPidProfile->itermWindupPointPercent); diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 3462de3470..a97730f507 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -358,18 +358,18 @@ static CMS_Menu cmsx_menuFilterGlobal = { .entries = cmsx_menuFilterGlobalEntries, }; -static uint16_t cmsx_dterm_lpf_hz; +static uint16_t cmsx_dterm_lowpass_hz; static uint16_t cmsx_dterm_notch_hz; static uint16_t cmsx_dterm_notch_cutoff; -static uint16_t cmsx_yaw_lpf_hz; +static uint16_t cmsx_yaw_lowpass_hz; static long cmsx_FilterPerProfileRead(void) { const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex); - cmsx_dterm_lpf_hz = pidProfile->dterm_lpf_hz; + cmsx_dterm_lowpass_hz = pidProfile->dterm_lowpass_hz; cmsx_dterm_notch_hz = pidProfile->dterm_notch_hz; cmsx_dterm_notch_cutoff = pidProfile->dterm_notch_cutoff; - cmsx_yaw_lpf_hz = pidProfile->yaw_lpf_hz; + cmsx_yaw_lowpass_hz = pidProfile->yaw_lowpass_hz; return 0; } @@ -379,10 +379,10 @@ static long cmsx_FilterPerProfileWriteback(const OSD_Entry *self) UNUSED(self); pidProfile_t *pidProfile = currentPidProfile; - pidProfile->dterm_lpf_hz = cmsx_dterm_lpf_hz; + pidProfile->dterm_lowpass_hz = cmsx_dterm_lowpass_hz; pidProfile->dterm_notch_hz = cmsx_dterm_notch_hz; pidProfile->dterm_notch_cutoff = cmsx_dterm_notch_cutoff; - pidProfile->yaw_lpf_hz = cmsx_yaw_lpf_hz; + pidProfile->yaw_lowpass_hz = cmsx_yaw_lowpass_hz; return 0; } @@ -391,10 +391,10 @@ static OSD_Entry cmsx_menuFilterPerProfileEntries[] = { { "-- FILTER PP --", OME_Label, NULL, NULL, 0 }, - { "DTERM LPF", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_lpf_hz, 0, 500, 1 }, 0 }, + { "DTERM LPF", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_lowpass_hz, 0, 500, 1 }, 0 }, { "DTERM NF", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_notch_hz, 0, 500, 1 }, 0 }, { "DTERM NFCO", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_notch_cutoff, 0, 500, 1 }, 0 }, - { "YAW LPF", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_yaw_lpf_hz, 0, 500, 1 }, 0 }, + { "YAW LPF", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_yaw_lowpass_hz, 0, 500, 1 }, 0 }, { "BACK", OME_Back, NULL, NULL, 0 }, { NULL, OME_END, NULL, NULL, 0 } diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 6861461073..72f5ee90e3 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -103,8 +103,9 @@ void resetPidProfile(pidProfile_t *pidProfile) .pidSumLimit = PIDSUM_LIMIT, .pidSumLimitYaw = PIDSUM_LIMIT_YAW, - .yaw_lpf_hz = 0, - .dterm_lpf_hz = 100, // filtering ON by default + .yaw_lowpass_hz = 0, + .dterm_lowpass_hz = 100, // filtering ON by default + .dterm_lowpass2_hz = 0, // second Dterm LPF OFF by default .dterm_notch_hz = 260, .dterm_notch_cutoff = 160, .dterm_filter_type = FILTER_BIQUAD, @@ -167,20 +168,22 @@ void pidStabilisationState(pidStabilisationState_e pidControllerState) const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH }; -static FAST_RAM filterApplyFnPtr dtermNotchFilterApplyFn; -static FAST_RAM void *dtermFilterNotch[2]; -static FAST_RAM filterApplyFnPtr dtermLpfApplyFn; -static FAST_RAM void *dtermFilterLpf[2]; -static FAST_RAM filterApplyFnPtr ptermYawFilterApplyFn; -static FAST_RAM void *ptermYawFilter; - -typedef union dtermFilterLpf_u { - pt1Filter_t pt1Filter[2]; - biquadFilter_t biquadFilter[2]; +typedef union dtermLowpass_u { + pt1Filter_t pt1Filter; + biquadFilter_t biquadFilter; #if defined(USE_FIR_FILTER_DENOISE) - firFilterDenoise_t denoisingFilter[2]; + firFilterDenoise_t denoisingFilter; #endif -} dtermFilterLpf_t; +} dtermLowpass_t; + +static FAST_RAM filterApplyFnPtr dtermNotchApplyFn; +static FAST_RAM biquadFilter_t dtermNotch[2]; +static FAST_RAM filterApplyFnPtr dtermLowpassApplyFn; +static FAST_RAM dtermLowpass_t dtermLowpass[2]; +static FAST_RAM filterApplyFnPtr dtermLowpass2ApplyFn; +static FAST_RAM pt1Filter_t dtermLowpass2[2]; +static FAST_RAM filterApplyFnPtr ptermYawLowpassApplyFn; +static FAST_RAM pt1Filter_t ptermYawLowpass; void pidInitFilters(const pidProfile_t *pidProfile) { @@ -188,9 +191,9 @@ void pidInitFilters(const pidProfile_t *pidProfile) if (targetPidLooptime == 0) { // no looptime set, so set all the filters to null - dtermNotchFilterApplyFn = nullFilterApply; - dtermLpfApplyFn = nullFilterApply; - ptermYawFilterApplyFn = nullFilterApply; + dtermNotchApplyFn = nullFilterApply; + dtermLowpassApplyFn = nullFilterApply; + ptermYawLowpassApplyFn = nullFilterApply; return; } @@ -208,58 +211,60 @@ void pidInitFilters(const pidProfile_t *pidProfile) } if (dTermNotchHz != 0 && pidProfile->dterm_notch_cutoff != 0) { - static biquadFilter_t biquadFilterNotch[2]; - dtermNotchFilterApplyFn = (filterApplyFnPtr)biquadFilterApply; + dtermNotchApplyFn = (filterApplyFnPtr)biquadFilterApply; const float notchQ = filterGetNotchQ(dTermNotchHz, pidProfile->dterm_notch_cutoff); for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) { - dtermFilterNotch[axis] = &biquadFilterNotch[axis]; - biquadFilterInit(dtermFilterNotch[axis], dTermNotchHz, targetPidLooptime, notchQ, FILTER_NOTCH); + biquadFilterInit(&dtermNotch[axis], dTermNotchHz, targetPidLooptime, notchQ, FILTER_NOTCH); } } else { - dtermNotchFilterApplyFn = nullFilterApply; + dtermNotchApplyFn = nullFilterApply; + } + + //2nd Dterm Lowpass Filter + if (pidProfile->dterm_lowpass2_hz == 0 || pidProfile->dterm_lowpass2_hz > pidFrequencyNyquist) { + dtermLowpass2ApplyFn = nullFilterApply; + } else { + dtermLowpass2ApplyFn = (filterApplyFnPtr)pt1FilterApply; + for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) { + pt1FilterInit(&dtermLowpass2[axis], pidProfile->dterm_lowpass2_hz, dT); + } } - static dtermFilterLpf_t dtermFilterLpfUnion; - if (pidProfile->dterm_lpf_hz == 0 || pidProfile->dterm_lpf_hz > pidFrequencyNyquist) { - dtermLpfApplyFn = nullFilterApply; + if (pidProfile->dterm_lowpass_hz == 0 || pidProfile->dterm_lowpass_hz > pidFrequencyNyquist) { + dtermLowpassApplyFn = nullFilterApply; } else { switch (pidProfile->dterm_filter_type) { default: - dtermLpfApplyFn = nullFilterApply; + dtermLowpassApplyFn = nullFilterApply; break; case FILTER_PT1: - dtermLpfApplyFn = (filterApplyFnPtr)pt1FilterApply; + dtermLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply; for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) { - dtermFilterLpf[axis] = &dtermFilterLpfUnion.pt1Filter[axis]; - pt1FilterInit(dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, dT); + pt1FilterInit(&dtermLowpass[axis].pt1Filter, pidProfile->dterm_lowpass_hz, dT); } break; case FILTER_BIQUAD: - dtermLpfApplyFn = (filterApplyFnPtr)biquadFilterApply; + dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApply; for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) { - dtermFilterLpf[axis] = &dtermFilterLpfUnion.biquadFilter[axis]; - biquadFilterInitLPF(dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime); + biquadFilterInitLPF(&dtermLowpass[axis].biquadFilter, pidProfile->dterm_lowpass_hz, targetPidLooptime); } break; #if defined(USE_FIR_FILTER_DENOISE) case FILTER_FIR: - dtermLpfApplyFn = (filterApplyFnPtr)firFilterDenoiseUpdate; + dtermLowpassApplyFn = (filterApplyFnPtr)firFilterDenoiseUpdate; for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) { - dtermFilterLpf[axis] = &dtermFilterLpfUnion.denoisingFilter[axis]; - firFilterDenoiseInit(dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime); + firFilterDenoiseInit(&dtermLowpass[axis].denoisingFilter, pidProfile->dterm_lowpass_hz, targetPidLooptime); } break; #endif } } - static pt1Filter_t pt1FilterYaw; - if (pidProfile->yaw_lpf_hz == 0 || pidProfile->yaw_lpf_hz > pidFrequencyNyquist) { - ptermYawFilterApplyFn = nullFilterApply; + if (pidProfile->yaw_lowpass_hz == 0 || pidProfile->yaw_lowpass_hz > pidFrequencyNyquist) { + ptermYawLowpassApplyFn = nullFilterApply; } else { - ptermYawFilterApplyFn = (filterApplyFnPtr)pt1FilterApply; - ptermYawFilter = &pt1FilterYaw; - pt1FilterInit(ptermYawFilter, pidProfile->yaw_lpf_hz, dT); + ptermYawLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply; + pt1FilterInit(&ptermYawLowpass, pidProfile->yaw_lowpass_hz, dT); } } @@ -498,7 +503,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an // -----calculate P component and add Dynamic Part based on stick input axisPID_P[axis] = Kp[axis] * errorRate * tpaFactor; if (axis == FD_YAW) { - axisPID_P[axis] = ptermYawFilterApplyFn(ptermYawFilter, axisPID_P[axis]); + axisPID_P[axis] = ptermYawLowpassApplyFn((filter_t *) &ptermYawLowpass, axisPID_P[axis]); } // -----calculate I component @@ -513,8 +518,9 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an // -----calculate D component if (axis != FD_YAW) { // apply filters - float gyroRateFiltered = dtermNotchFilterApplyFn(dtermFilterNotch[axis], gyroRate); - gyroRateFiltered = dtermLpfApplyFn(dtermFilterLpf[axis], gyroRateFiltered); + float gyroRateFiltered = dtermNotchApplyFn((filter_t *) &dtermNotch[axis], gyroRate); + gyroRateFiltered = dtermLowpassApplyFn((filter_t *) &dtermLowpass[axis], gyroRateFiltered); + gyroRateFiltered = dtermLowpass2ApplyFn((filter_t *) &dtermLowpass2[axis], gyroRateFiltered); const float rD = dynCd * MIN(getRcDeflectionAbs(axis) * relaxFactor, 1.0f) * currentPidSetpoint - gyroRateFiltered; // cr - y // Divide rate change by deltaT to get differential (ie dr/dt) diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 15854919e5..4ea4ec4749 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -73,8 +73,8 @@ typedef struct pid8_s { typedef struct pidProfile_s { pid8_t pid[PID_ITEM_COUNT]; - uint16_t yaw_lpf_hz; // Additional yaw filter when yaw axis too noisy - uint16_t dterm_lpf_hz; // Delta Filter in hz + 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_notch_hz; // Biquad dterm notch hz uint16_t dterm_notch_cutoff; // Biquad dterm notch low cutoff uint8_t dterm_filter_type; // Filter selection for dterm @@ -105,6 +105,7 @@ typedef struct pidProfile_s { pidCrashRecovery_e crash_recovery; // off, on, on and beeps when it is in crash recovery mode 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 } pidProfile_t; #ifndef USE_OSD_SLAVE diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c index fc79534464..f2d4b708f7 100644 --- a/src/main/interface/msp.c +++ b/src/main/interface/msp.c @@ -1186,8 +1186,8 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) case MSP_FILTER_CONFIG : sbufWriteU8(dst, gyroConfig()->gyro_soft_lpf_hz); - sbufWriteU16(dst, currentPidProfile->dterm_lpf_hz); - sbufWriteU16(dst, currentPidProfile->yaw_lpf_hz); + sbufWriteU16(dst, currentPidProfile->dterm_lowpass_hz); + sbufWriteU16(dst, currentPidProfile->yaw_lowpass_hz); sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_hz_1); sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_cutoff_1); sbufWriteU16(dst, currentPidProfile->dterm_notch_hz); @@ -1658,8 +1658,8 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) case MSP_SET_FILTER_CONFIG: gyroConfigMutable()->gyro_soft_lpf_hz = sbufReadU8(src); - currentPidProfile->dterm_lpf_hz = sbufReadU16(src); - currentPidProfile->yaw_lpf_hz = sbufReadU16(src); + currentPidProfile->dterm_lowpass_hz = sbufReadU16(src); + currentPidProfile->yaw_lowpass_hz = sbufReadU16(src); if (sbufBytesRemaining(src) >= 8) { gyroConfigMutable()->gyro_soft_notch_hz_1 = sbufReadU16(src); gyroConfigMutable()->gyro_soft_notch_cutoff_1 = sbufReadU16(src); diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 02f678bd7f..9baf01df13 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -652,7 +652,8 @@ const clivalue_t valueTable[] = { // PG_PID_PROFILE { "dterm_lowpass_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LOWPASS_TYPE }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_filter_type) }, - { "dterm_lowpass", VAR_INT16 | PROFILE_VALUE, .config.minmax = { 0, 16000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_lpf_hz) }, + { "dterm_lowpass_hz", VAR_INT16 | PROFILE_VALUE, .config.minmax = { 0, 16000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_lowpass_hz) }, + { "dterm_lowpass2_hz", VAR_INT16 | PROFILE_VALUE, .config.minmax = { 0, 16000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_lowpass2_hz) }, { "dterm_notch_hz", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 16000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_notch_hz) }, { "dterm_notch_cutoff", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 16000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_notch_cutoff) }, { "vbat_pid_gain", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, vbatPidCompensation) }, @@ -677,7 +678,7 @@ const clivalue_t valueTable[] = { { "iterm_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermLimit) }, { "pidsum_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 100, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, pidSumLimit) }, { "pidsum_limit_yaw", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 100, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, pidSumLimitYaw) }, - { "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, yaw_lpf_hz) }, + { "yaw_lowpass_hz", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, yaw_lowpass_hz) }, { "p_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_PITCH].P) }, { "i_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_PITCH].I) },