diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 6ca00b0446..8346090a87 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -1041,7 +1041,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim // loop execution to be delayed. const float delta = - (gyroRateDterm[axis] - previousGyroRateDterm[axis]) * pidRuntime.pidFrequency; - float preTpaData = pidRuntime.pidCoefficient[axis].Kd * delta; + float preTpaD = pidRuntime.pidCoefficient[axis].Kd * delta; #if defined(USE_ACC) if (cmpTimeUs(currentTimeUs, levelModeStartTimeUs) > CRASH_RECOVERY_DETECTION_DELAY_US) { @@ -1052,12 +1052,12 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim #if defined(USE_D_MIN) float dMinFactor = 1.0f; if (pidRuntime.dMinPercent[axis] > 0) { - float dMinGyroFactor = biquadFilterApply(&pidRuntime.dMinRange[axis], delta); + float dMinGyroFactor = pt2FilterApply(&pidRuntime.dMinRange[axis], delta); dMinGyroFactor = fabsf(dMinGyroFactor) * pidRuntime.dMinGyroGain; const float dMinSetpointFactor = (fabsf(pidSetpointDelta)) * pidRuntime.dMinSetpointGain; dMinFactor = MAX(dMinGyroFactor, dMinSetpointFactor); dMinFactor = pidRuntime.dMinPercent[axis] + (1.0f - pidRuntime.dMinPercent[axis]) * dMinFactor; - dMinFactor = pt1FilterApply(&pidRuntime.dMinLowpass[axis], dMinFactor); + dMinFactor = pt2FilterApply(&pidRuntime.dMinLowpass[axis], dMinFactor); dMinFactor = MIN(dMinFactor, 1.0f); if (axis == FD_ROLL) { DEBUG_SET(DEBUG_D_MIN, 0, lrintf(dMinGyroFactor * 100)); @@ -1069,17 +1069,17 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim } // Apply the dMinFactor - preTpaData *= dMinFactor; + preTpaD *= dMinFactor; #endif - pidData[axis].D = preTpaData * tpaFactor; + pidData[axis].D = preTpaD * tpaFactor; // Log the value of D pre application of TPA - preTpaData *= D_LPF_FILT_SCALE; + preTpaD *= D_LPF_FILT_SCALE; if (axis == FD_ROLL) { - DEBUG_SET(DEBUG_D_LPF, 2, lrintf(preTpaData)); + DEBUG_SET(DEBUG_D_LPF, 2, lrintf(preTpaD)); } else if (axis == FD_PITCH) { - DEBUG_SET(DEBUG_D_LPF, 3, lrintf(preTpaData)); + DEBUG_SET(DEBUG_D_LPF, 3, lrintf(preTpaD)); } } else { pidData[axis].D = 0; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index f99140cbae..a6600064d2 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -329,8 +329,8 @@ typedef struct pidRuntime_s { #endif #ifdef USE_D_MIN - biquadFilter_t dMinRange[XYZ_AXIS_COUNT]; - pt1Filter_t dMinLowpass[XYZ_AXIS_COUNT]; + pt2Filter_t dMinRange[XYZ_AXIS_COUNT]; + pt2Filter_t dMinLowpass[XYZ_AXIS_COUNT]; float dMinPercent[XYZ_AXIS_COUNT]; float dMinGyroGain; float dMinSetpointGain; diff --git a/src/main/flight/pid_init.c b/src/main/flight/pid_init.c index 72243eadf7..8d1db7711e 100644 --- a/src/main/flight/pid_init.c +++ b/src/main/flight/pid_init.c @@ -46,10 +46,10 @@ #include "pid_init.h" #if defined(USE_D_MIN) -#define D_MIN_RANGE_HZ 80 // Biquad lowpass input cutoff to peak D around propwash frequencies -#define D_MIN_LOWPASS_HZ 10 // PT1 lowpass cutoff to smooth the boost effect -#define D_MIN_GAIN_FACTOR 0.00005f -#define D_MIN_SETPOINT_GAIN_FACTOR 0.00005f +#define D_MIN_RANGE_HZ 85 // PT2 lowpass input cutoff to peak D around propwash frequencies +#define D_MIN_LOWPASS_HZ 35 // PT2 lowpass cutoff to smooth the boost effect +#define D_MIN_GAIN_FACTOR 0.00008f +#define D_MIN_SETPOINT_GAIN_FACTOR 0.00008f #endif #define ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF 15 // The anti gravity throttle highpass filter cutoff @@ -224,8 +224,8 @@ void pidInitFilters(const pidProfile_t *pidProfile) // in-flight adjustments and transition from 0 to > 0 in flight the feature // won't work because the filter wasn't initialized. for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { - biquadFilterInitLPF(&pidRuntime.dMinRange[axis], D_MIN_RANGE_HZ, targetPidLooptime); - pt1FilterInit(&pidRuntime.dMinLowpass[axis], pt1FilterGain(D_MIN_LOWPASS_HZ, pidRuntime.dT)); + pt2FilterInit(&pidRuntime.dMinRange[axis], pt2FilterGain(D_MIN_RANGE_HZ, pidRuntime.dT)); + pt2FilterInit(&pidRuntime.dMinLowpass[axis], pt2FilterGain(D_MIN_LOWPASS_HZ, pidRuntime.dT)); } #endif