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

Optimized the PID loop a little bit (#5661)

Browse source 
* * Put PID variables into the structure
* Precalculate DTerm gyro filter outside the axis loop
* Removed unused variables PIDweight[3], airmodeWasActivated
* If zero throttle or gyro overflow, we can just set values and exit, this saves checks and jumps in axis loop
* Compute PIDSUM after the axis loop, this saves branching inside the loop because of Yaw has no D term

* * Incorporated review changes from DieHertz and fujin

* * Incorporated another review requests from DieHertz
  - PidSum renamed to Sum
  - pidData[3] redone to pidData[XYZ_AXIS_COUNT]
This commit is contained in:
Miroslav Drbal [ApoC] 2018-04-10 22:22:51 +02:00 • committed by Michael Keller
parent 696478d04c
commit 045557561d
7 changed files with 100 additions and 80 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -53,12 +53,12 @@
FAST_RAM uint32_t targetPidLooptime;
FAST_RAM pidAxisData_t pidData[XYZ_AXIS_COUNT];
static FAST_RAM bool pidStabilisationEnabled;
static FAST_RAM bool inCrashRecoveryMode = false;
FAST_RAM float axisPID_P[3], axisPID_I[3], axisPID_D[3], axisPIDSum[3];
static FAST_RAM float dT;
PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 2);
@ -154,7 +154,7 @@ static void pidSetTargetLooptime(uint32_t pidLooptime)
void pidResetITerm(void)
{
for (int axis = 0; axis < 3; axis++) {
axisPID_I[axis] = 0.0f;
pidData[axis].I = 0.0f;
}
}
@ -274,7 +274,13 @@ void pidInitFilters(const pidProfile_t *pidProfile)
pt1FilterInit(&throttleLpf, pt1FilterGain(pidProfile->throttle_boost_cutoff, dT));
}
static FAST_RAM float Kp[3], Ki[3], Kd[3];
typedef struct pidCoefficient_s {
float Kp;
float Ki;
float Kd;
} pidCoefficient_t;
static FAST_RAM pidCoefficient_t pidCoefficient[3];
static FAST_RAM float maxVelocity[3];
static FAST_RAM float relaxFactor;
static FAST_RAM float dtermSetpointWeight;
@ -297,9 +303,9 @@ static FAST_RAM bool itermRotation;
void pidInitConfig(const pidProfile_t *pidProfile)
{
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
Kp[axis] = PTERM_SCALE * pidProfile->pid[axis].P;
Ki[axis] = ITERM_SCALE * pidProfile->pid[axis].I;
Kd[axis] = DTERM_SCALE * pidProfile->pid[axis].D;
pidCoefficient[axis].Kp = PTERM_SCALE * pidProfile->pid[axis].P;
pidCoefficient[axis].Ki = ITERM_SCALE * pidProfile->pid[axis].I;
pidCoefficient[axis].Kd = DTERM_SCALE * pidProfile->pid[axis].D;
}
dtermSetpointWeight = pidProfile->dtermSetpointWeight / 127.0f;
@ -440,10 +446,9 @@ static float accelerationLimit(int axis, float currentPidSetpoint)
static timeUs_t crashDetectedAtUs;
void handleCrashRecovery(
static void handleCrashRecovery(
const pidCrashRecovery_e crash_recovery, const rollAndPitchTrims_t *angleTrim,
const int axis, const timeUs_t currentTimeUs, const float gyroRate,
float *axisPID_I, float *currentPidSetpoint, float *errorRate)
const int axis, const timeUs_t currentTimeUs, const float gyroRate, float *currentPidSetpoint, float *errorRate)
{
if (inCrashRecoveryMode && cmpTimeUs(currentTimeUs, crashDetectedAtUs) > crashTimeDelayUs) {
if (crash_recovery == PID_CRASH_RECOVERY_BEEP) {
@ -462,7 +467,7 @@ void handleCrashRecovery(
}
// reset ITerm, since accumulated error before crash is now meaningless
// and ITerm windup during crash recovery can be extreme, especially on yaw axis
axisPID_I[axis] = 0.0f;
pidData[axis].I = 0.0f;
if (cmpTimeUs(currentTimeUs, crashDetectedAtUs) > crashTimeLimitUs
|| (getMotorMixRange() < 1.0f
&& ABS(gyro.gyroADCf[FD_ROLL]) < crashRecoveryRate
@ -483,7 +488,7 @@ void handleCrashRecovery(
}
}
void detectAndSetCrashRecovery(
static void detectAndSetCrashRecovery(
const pidCrashRecovery_e crash_recovery, const uint8_t axis,
const timeUs_t currentTimeUs, const float delta, const float errorRate)
{
@ -510,7 +515,7 @@ void detectAndSetCrashRecovery(
}
}
void handleItermRotation(const float deltaT, float *axisPID_I)
static void handleItermRotation(const float deltaT)
{
// rotate old I to the new coordinate system
const float gyroToAngle = deltaT * RAD;
@ -518,9 +523,9 @@ void handleItermRotation(const float deltaT, float *axisPID_I)
int i_1 = (i + 1) % 3;
int i_2 = (i + 2) % 3;
float angle = gyro.gyroADCf[i] * gyroToAngle;
float newPID_I_i_1 = axisPID_I[i_1] + axisPID_I[i_2] * angle;
axisPID_I[i_2] -= axisPID_I[i_1] * angle;
axisPID_I[i_1] = newPID_I_i_1;
float newPID_I_i_1 = pidData[i_1].I + pidData[i_2].I * angle;
pidData[i_2].I -= pidData[i_1].I * angle;
pidData[i_1].I = newPID_I_i_1;
}
}
@ -528,11 +533,25 @@ void handleItermRotation(const float deltaT, float *axisPID_I)
// Based on 2DOF reference design (matlab)
void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, timeUs_t currentTimeUs)
{
static float previousGyroRateFiltered[2];
static float previousGyroRateDterm[2];
static float previousPidSetpoint[2];
static timeUs_t previousTimeUs;
// Disable PID control if at zero throttle or if gyro overflow detected
if (!pidStabilisationEnabled || gyroOverflowDetected()) {
for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
pidData[axis].P = 0;
pidData[axis].I = 0;
pidData[axis].D = 0;
pidData[axis].Sum = 0;
}
return;
}
const float tpaFactor = getThrottlePIDAttenuation();
const float motorMixRange = getMotorMixRange();
static timeUs_t previousTimeUs;
// calculate actual deltaT in seconds
const float deltaT = (currentTimeUs - previousTimeUs) * 0.000001f;
@ -546,12 +565,20 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
// Dynamic d component, enable 2-DOF PID controller only for rate mode
const float dynCd = flightModeFlags ? 0.0f : dtermSetpointWeight;
// Precalculate gyro deta for D-term here, this allows loop unrolling
float gyroRateDterm[2];
for (int axis = FD_ROLL; axis < FD_YAW; ++axis) {
gyroRateDterm[axis] = dtermNotchApplyFn((filter_t *) &dtermNotch[axis], gyro.gyroADCf[axis]);
gyroRateDterm[axis] = dtermLowpassApplyFn((filter_t *) &dtermLowpass[axis], gyroRateDterm[axis]);
gyroRateDterm[axis] = dtermLowpass2ApplyFn((filter_t *) &dtermLowpass2[axis], gyroRateDterm[axis]);
}
if (itermRotation) {
handleItermRotation(deltaT, axisPID_I);
handleItermRotation(deltaT);
}
// ----------PID controller----------
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
float currentPidSetpoint = getSetpointRate(axis);
if (maxVelocity[axis]) {
currentPidSetpoint = accelerationLimit(axis, currentPidSetpoint);
@ -567,63 +594,52 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
handleCrashRecovery(
pidProfile->crash_recovery, angleTrim, axis, currentTimeUs, gyroRate,
axisPID_I, &currentPidSetpoint, &errorRate);
&currentPidSetpoint, &errorRate);
// --------low-level gyro-based PID based on 2DOF PID controller. ----------
// 2-DOF PID controller with optional filter on derivative term.
// b = 1 and only c (dtermSetpointWeight) can be tuned (amount derivative on measurement or error).
// -----calculate P component and add Dynamic Part based on stick input
axisPID_P[axis] = Kp[axis] * errorRate * tpaFactor;
pidData[axis].P = pidCoefficient[axis].Kp * errorRate * tpaFactor;
if (axis == FD_YAW) {
axisPID_P[axis] = ptermYawLowpassApplyFn((filter_t *) &ptermYawLowpass, axisPID_P[axis]);
pidData[axis].P = ptermYawLowpassApplyFn((filter_t *) &ptermYawLowpass, pidData[axis].P);
}
// -----calculate I component
const float ITerm = axisPID_I[axis];
const float ITermNew = constrainf(ITerm + Ki[axis] * errorRate * dynCi, -itermLimit, itermLimit);
const float ITerm = pidData[axis].I;
const float ITermNew = constrainf(ITerm + pidCoefficient[axis].Ki * errorRate * dynCi, -itermLimit, itermLimit);
const bool outputSaturated = mixerIsOutputSaturated(axis, errorRate);
if (outputSaturated == false || ABS(ITermNew) < ABS(ITerm)) {
// Only increase ITerm if output is not saturated
axisPID_I[axis] = ITermNew;
pidData[axis].I = ITermNew;
}
// -----calculate D component
if (axis != FD_YAW) {
// apply filters
float gyroRateFiltered = dtermNotchApplyFn((filter_t *) &dtermNotch[axis], gyroRate);
gyroRateFiltered = dtermLowpassApplyFn((filter_t *) &dtermLowpass[axis], gyroRateFiltered);
gyroRateFiltered = dtermLowpass2ApplyFn((filter_t *) &dtermLowpass2[axis], gyroRateFiltered);
// no transition if relaxFactor == 0
float transition = 1;
if (relaxFactor > 0) {
transition = getRcDeflectionAbs(axis) * relaxFactor;
}
float transition = relaxFactor > 0 ? getRcDeflectionAbs(axis) * relaxFactor : 1;
// Divide rate change by deltaT to get differential (ie dr/dt)
const float delta = (
dynCd * transition * (currentPidSetpoint - previousPidSetpoint[axis]) -
(gyroRateFiltered - previousGyroRateFiltered[axis])) / deltaT;
(gyroRateDterm[axis] - previousGyroRateDterm[axis])) / deltaT;
previousPidSetpoint[axis] = currentPidSetpoint;
previousGyroRateFiltered[axis] = gyroRateFiltered;
previousGyroRateDterm[axis] = gyroRateDterm[axis];
detectAndSetCrashRecovery(pidProfile->crash_recovery, axis, currentTimeUs, delta, errorRate);
axisPID_D[axis] = Kd[axis] * delta * tpaFactor;
axisPIDSum[axis] = axisPID_P[axis] + axisPID_I[axis] + axisPID_D[axis];
} else {
axisPIDSum[axis] = axisPID_P[axis] + axisPID_I[axis];
}
// Disable PID control if at zero throttle or if gyro overflow detected
if (!pidStabilisationEnabled || gyroOverflowDetected()) {
axisPID_P[axis] = 0;
axisPID_I[axis] = 0;
axisPID_D[axis] = 0;
axisPIDSum[axis] = 0;
pidData[axis].D = pidCoefficient[axis].Kd * delta * tpaFactor;
}
}
// calculating the PID sum
pidData[FD_ROLL].Sum = pidData[FD_ROLL].P + pidData[FD_ROLL].I + pidData[FD_ROLL].D;
pidData[FD_PITCH].Sum = pidData[FD_PITCH].P + pidData[FD_PITCH].I + pidData[FD_PITCH].D;
// YAW has no D
pidData[FD_YAW].Sum = pidData[FD_YAW].P + pidData[FD_YAW].I;
}
bool crashRecoveryModeActive(void)