ardupilot/libraries/AP_PID/AP_PID.h

// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-

/// @file	AP_PID.h
/// @brief	Generic PID algorithm, with EEPROM-backed storage of constants.

#ifndef AP_PID_h
#define AP_PID_h

#include <AP_Common.h>
#include <math.h>               // for fabs()

/// @class	AP_PID
/// @brief	Object managing one PID control
class AP_PID {
public:

    AP_PID();

    long        get_pid(int32_t error, uint16_t dt, float scaler = 1.0);

    /// Reset the PID integrator
    ///
    void        reset_I();

    void        kP(const float v)               {
        _kp = v;
    }
    void        kI(const float v)               {
        _ki = v;
    }
    void        kD(const float v)               {
        _kd = v;
    }
    void        imax(const int16_t v)   {
        _imax = v;
    }

    float           kP()                    {
        return _kp;
    }
    float           kI()                    {
        return _ki;
    }
    float           kD()                    {
        return _kd;
    }
    float           imax()                  {
        return _imax;
    }

    float        get_integrator() const {
        return _integrator;
    }

private:
    float           _kp;
    float           _ki;
    float           _kd;
    float           _imax;

    float           _integrator;                                ///< integrator value
    int32_t         _last_error;                                ///< last error for derivative
    float           _last_derivative;                           ///< last derivative for low-pass filter

    /// Low pass filter cut frequency for derivative calculation.
    ///
    /// 20 Hz becasue anything over that is probably noise, see
    /// http://en.wikipedia.org/wiki/Low-pass_filter.
    ///
    static const uint8_t        _fCut = 20;
};

#endif