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

Equal pid.c optimalisation to betaflight pid

Browse source
This commit is contained in:
borisbstyle 2016-08-23 01:06:19 +02:00
parent f9c827c726
commit e6a11960fd
1 changed files with 7 additions and 7 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -286,13 +286,7 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
ITerm = errorGyroIf[axis];
//-----calculate D-term (Yaw D not yet supported)
if (axis == YAW) {
if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT());
axisPID[axis] = lrintf(PTerm + ITerm);
DTerm = 0.0f; // needed for blackbox
} else {
if (axis != YAW) {
rD = c[axis] * setpointRate[axis] - PVRate; // cr - y
delta = rD - lastRateError[axis];
lastRateError[axis] = rD;
@ -317,6 +311,12 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
// -----calculate total PID output
axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -900, 900);
} else {
if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT());
axisPID[axis] = lrintf(PTerm + ITerm);
DTerm = 0.0f; // needed for blackbox
}
// Disable PID control at zero throttle