rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #10889 from ctzsnooze/dmin-to-pt2-with-cutoff-optimisation

This commit is contained in:
Michael Keller 2021-08-31 23:50:52 +12:00 committed by GitHub
parent c283981d54 ff19f3fecb
commit bdccfc3543
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 16 additions and 16 deletions
Show Stats 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.
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;

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

@ -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;

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

@ -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