-
Notifications
You must be signed in to change notification settings - Fork 0
Detailed Calculation Flow
const uint32_t now = millis();The current Arduino system time is used as the basis for the controller timing.
const uint32_t elapsedMs =
static_cast<uint32_t>(now - mLastComputeMs);Using unsigned subtraction makes the calculation safe across the millis() rollover.
On typical Arduino platforms using a 32-bit millisecond counter, millis() wraps after roughly 49.7 days.
The subtraction remains valid as long as the elapsed interval is within the normal unsigned arithmetic range.
if (mInitialized &&
elapsedMs < mSampleTimeMs)
{
return mOut;
}If not enough time has passed since the previous calculation, the previous output is returned.
No P, I, or D calculation is performed during this call.
if (!mInitialized)
{
mPrevActual = actual;
mDerivativeFiltered = 0.0;
mLastComputeMs = now;
mInitialized = true;
return mOut;
}The derivative term requires a previous process measurement.
During the first call, no valid previous value exists.
The controller therefore stores the current measurement and waits until the next calculation before evaluating the derivative.
This prevents a large artificial derivative spike during startup.
const double dt =
static_cast<double>(elapsedMs) / 1000.0;Example:
elapsedMs = 100
dt = 0.1 s
Using seconds is important because the integral and derivative gains are then defined consistently with time.
const double direction =
(mDirection == Direction::DIRECT)
? 1.0
: -1.0;The raw process error is calculated as:
const double rawError =
setpoint - actual;The direction-adjusted control error is:
const double error =
direction * rawError;This allows the same internal PID equations to support both direct and reverse control action.
const double p =
mKp * error;The proportional term reacts directly to the current error:
Example:
Kp = 3
Error = 20
P = 60
As the actual value approaches the setpoint, the proportional term naturally decreases.
const double derivativeActual =
(actual - mPrevActual) / dt;This calculates the rate of change of the measured process value.
The derivative contribution is inverted according to controller direction:
const double rawDerivative =
-direction * derivativeActual;For a direct-acting controller:
Actual value rises quickly
->
Derivative becomes negative
->
Controller output is reduced
This creates a braking effect before the setpoint is reached.
That behavior is particularly useful for reducing overshoot.
Using derivative-on-measurement also avoids a derivative kick when the setpoint changes abruptly.
If filtering is enabled:
const double alpha =
dt /
(mDerivativeFilterTau + dt);
mDerivativeFiltered +=
alpha *
(rawDerivative - mDerivativeFiltered);This is a first-order low-pass filter.
A larger filter time constant produces a smoother but slower derivative response.
If filtering is disabled:
mDerivativeFiltered = rawDerivative;The final D term is:
const double d =
mKd * mDerivativeFiltered;mPrevActual = actual;The current measurement becomes the previous measurement for the next PID calculation.
const double toleranceAbsolute =
fabs(setpoint) * mTolerance;Example:
Setpoint = 100
Tolerance = 0.02
Absolute tolerance = 2
const bool outsideTolerance =
fabs(rawError) > toleranceAbsolute;If the error is inside the tolerance band, the integral term is frozen.
P and D remain active.
bool insideIntegralRange = true;
if (mIntegralActivationRange > 0.0)
{
insideIntegralRange =
fabs(rawError)
<= mIntegralActivationRange;
}If the integral activation range is configured, integration is only permitted near the setpoint.
Example:
Setpoint = 100
Integral activation range = 10
Then:
Error 20 -> integral disabled
Error 15 -> integral disabled
Error 10 -> integral enabled
Error 5 -> integral enabled
const bool integrationAllowed =
(mKi > 0.0) &&
outsideTolerance &&
insideIntegralRange;Integration is allowed only if all conditions are true:
-
Kiis greater than zero - the error is outside the tolerance band
- the error is inside the integral activation range
This results in the following behavior:
Far from setpoint Near setpoint Inside tolerance
| | |
v v v
I disabled I enabled I frozen
P active P active P active
D active D active D active
double integralCandidate =
mIntegral;Instead of modifying the real integral state immediately, a candidate value is calculated first:
if (integrationAllowed)
{
integralCandidate +=
error * dt;
}The discrete integral is therefore:
Example:
Previous integral state = 20
Error = 5
dt = 0.1
Candidate = 20 + 5 * 0.1
= 20.5
const double iCandidate =
mKi * integralCandidate;
const double candidateOutput =
p +
iCandidate +
d;This allows the controller to determine whether accepting the new integral value would drive the output further into saturation.
bool integralDrivesIntoSaturation = false;Upper saturation:
if (candidateOutput > mMax &&
error > 0.0)
{
integralDrivesIntoSaturation = true;
}Lower saturation:
if (candidateOutput < mMin &&
error < 0.0)
{
integralDrivesIntoSaturation = true;
}If the integral would make an already saturated output even more saturated, the new integral value is rejected.
This prevents integral windup.
if (integrationAllowed &&
!integralDrivesIntoSaturation)
{
mIntegral =
integralCandidate;
}The candidate is accepted only when integration is permitted and it does not worsen saturation.
The final integral contribution is:
const double i =
mKi * mIntegral;double output =
p +
i +
d;This is the basic controller equation:
output =
clamp(
output,
mMin,
mMax);Regardless of the internal PID result, the output cannot exceed the configured controller limits.
If enabled:
const double maxChange =
mOutputSlewRate * dt;The desired output change is:
const double change =
output - mOut;If the requested change is too large:
if (change > maxChange)
{
output =
mOut + maxChange;
}
else if (change < -maxChange)
{
output =
mOut - maxChange;
}This limits both rising and falling output changes.
mOut =
clamp(
output,
mMin,
mMax);
return mOut;The final output is stored for the next cycle and returned to the caller.