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

72
libraries/PID/PID.cpp
View File

@ -1,19 +1,12 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/// @file PID.cpp /// @file PID.cpp
/// @brief Generic PID algorithm, with EEPROM-backed storage of constants. /// @brief Generic PID algorithm
#include <math.h> #include <math.h>
#include <avr/eeprom.h>
#include "PID.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 long
PID::get_pid(int32_t error, uint16_t dt, float scaler) PID::get_pid(int32_t error, uint16_t dt, float scaler)
{ {
@ -24,7 +17,7 @@ PID::get_pid(int32_t error, uint16_t dt, float scaler)
output += error * _kp; output += error * _kp;
// Compute derivative component if time has elapsed // Compute derivative component if time has elapsed
if (_kd && (dt > 0)) { if ((fabs(_kd) > 0) && (dt > 0)) {
float derivative = (error - _last_error) / delta_time; float derivative = (error - _last_error) / delta_time;
// discrete low pass filter, cuts out the // discrete low pass filter, cuts out the
@ -45,7 +38,7 @@ PID::get_pid(int32_t error, uint16_t dt, float scaler)
output *= scaler; output *= scaler;
// Compute integral component if time has elapsed // Compute integral component if time has elapsed
if (_ki && (dt > 0)) { if ((fabs(_ki) > 0) && (dt > 0)) {
_integrator += (error * _ki) * scaler * delta_time; _integrator += (error * _ki) * scaler * delta_time;
if (_integrator < -_imax) { if (_integrator < -_imax) {
_integrator = -_imax; _integrator = -_imax;
@ -58,63 +51,22 @@ PID::get_pid(int32_t error, uint16_t dt, float scaler)
return output; return output;
} }
void
PID::reset_I()
{
_integrator = 0;
_last_error = 0;
_last_derivative = 0;
}
void void
PID::load_gains() PID::load_gains()
{ {
switch(_storage) _group.load();
{
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;
}
} }
void void
PID::save_gains() PID::save_gains()
{ {
switch(_storage) _group.save();
{
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;
} }
}

131
libraries/PID/PID.h
View File

@ -6,61 +6,67 @@
#ifndef PID_h #ifndef PID_h
#define PID_h #define PID_h
#include <stdint.h> #include <AP_Common.h>
#include <math.h> // for fabs()
/// @class PID /// @class PID
/// @brief Object managing one PID control /// @brief Object managing one PID control
class PID { class PID {
public: 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. /// @note PIDs must be named to avoid either multiple parameters with the
/// Gains are managed internally, and must be read/written using the /// same name, or an overly complex constructor.
/// accessor functions.
/// ///
PID() : /// @param key Storage key assigned to this PID. Should be unique.
_storage(STORE_OFF), /// @param name Name by which the PID is known, or NULL for an anonymous PID.
_address(0), /// The name is prefixed to the P, I, D, IMAX variable names when
_gain_array(&_local_gains[0]) /// 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 _group(key, name),
/// // group, index, initial value, name
/// The PID will manage gains internally, and the load/save functions _kp (&_group, 0, initial_p, PSTR("P")),
/// will use 16 bytes of EEPROM storage to store gain values. _ki (&_group, 1, initial_i, PSTR("I")),
/// _kd (&_group, 2, initial_d, PSTR("D")),
/// @param address EEPROM base address at which PID parameters _imax(&_group, 3, initial_imax, PSTR("IMAX"))
/// are stored.
///
PID(uint16_t address, storage_t storage = STORE_EEPROM_UINT16) :
_storage(storage),
_address(address),
_gain_array(&_local_gains[0])
{ {
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
/// @param gain_array Address of an array of float values. The /// they are reported.
/// array is used as kP, kI, kD and imax /// @param initial_p Initial value for the P term.
/// respectively. /// @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(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) :
PID(float *gain_array) : _group(AP_Var::k_key_none, name),
_storage(STORE_OFF), // group, index, initial value, name
_gain_array(gain_array) _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"))
{ {
} }
@ -81,11 +87,7 @@ public:
/// Reset the PID integrator /// Reset the PID integrator
/// ///
void reset_I() { void reset_I();
_integrator = 0;
_last_error = 0;
_last_derivative = 0;
}
/// Load gain properties /// Load gain properties
/// ///
@ -97,30 +99,33 @@ public:
/// @name parameter accessors /// @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; } /// Overload the function call operator to permit relatively easy initialisation
void kI(const float v) { _gain_array[1] = v; } void operator() (const float p,
void kD(const float v) { _gain_array[2] = v; } const float i,
void imax(const float v) { _gain_array[3] = fabs(v); } const float d,
const int16_t imax) {
_kp = p; _ki = i; _kd = d; _imax = imax;
}
void address(const uint16_t v) { _address = v; } 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(); }
// one-shot operator for setting all of the gain properties at once void kP(const float v) { _kp = v; }
//void operator ()(const float p, const float i, const float d, const float max) void kI(const float v) { _ki = v; }
//{ kP(p); kI(i); kD(d); imax(max); } void kD(const float v) { _kd = v; }
//@} void imax(const int16_t v) { _imax = abs(v); }
float get_integrator() { return _integrator; } float get_integrator() const { return _integrator; }
private: private:
uint16_t _address; ///< EEPROM address for save/restore of P/I/D AP_Var_group _group;
float *_gain_array; ///< pointer to the gains for this pid AP_Float16 _kp;
AP_Float16 _ki;
float _local_gains[4]; ///< local storage for gains when not globally managed AP_Float16 _kd;
AP_Int16 _imax;
float _integrator; ///< integrator value float _integrator; ///< integrator value
int32_t _last_error; ///< last error for derivative int32_t _last_error; ///< last error for derivative

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

@ -3,13 +3,15 @@
2010 Code by Jason Short. DIYDrones.com 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 <APM_RC.h> // ArduPilot RC Library
#include <PID.h> // ArduPilot Mega RC Library
long radio_in; long radio_in;
long radio_trim; long radio_trim;
PID pid(0x16); PID pid(10, "TEST1_");
void setup() void setup()
{ {
@ -25,6 +27,13 @@ void setup()
pid.kI(0); pid.kI(0);
pid.kD(0.5); pid.kD(0.5);
pid.imax(50); 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() void loop()