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

More efficiency in PID code // add yaw velocity to setpoint

Browse source
This commit is contained in:
borisbstyle 2016-08-04 00:50:11 +02:00
parent a64741365c
commit b4e61d49c9
6 changed files with 44 additions and 46 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -133,13 +133,10 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
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;
float delta;
int axis;
float horizonLevelStrength = 1;
static int16_t axisPIDState[3];
static float velocityWindupFactor[3] = { 1.0f, 1.0f, 1.0f };
const float velocityFactor = getdT() * 1000.0f;
float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float
@ -180,13 +177,31 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
// ----------PID controller----------
for (axis = 0; axis < 3; axis++) {
static uint8_t configP[3], configI[3], configD[3];
// Prevent unnecessary computing and check for changed PIDs. No need for individual checks. Only pids is fine for now
// Prepare all parameters for PID controller
float Kp = PTERM_SCALE * pidProfile->P8[axis];
float Ki = ITERM_SCALE * pidProfile->I8[axis];
float Kd = DTERM_SCALE * pidProfile->D8[axis];
float b = pidProfile->ptermSetpointWeight / 100.0f;
float c = pidProfile->dtermSetpointWeight / 100.0f;
float velocityMax = (axis == YAW) ? (float)pidProfile->pidMaxVelocityYaw * velocityFactor : (float)pidProfile->pidMaxVelocity * velocityFactor;
if ((pidProfile->P8[axis] != configP[axis]) || (pidProfile->I8[axis] != configI[axis]) || (pidProfile->D8[axis] != configD[axis])) {
Kp[axis] = PTERM_SCALE * pidProfile->P8[axis];
Ki[axis] = ITERM_SCALE * pidProfile->I8[axis];
Kd[axis] = DTERM_SCALE * pidProfile->D8[axis];
b[axis] = pidProfile->ptermSetpointWeight / 100.0f;
c[axis] = pidProfile->dtermSetpointWeight / 100.0f;
yawMaxVelocity = pidProfile->pidMaxVelocityYaw * 1000 * getdT();
configP[axis] = pidProfile->P8[axis];
configI[axis] = pidProfile->I8[axis];
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;
}
yawPreviousRate = setpointRate[axis];
}
// Yaw control is GYRO based, direct sticks control is applied to rate PID
if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) {
@ -214,9 +229,8 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
// ---------- 2-DOF PID controller with optional filter on derivative term. b = 1 and only c can be tuned (amount derivative on measurement or error). ----------
// Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
// ----- calculate error / angle rates ----------
errorRate = setpointRate[axis] - PVRate; // r - y
rP = b * setpointRate[axis] - PVRate; // br - y
rD = c * setpointRate[axis] - PVRate; // cr - y
errorRate = setpointRate[axis] - PVRate; // r - y
rP = b[axis] * setpointRate[axis] - PVRate; // br - y
// Slowly restore original setpoint with more stick input
float diffRate = errorRate - rP;
@ -251,7 +265,7 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
}
// -----calculate P component
PTerm = Kp * rP * dynReduction;
PTerm = Kp[axis] * rP * dynReduction;
// -----calculate I component.
// Reduce strong Iterm accumulation during higher stick inputs
@ -260,9 +274,9 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
// Handle All windup Scenarios
// limit maximum integrator value to prevent WindUp
float itermScaler = setpointRateScaler * kiThrottleGain * velocityWindupFactor[axis];
float itermScaler = setpointRateScaler * kiThrottleGain;
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + Ki * errorRate * getdT() * itermScaler, -250.0f, 250.0f);
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + Ki[axis] * errorRate * getdT() * itermScaler, -250.0f, 250.0f);
// I coefficient (I8) moved before integration to make limiting independent from PID settings
ITerm = errorGyroIf[axis];
@ -275,13 +289,14 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
DTerm = 0.0f; // needed for blackbox
} else {
rD = c[axis] * setpointRate[axis] - PVRate; // cr - y
delta = rD - lastRateError[axis];
lastRateError[axis] = rD;
// Divide delta by targetLooptime to get differential (ie dr/dt)
delta *= (1.0f / getdT());
if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = Kd * delta * dynReduction;
if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = Kd[axis] * delta * dynReduction;
// Filter delta
if (dtermNotchInitialised) delta = biquadFilterApply(&dtermFilterNotch[axis], delta);
@ -294,30 +309,15 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
}
}
DTerm = constrainf(Kd * delta * dynReduction, -300.0f, 300.0f);
DTerm = Kd[axis] * delta * dynReduction;
// -----calculate total PID output
axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000);
axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -900, 900);
}
// Disable PID control at zero throttle
if (!pidStabilisationEnabled) axisPID[axis] = 0;
// Velocity limit only active below 1000
if (velocityMax < 1000) {
int16_t currentVelocity = axisPID[axis] - axisPIDState[axis];
if (debugMode == DEBUG_VELOCITY) debug[axis] = currentVelocity;
if (ABS(currentVelocity) > velocityMax) {
axisPID[axis] = (currentVelocity > 0) ? axisPIDState[axis] + velocityMax : axisPIDState[axis] - velocityMax;
velocityWindupFactor[axis] = ABS(currentVelocity) / velocityMax;
}
else {
velocityWindupFactor[axis] = 1.0f;
}
axisPIDState[axis] = axisPID[axis];
}
#ifdef GTUNE
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
calculate_Gtune(axis);