File content as of revision 3:8172f254dff4:

/** PID.cpp
 * UWB Quadcopter project
 *
 * Implementation of the Proportional Integral Derivative
 * control system for the quadcopter.
 * http://nicisdigital.wordpress.com/2011/06/27/proportional-integral-derivative-pid-controller/
 *
 * @author UWB Quadcopter group
 */

#include "PID.h"

PID::PID(const float proportionalGain,
         const float integralGain,
         const float derivativeGain,
         const float windupGainGuard) :
    proportionalGain(proportionalGain),
    integralGain(integralGain),
    derivativeGain(derivativeGain),
    windupGainGuard(windupGainGuard)
{
    integralError = 0.0f;
    previousError = 0.0f;
}

float PID::correct(const float currentPosition,
                   const float targetPosition,
                   const float dt)
{
    float currentError = 0;
    float proportionalControl, integralControl, derivativeControl;

    // How far away we are from the target
    currentError += targetPosition - currentPosition;

    // Sum of the errors the controller has incurred over time
    integralError = currentError * dt;
    // Cap the sum of errors to prevent excessive correction
    if (integralError < -windupGainGuard) {
        integralError = -windupGainGuard;
    } else if (integralError > windupGainGuard) {
        integralError = windupGainGuard;
    }

    // Calculate the correction, based on the tuning values
    proportionalControl = proportionalGain * currentError;
    integralControl = integralError * integralGain;
    derivativeControl = derivativeGain * ((currentError - previousError) / dt);

    // Save current error for next call to correction
    previousError = currentError;

    return proportionalControl + integralControl + derivativeControl;

}