Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Use AP_Var to store PID scaling values. 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1652 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
DrZiplok@gmail.com 2011-02-14 04:45:31 +00:00
parent 8e7301b4d3
commit 223af24398
3 changed files with 103 additions and 137 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

74
libraries/PID/PID.cpp
View File

@ -1,19 +1,12 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/// @file PID.cpp
/// @brief Generic PID algorithm, with EEPROM-backed storage of constants.
/// @brief Generic PID algorithm
#include <math.h>
#include <avr/eeprom.h>
#include "PID.h"
// make the gain array members a little easier to identify
#define _kp _gain_array[0]
#define _ki _gain_array[1]
#define _kd _gain_array[2]
#define _imax _gain_array[3]
long
PID::get_pid(int32_t error, uint16_t dt, float scaler)
{
@ -24,14 +17,14 @@ PID::get_pid(int32_t error, uint16_t dt, float scaler)
output += error * _kp;
// Compute derivative component if time has elapsed
if (_kd && (dt > 0)) {
if ((fabs(_kd) > 0) && (dt > 0)) {
float derivative = (error - _last_error) / delta_time;
// discrete low pass filter, cuts out the
// high frequency noise that can drive the controller crazy
float RC = 1/(2*M_PI*_fCut);
derivative = _last_derivative +
(delta_time / (RC + delta_time)) * (derivative - _last_derivative);
(delta_time / (RC + delta_time)) * (derivative - _last_derivative);
// update state
_last_error = error;
@ -45,7 +38,7 @@ PID::get_pid(int32_t error, uint16_t dt, float scaler)
output *= scaler;
// Compute integral component if time has elapsed
if (_ki && (dt > 0)) {
if ((fabs(_ki) > 0) && (dt > 0)) {
_integrator += (error * _ki) * scaler * delta_time;
if (_integrator < -_imax) {
_integrator = -_imax;
@ -58,63 +51,22 @@ PID::get_pid(int32_t error, uint16_t dt, float scaler)
return output;
}
void
PID::reset_I()
{
_integrator = 0;
_last_error = 0;
_last_derivative = 0;
}
void
PID::load_gains()
{
switch(_storage)
{
case STORE_OFF:
// load is a NOP if the gain array is managed externally
break;
case STORE_EEPROM_UINT16:
_kp = (float)(eeprom_read_word((uint16_t *) _address)) / 1000.0;
_ki = (float)(eeprom_read_word((uint16_t *) (_address + 2))) / 1000.0;
_kd = (float)(eeprom_read_word((uint16_t *) (_address + 4))) / 1000.0;
_imax = (float)(eeprom_read_word((uint16_t *) (_address + 6)));
//_kp = (float)_ee.read_int(_address) / 1000.0;
//_ki = (float)_ee.read_int(_address + 2) / 1000.0;
//_kd = (float)_ee.read_int(_address + 4) / 1000.0;
//_imax = (float)_ee.read_int(_address + 6) * 100;
break;
case STORE_EEPROM_FLOAT:
eeprom_read_block((void*)&_kp,(const void*)(_address),sizeof(_kp));
eeprom_read_block((void*)&_ki,(const void*)(_address+4),sizeof(_ki));
eeprom_read_block((void*)&_kd,(const void*)(_address+8),sizeof(_kd));
eeprom_read_block((void*)&_imax,(const void*)(_address+12),sizeof(_imax));
break;
}
_group.load();
}
void
PID::save_gains()
{
switch(_storage)
{
case STORE_OFF:
// save is a NOP if the gain array is managed externally
break;
case STORE_EEPROM_UINT16:
eeprom_write_word((uint16_t *) _address, (int)(_kp * 1000.0));
eeprom_write_word((uint16_t *) (_address + 2), (int)(_ki * 1000.0));
eeprom_write_word((uint16_t *) (_address + 4), (int)(_kd * 1000.0));
eeprom_write_word((uint16_t *) (_address + 6), (int)(_imax));
/*_ee.write_int(_address, (int)(_kp * 1000));
_ee.write_int(_address + 2, (int)(_ki * 1000));
_ee.write_int(_address + 4, (int)(_kd * 1000));
_ee.write_int(_address + 6, (int)(_imax /100));
*/
break;
case STORE_EEPROM_FLOAT:
eeprom_write_block((const void *)&_kp,(void *)(_address),sizeof(_kp));
eeprom_write_block((const void *)&_ki,(void *)(_address+4),sizeof(_ki));
eeprom_write_block((const void *)&_kd,(void *)(_address+8),sizeof(_kd));
eeprom_write_block((const void *)&_imax,(void *)(_address+12),sizeof(_imax));
break;
}
_group.save();
}

151
libraries/PID/PID.h
View File

@ -6,62 +6,68 @@
#ifndef PID_h
#define PID_h
#include <stdint.h>
#include <AP_Common.h>
#include <math.h> // for fabs()
/// @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
/// Constructor for PID that saves its settings to EEPROM
///
/// A PID constructed in this fashion does not support save/restore.
/// Gains are managed internally, and must be read/written using the
/// accessor functions.
/// @note PIDs must be named to avoid either multiple parameters with the
/// same name, or an overly complex constructor.
///
PID() :
_storage(STORE_OFF),
_address(0),
_gain_array(&_local_gains[0])
{}
/// @param key Storage key assigned to this PID. Should be unique.
/// @param name Name by which the PID is known, or NULL for an anonymous PID.
/// The name is prefixed to the P, I, D, IMAX variable names when
/// they are reported.
/// @param initial_p Initial value for the P term.
/// @param initial_i Initial value for the I term.
/// @param initial_d Initial value for the D term.
/// @param initial_imax Initial value for the imax term.4
///
PID(AP_Var::Key key,
const prog_char *name,
const float &initial_p = 0.0,
const float &initial_i = 0.0,
const float &initial_d = 0.0,
const int16_t &initial_imax = 0.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])
_group(key, name),
// group, index, initial value, name
_kp (&_group, 0, initial_p, PSTR("P")),
_ki (&_group, 1, initial_i, PSTR("I")),
_kd (&_group, 2, initial_d, PSTR("D")),
_imax(&_group, 3, initial_imax, PSTR("IMAX"))
{
load_gains();
// no need for explicit load, assuming that the main code uses AP_Var::load_all.
}
/// Constructor
/// Constructor for PID that does not save its settings.
///
/// Gain values for the PID are managed externally; load/save are a NOP.
/// @param name Name by which the PID is known, or NULL for an anonymous PID.
/// The name is prefixed to the P, I, D, IMAX variable names when
/// they are reported.
/// @param initial_p Initial value for the P term.
/// @param initial_i Initial value for the I term.
/// @param initial_d Initial value for the D term.
/// @param initial_imax Initial value for the imax term.4
///
/// @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)
{
PID(const prog_char *name,
const float &initial_p = 0.0,
const float &initial_i = 0.0,
const float &initial_d = 0.0,
const int16_t &initial_imax = 0.0) :
_group(AP_Var::k_key_none, name),
// group, index, initial value, name
_kp (&_group, 0, initial_p, PSTR("P")),
_ki (&_group, 1, initial_i, PSTR("I")),
_kd (&_group, 2, initial_d, PSTR("D")),
_imax(&_group, 3, initial_imax, PSTR("IMAX"))
{
}
/// Iterate the PID, return the new control value
@ -76,16 +82,12 @@ public:
/// @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;
}
void reset_I();
/// Load gain properties
///
@ -97,41 +99,44 @@ public:
/// @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) { _gain_array[3] = fabs(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; }
/// Overload the function call operator to permit relatively easy initialisation
void operator() (const float p,
const float i,
const float d,
const int16_t imax) {
_kp = p; _ki = i; _kd = d; _imax = imax;
}
float kP() const { return _kp.get(); }
float kI() const { return _ki.get(); }
float kD() const { return _kd.get(); }
int16_t imax() const { return _imax.get(); }
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 = abs(v); }
float get_integrator() const { return _integrator; }
private:
uint16_t _address; ///< EEPROM address for save/restore of P/I/D
float *_gain_array; ///< pointer to the gains for this pid
AP_Var_group _group;
AP_Float16 _kp;
AP_Float16 _ki;
AP_Float16 _kd;
AP_Int16 _imax;
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
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;
static const uint8_t _fCut = 20;
};
#endif

15
libraries/PID/examples/pid/pid.pde
View File

@ -3,13 +3,15 @@
2010 Code by Jason Short. DIYDrones.com
*/
#include <PID.h> // ArduPilot Mega RC Library
#include <FastSerial.h>
#include <AP_Common.h>
#include <APM_RC.h> // ArduPilot RC Library
#include <PID.h> // ArduPilot Mega RC Library
long radio_in;
long radio_trim;
PID pid(0x16);
PID pid(10, "TEST1_");
void setup()
{
@ -25,6 +27,13 @@ void setup()
pid.kI(0);
pid.kD(0.5);
pid.imax(50);
pid.save_gains();
pid.kP(0);
pid.kI(0);
pid.kD(0);
pid.imax(0);
pid.load_gains();
Serial.printf("P %f I %f D %f imax %f\n", pid.kP(), pid.kI(), pid.kD(), pid.imax());
}
void loop()
@ -36,4 +45,4 @@ void loop()
Serial.print("control: ");
Serial.println(control,DEC);
}
}