mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-23 08:15:30 +03:00
Yaw pterm_cut_hz (derived from pterm_cut_hz)
This commit is contained in:
parent
7a6fbc1702
commit
bb12ad8646
2 changed files with 34 additions and 6 deletions
|
@ -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;
|
||||||
|
|
|
@ -188,7 +188,12 @@ 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) {
|
||||||
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz, dT);
|
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);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// -----calculate I component.
|
// -----calculate I component.
|
||||||
|
@ -298,7 +303,12 @@ 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) {
|
||||||
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz, dT);
|
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);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
delta = (gyroADC[axis] - lastGyro[axis]) / 4;
|
delta = (gyroADC[axis] - lastGyro[axis]) / 4;
|
||||||
|
@ -388,7 +398,12 @@ 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) {
|
||||||
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz, dT);
|
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);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
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
|
||||||
|
@ -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) {
|
||||||
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz, dT);
|
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);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
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,7 +801,12 @@ 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) {
|
||||||
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz, dT);
|
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);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// -----calculate I component
|
// -----calculate I component
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue