ardupilot/libraries/PID/PID.h

140 lines
3.4 KiB
C
Raw Normal View History

// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/// @file PID.h
/// @brief Generic PID algorithm, with EEPROM-backed storage of constants.
#ifndef PID_h
#define PID_h
#include <stdint.h>
#include <AP_EEPROMB.h>
/// @class PID
/// @brief Object managing one PID control
class PID {
public:
///
// EEProm Storage Type
enum storage_t
{
STORE_OFF,
STORE_EEPROM_FLOAT,
STORE_EEPROM_UINT16
} _storage;
/// Constructor
///
/// A PID constructed in this fashion does not support save/restore.
/// Gains are managed internally, and must be read/written using the
/// accessor functions.
///
PID() :
_storage(STORE_OFF),
_address(0),
_gain_array(&_local_gains[0])
{}
/// Constructor
///
/// The PID will manage gains internally, and the load/save functions
/// will use 16 bytes of EEPROM storage to store gain values.
///
/// @param address EEPROM base address at which PID parameters
/// are stored.
///
PID(uint16_t address, storage_t storage = STORE_EEPROM_UINT16) :
_storage(storage),
_address(address),
_gain_array(&_local_gains[0])
{
load_gains();
}
/// Constructor
///
/// Gain values for the PID are managed externally; load/save are a NOP.
///
/// @param gain_array Address of an array of float values. The
/// array is used as kP, kI, kD and imax
/// respectively.
///
PID(float *gain_array) :
_storage(STORE_OFF),
_gain_array(gain_array)
{
}
/// Iterate the PID, return the new control value
///
/// Positive error produces positive output.
///
/// @param error The measured error value
/// @param dt The time delta in milliseconds (note
/// that update interval cannot be more
/// than 65.535 seconds due to limited range
/// of the data type).
/// @param scaler An arbitrary scale factor
///
/// @returns The updated control output.
///
long get_pid(int32_t error, uint16_t dt, float scaler = 1.0);
/// Reset the PID integrator
///
void reset_I() {
_integrator = 0;
_last_error = 0;
_last_derivative = 0;
}
/// Load gain properties
///
void load_gains();
/// Save gain properties
///
void save_gains();
/// @name parameter accessors
//@{
float kP() { return _gain_array[0]; }
float kI() { return _gain_array[1]; }
float kD() { return _gain_array[2]; }
float imax() { return _gain_array[3]; }
void kP(const float v) { _gain_array[0] = v; }
void kI(const float v) { _gain_array[1] = v; }
void kD(const float v) { _gain_array[2] = v; }
void imax(const float v);
void address(const uint16_t v) { _address = v; }
// one-shot operator for setting all of the gain properties at once
//void operator ()(const float p, const float i, const float d, const float max)
//{ kP(p); kI(i); kD(d); imax(max); }
//@}
float get_integrator() { return _integrator; }
private:
AP_EEPROMB _ee;
uint16_t _address; ///< EEPROM address for save/restore of P/I/D
float *_gain_array; ///< pointer to the gains for this pid
float _local_gains[4]; ///< local storage for gains when not globally managed
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 _RC = 20;
};
#endif