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

Yaw pterm_cut_hz (derived from pterm_cut_hz)

Browse source
This commit is contained in:
borisbstyle 2015-08-18 22:11:29 +02:00
parent 7a6fbc1702
commit bb12ad8646
2 changed files with 34 additions and 6 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/common/filter.h
View file

@ -5,6 +5,8 @@
* Author: borisb * Author: borisb
*/ */
#define YAW_PTERM_CUT_RATIO 2
typedef struct filterStatePt1_s { typedef struct filterStatePt1_s {
float state; float state;
float RC; float RC;

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

@ -188,8 +188,13 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// Pterm low pass // Pterm low pass
if (pidProfile->pterm_cut_hz) { if (pidProfile->pterm_cut_hz) {
if (axis == YAW) {
PTerm = filterApplyPt1(PTerm, &PTermState[axis], constrain(pidProfile->pterm_cut_hz / YAW_PTERM_CUT_RATIO, 1, pidProfile->pterm_cut_hz), dT);
}
else {
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz, dT); PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz, dT);
} }
}
// -----calculate I component. // -----calculate I component.
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * 10, -250.0f, 250.0f); errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * 10, -250.0f, 250.0f);
@ -298,8 +303,13 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// Pterm low pass // Pterm low pass
if (pidProfile->pterm_cut_hz) { if (pidProfile->pterm_cut_hz) {
if (axis == YAW) {
PTerm = filterApplyPt1(PTerm, &PTermState[axis], constrain(pidProfile->pterm_cut_hz / YAW_PTERM_CUT_RATIO, 1, pidProfile->pterm_cut_hz), dT);
}
else {
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz, dT); PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz, dT);
} }
}
delta = (gyroADC[axis] - lastGyro[axis]) / 4; delta = (gyroADC[axis] - lastGyro[axis]) / 4;
lastGyro[axis] = gyroADC[axis]; lastGyro[axis] = gyroADC[axis];
@ -388,8 +398,13 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
// Pterm low pass // Pterm low pass
if (pidProfile->pterm_cut_hz) { if (pidProfile->pterm_cut_hz) {
if (axis == YAW) {
PTerm = filterApplyPt1(PTerm, &PTermState[axis], constrain(pidProfile->pterm_cut_hz / YAW_PTERM_CUT_RATIO, 1, pidProfile->pterm_cut_hz), dT);
}
else {
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz, dT); PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz, dT);
} }
}
delta = (gyroADC[axis] - lastGyro[axis]) / 4; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 delta = (gyroADC[axis] - lastGyro[axis]) / 4; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastGyro[axis] = gyroADC[axis]; lastGyro[axis] = gyroADC[axis];
@ -512,8 +527,14 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con
// Pterm low pass // Pterm low pass
if (pidProfile->pterm_cut_hz) { if (pidProfile->pterm_cut_hz) {
if (axis == YAW) {
PTerm = filterApplyPt1(PTerm, &PTermState[axis], constrain(pidProfile->pterm_cut_hz / YAW_PTERM_CUT_RATIO, 1, pidProfile->pterm_cut_hz), dT);
}
else {
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz, dT); PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz, dT);
} }
}
delta = (gyroADC[axis] - lastGyro[axis]) / 4; delta = (gyroADC[axis] - lastGyro[axis]) / 4;
lastGyro[axis] = gyroADC[axis]; lastGyro[axis] = gyroADC[axis];
deltaSum = delta1[axis] + delta2[axis] + delta; deltaSum = delta1[axis] + delta2[axis] + delta;
@ -685,7 +706,7 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
// Pterm low pass // Pterm low pass
if (pidProfile->pterm_cut_hz) { if (pidProfile->pterm_cut_hz) {
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz, dT); PTerm = filterApplyPt1(PTerm, &PTermState[axis], constrain(pidProfile->pterm_cut_hz / YAW_PTERM_CUT_RATIO, 1, pidProfile->pterm_cut_hz), dT);
} }
axisPID[FD_YAW] = PTerm + ITerm; axisPID[FD_YAW] = PTerm + ITerm;
@ -780,8 +801,13 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
// Pterm low pass // Pterm low pass
if (pidProfile->pterm_cut_hz) { if (pidProfile->pterm_cut_hz) {
if (axis == YAW) {
PTerm = filterApplyPt1(PTerm, &PTermState[axis], constrain(pidProfile->pterm_cut_hz / YAW_PTERM_CUT_RATIO, 1, pidProfile->pterm_cut_hz), dT);
}
else {
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz, dT); PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz, dT);
} }
}
// -----calculate I component // -----calculate I component
// there should be no division before accumulating the error to integrator, because the precision would be reduced. // there should be no division before accumulating the error to integrator, because the precision would be reduced.