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

Fix rebase artifacts

This commit is contained in:
borisbstyle 2016-08-05 13:55:09 +02:00
parent 75e8b51d0a
commit 3999fbc95b
1 changed files with 8 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/main/flight/pid_betaflight.c
View File

@ -72,7 +72,7 @@ void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
float errorRate = 0, rP = 0, rD = 0, PVRate = 0; float errorRate = 0, rP = 0, rD = 0, PVRate = 0;
float ITerm,PTerm,DTerm; float ITerm,PTerm,DTerm;
static float lastRateError[2]; static float lastRateError[2];
static float Kp[3], Ki[3], Kd[3], b[3], c[3], yawMaxVelocity, yawPreviousRate; static float Kp[3], Ki[3], Kd[3], b[3], c[3], rollPitchMaxVelocity, yawMaxVelocity, previousSetpoint[3];
float delta; float delta;
int axis; int axis;
float horizonLevelStrength = 1; float horizonLevelStrength = 1;
@ -134,13 +134,14 @@ void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
configD[axis] = pidProfile->D8[axis]; configD[axis] = pidProfile->D8[axis];
} }
// Limit abrupt yaw inputs // Limit abrupt yaw inputs / stops
if (axis == YAW && pidProfile->pidMaxVelocityYaw) { float maxVelocity = (axis == YAW) ? yawMaxVelocity : rollPitchMaxVelocity;
float yawCurrentVelocity = setpointRate[axis] - yawPreviousRate; if (maxVelocity) {
if (ABS(yawCurrentVelocity) > yawMaxVelocity) { float currentVelocity = setpointRate[axis] - previousSetpoint[axis];
setpointRate[axis] = (yawCurrentVelocity > 0) ? yawPreviousRate + yawMaxVelocity : yawPreviousRate - yawMaxVelocity; if (ABS(currentVelocity) > maxVelocity) {
setpointRate[axis] = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity : previousSetpoint[axis] - maxVelocity;
} }
yawPreviousRate = setpointRate[axis]; previousSetpoint[axis] = setpointRate[axis];
} }
// Yaw control is GYRO based, direct sticks control is applied to rate PID // Yaw control is GYRO based, direct sticks control is applied to rate PID