Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 12:25:20 +03:00
Code Issues Wiki Activity

D_min smoothing changed to PT2 and re-tuned

Browse source
This commit is contained in:
ctzsnooze 2021-08-31 11:37:33 +10:00
parent 86aa5cc84e
commit ff19f3fecb
3 changed files with 16 additions and 16 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1041,7 +1041,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
// loop execution to be delayed. // loop execution to be delayed.
const float delta = const float delta =
- (gyroRateDterm[axis] - previousGyroRateDterm[axis]) * pidRuntime.pidFrequency; - (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 defined(USE_ACC)
if (cmpTimeUs(currentTimeUs, levelModeStartTimeUs) > CRASH_RECOVERY_DETECTION_DELAY_US) { 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) #if defined(USE_D_MIN)
float dMinFactor = 1.0f; float dMinFactor = 1.0f;
if (pidRuntime.dMinPercent[axis] > 0) { if (pidRuntime.dMinPercent[axis] > 0) {
float dMinGyroFactor = biquadFilterApply(&pidRuntime.dMinRange[axis], delta); float dMinGyroFactor = pt2FilterApply(&pidRuntime.dMinRange[axis], delta);
dMinGyroFactor = fabsf(dMinGyroFactor) * pidRuntime.dMinGyroGain; dMinGyroFactor = fabsf(dMinGyroFactor) * pidRuntime.dMinGyroGain;
const float dMinSetpointFactor = (fabsf(pidSetpointDelta)) * pidRuntime.dMinSetpointGain; const float dMinSetpointFactor = (fabsf(pidSetpointDelta)) * pidRuntime.dMinSetpointGain;
dMinFactor = MAX(dMinGyroFactor, dMinSetpointFactor); dMinFactor = MAX(dMinGyroFactor, dMinSetpointFactor);
dMinFactor = pidRuntime.dMinPercent[axis] + (1.0f - pidRuntime.dMinPercent[axis]) * dMinFactor; 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); dMinFactor = MIN(dMinFactor, 1.0f);
if (axis == FD_ROLL) { if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_D_MIN, 0, lrintf(dMinGyroFactor * 100)); 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 // Apply the dMinFactor
preTpaData *= dMinFactor; preTpaD *= dMinFactor;
#endif #endif
pidData[axis].D = preTpaData * tpaFactor; pidData[axis].D = preTpaD * tpaFactor;
// Log the value of D pre application of TPA // Log the value of D pre application of TPA
preTpaData *= D_LPF_FILT_SCALE; preTpaD *= D_LPF_FILT_SCALE;
if (axis == FD_ROLL) { 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) { } else if (axis == FD_PITCH) {
DEBUG_SET(DEBUG_D_LPF, 3, lrintf(preTpaData)); DEBUG_SET(DEBUG_D_LPF, 3, lrintf(preTpaD));
} }
} else { } else {
pidData[axis].D = 0; pidData[axis].D = 0;

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

@ -329,8 +329,8 @@ typedef struct pidRuntime_s {
#endif #endif
#ifdef USE_D_MIN #ifdef USE_D_MIN
biquadFilter_t dMinRange[XYZ_AXIS_COUNT]; pt2Filter_t dMinRange[XYZ_AXIS_COUNT];
pt1Filter_t dMinLowpass[XYZ_AXIS_COUNT]; pt2Filter_t dMinLowpass[XYZ_AXIS_COUNT];
float dMinPercent[XYZ_AXIS_COUNT]; float dMinPercent[XYZ_AXIS_COUNT];
float dMinGyroGain; float dMinGyroGain;
float dMinSetpointGain; float dMinSetpointGain;

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

@ -46,10 +46,10 @@
#include "pid_init.h" #include "pid_init.h"
#if defined(USE_D_MIN) #if defined(USE_D_MIN)
#define D_MIN_RANGE_HZ 80 // Biquad lowpass input cutoff to peak D around propwash frequencies #define D_MIN_RANGE_HZ 85 // PT2 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_LOWPASS_HZ 35 // PT2 lowpass cutoff to smooth the boost effect
#define D_MIN_GAIN_FACTOR 0.00005f #define D_MIN_GAIN_FACTOR 0.00008f
#define D_MIN_SETPOINT_GAIN_FACTOR 0.00005f #define D_MIN_SETPOINT_GAIN_FACTOR 0.00008f
#endif #endif
#define ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF 15 // The anti gravity throttle highpass filter cutoff #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 // in-flight adjustments and transition from 0 to > 0 in flight the feature
// won't work because the filter wasn't initialized. // won't work because the filter wasn't initialized.
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
biquadFilterInitLPF(&pidRuntime.dMinRange[axis], D_MIN_RANGE_HZ, targetPidLooptime); pt2FilterInit(&pidRuntime.dMinRange[axis], pt2FilterGain(D_MIN_RANGE_HZ, pidRuntime.dT));
pt1FilterInit(&pidRuntime.dMinLowpass[axis], pt1FilterGain(D_MIN_LOWPASS_HZ, pidRuntime.dT)); pt2FilterInit(&pidRuntime.dMinLowpass[axis], pt2FilterGain(D_MIN_LOWPASS_HZ, pidRuntime.dT));
} }
#endif #endif