From 3999fbc95be957f22965f4da7403a09b6f1f9d8b Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 5 Aug 2016 13:55:09 +0200 Subject: [PATCH] Fix rebase artifacts --- src/main/flight/pid_betaflight.c | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/main/flight/pid_betaflight.c b/src/main/flight/pid_betaflight.c index 058e58760..a2dc204d9 100644 --- a/src/main/flight/pid_betaflight.c +++ b/src/main/flight/pid_betaflight.c @@ -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 ITerm,PTerm,DTerm; 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; int axis; float horizonLevelStrength = 1; @@ -134,13 +134,14 @@ void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio configD[axis] = pidProfile->D8[axis]; } - // Limit abrupt yaw inputs - if (axis == YAW && pidProfile->pidMaxVelocityYaw) { - float yawCurrentVelocity = setpointRate[axis] - yawPreviousRate; - if (ABS(yawCurrentVelocity) > yawMaxVelocity) { - setpointRate[axis] = (yawCurrentVelocity > 0) ? yawPreviousRate + yawMaxVelocity : yawPreviousRate - yawMaxVelocity; + // Limit abrupt yaw inputs / stops + float maxVelocity = (axis == YAW) ? yawMaxVelocity : rollPitchMaxVelocity; + if (maxVelocity) { + float currentVelocity = setpointRate[axis] - previousSetpoint[axis]; + 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