Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

With apologies to Jason for breaking his branch, clean this up and document it.

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@930 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
DrZiplok 2010-11-26 01:30:21 +00:00
parent 3ca85f94d4
commit 264cca6d24
4 changed files with 146 additions and 222 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

209
libraries/PID/PID.cpp
View File

@ -1,45 +1,41 @@
// -*- 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.
#include "PID.h"
PID::PID(float * _gain_array) :
gain_array(_gain_array)
{
}
long
PID::get_pid(long err, long dt, float scaler)
PID::get_pid(long error, long dt, float scaler)
{
// Positive error produces positive output
float output;
float error = (float)err;
float delta_time = (float)dt/1000.0;
float output = 0;
float delta_time = (float)dt / 1000;
// Compute proportional component
if(_kp != 0){
output += (_kp * error);
}
output += error * _kp;
if(_kd != 0 && dt > 0){
// Compute derivative component
//derivative = (error - previous_error)/delta_time
// Compute derivative component if time has elapsed
if (_kd && (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
derivative = _last_derivative + delta_time/
(RC + delta_time)*(derivative - _last_derivative);
//Serial.print("d: ");
//Serial.println(derivative,DEC);
derivative = _last_derivative +
((delta_time / (RC + delta_time)) * (derivative - _last_derivative));
// update state
_last_error = error;
_last_derivative = derivative;
output += _kd * derivative; // Sum the derivative component
// add in derivative component
output += _kd * derivative;
}
// scale the P and D components
output *= scaler;
// Compute integral component
if(_ki != 0 && dt > 0){
// Compute integral component if time has elapsed
if (_ki && (dt > 0)) {
_integrator += (error * _ki) * scaler * delta_time;
_integrator = constrain(_integrator, -_imax, _imax);
output += _integrator;
@ -48,7 +44,6 @@ PID::get_pid(long err, long dt, float scaler)
return output;
}
void
PID::reset_I(void)
{
@ -57,155 +52,35 @@ PID::reset_I(void)
_last_error_derivative = 0;
}
/// setters
void
PID::set_P(float p)
{
_kp = p;
}
void
PID::set_I(float i)
{
_ki = i;
}
void
PID::set_D(float d)
{
_kd = d;
}
void
PID::set_imax(int imax)
{
_imax = imax;
Serial.print("set imax ");
Serial.println(_imax, DEC);
}
/// getters
float
PID::get_P(void)
{
return _kp;
}
float
PID::get_I(void)
{
return _ki;
}
float
PID::get_D(void)
{
return _kd;
}
int
PID::get_imax(void)
{
return _imax;
}
void
PID::load_gains(int address)
{
//Serial.println("load gains ");
//Serial.println(address, DEC);
_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 = eeprom_read_word((uint16_t *) (address + 6)) * 100;
}
void
PID::save_gains(int address)
{
eeprom_write_word((uint16_t *) address, (int)(_kp * 1000));
eeprom_write_word((uint16_t *) (address + 2), (int)(_ki * 1000));
eeprom_write_word((uint16_t *) (address + 4), (int)(_kd * 1000));
eeprom_write_word((uint16_t *) (address + 6), (int)_imax/100);
}
void
PID::load_gains()
{
_kp = gain_array[0]/ 1000.0;
_ki = gain_array[1]/ 1000.0;
_kd = gain_array[2]/ 1000.0;
_imax = gain_array[3]/ 1000.0;
if (_address) {
_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 = eeprom_read_word((uint16_t *) (_address + 6)) * 100;
} else {
_kp = _gain_array[0]/ 1000.0;
_ki = _gain_array[1]/ 1000.0;
_kd = _gain_array[2]/ 1000.0;
_imax = _gain_array[3]/ 1000.0;
}
}
void
PID::save_gains()
{
gain_array[0] = _kp * 1000;
gain_array[1] = _ki * 1000;
gain_array[2] = _kd * 1000;
gain_array[3] = _imax * 1000;
if (_address) {
eeprom_write_word((uint16_t *) _address, (int)(_kp * 1000));
eeprom_write_word((uint16_t *) (_address + 2), (int)(_ki * 1000));
eeprom_write_word((uint16_t *) (_address + 4), (int)(_kd * 1000));
eeprom_write_word((uint16_t *) (_address + 6), (int)_imax/100);
} else {
_gain_array[0] = _kp * 1000;
_gain_array[1] = _ki * 1000;
_gain_array[2] = _kd * 1000;
_gain_array[3] = _imax * 1000;
}
}
/*
float
read_EE_compressed_float(int address, byte places)
{
float scale = places * 10;
int temp = eeprom_read_word((uint16_t *) address);
return ((float)temp / scale);
}
void write_EE_compressed_float(float value, int address, byte places)
{
float scale = places * 10;
int temp = value * scale;
eeprom_write_word((uint16_t *) address, temp);
}
*/
void
PID::test(void)
{
/*
int address = 0x46;
Serial.print("imax= ");
Serial.print(eeprom_read_word((uint16_t *) address));
Serial.print(" ");
address += 8;
Serial.print(eeprom_read_word((uint16_t *) address));
Serial.print(" ");
address += 8;
Serial.print(eeprom_read_word((uint16_t *) address));
Serial.print(" ");
address += 8;
Serial.print(eeprom_read_word((uint16_t *) address));
Serial.print(" ");
address += 8;
Serial.print(eeprom_read_word((uint16_t *) address));
Serial.print(" ");
address += 8;
Serial.print(eeprom_read_word((uint16_t *) address));
Serial.print(" ");
address += 8;
Serial.print(eeprom_read_word((uint16_t *) address));
Serial.print(" ");
address += 8;
Serial.print(eeprom_read_word((uint16_t *) address));
Serial.print(" ");
address += 8;
Serial.print(eeprom_read_word((uint16_t *) address));
Serial.print(" ");
address += 8;
Serial.print(eeprom_read_word((uint16_t *) address));
Serial.print(" ");
address += 8;
Serial.print(eeprom_read_word((uint16_t *) address));
Serial.print(" ");
address += 8;
Serial.println(eeprom_read_word((uint16_t *) address));
*/
}

118
libraries/PID/PID.h
View File

@ -1,52 +1,100 @@
// -*- 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 <avr/eeprom.h>
#include "WProgram.h"
//#define PID_SIZE 8
/// @class PID
/// @brief Object managing one PID control
class PID {
public:
// note, if using the eeprom load_gains/save_gains functions
// it is not necessary to pass a gain_array pointer, instead
// you need to pass address to load_gains/ save_gains functions
PID(float * gain_array = 0);
/// Constructor
///
/// @param address EEPROM base address at which PID parameters
/// are stored. Zero if the PID does not support
/// save/restore.
///
PID(uint16_t address = 0) :
_gain_array(0),
_address(address),
_integrator(0),
_last_error(0),
_last_error_derivative(0)
{}
/// Constructor
///
/// @param gain_array Address of an array of floats from which
/// gains will be loaded and to which they
/// are saved.
///
PID(float *gain_array) :
_gain_array(gain_array),
_address(0),
_integrator(0),
_last_error(0),
_last_error_derivative(0)
{}
/// Iterate the PID, return the new control value
///
/// Positive error produces positive output.
///
/// @param err The measured error value
/// @param dt The time delta in milliseconds
/// @param scaler An arbitrary scale factor
///
/// @returns The updated control output.
///
long get_pid(long err, long dt, float scaler);
void reset_I();
void load_gains(int address);
void save_gains(int address);
/// Reset the PID integrator
///
void reset_I() { _integrator = 0; _last_error = 0; }
/// Load gain properties
///
void load_gains();
/// Save gain properties
///
void save_gains();
void set_P(float p);
void set_I(float i);
void set_D(float d);
void set_imax(int imax);
void test(void);
float get_P(void);
float get_I(void);
float get_D(void);
int get_imax(void);
float _imax; // integrator max
float _integrator; // integrator value
float _last_error; // last error for derivative
float _kp;
float _ki;
float _kd;
/// @name parameter accessors
//@{
float kP() { return _kp; }
float &kP() { return _kp; }
float kI() { return _ki; }
float &kI() { return _kd; }
float kD() { return _kd; }
float &kD() { return _kd; }
float imax() { return _imax; }
float &imax() { return _imax; }
//@}
private:
// low pass filter cut frequency
// for derivative calculation,
// set to 20 Hz becasue anything over that
// is probably noise, see
// http://en.wikipedia.org/wiki/Low-pass_filter
static uint8_t RC = 20;
uint16_t _address ///< EEPROM address for save/restore of P/I/D
float *_gain_array; ///< pointer to the gains for this pid
// pointer to the gains for this pid,
// if not using eeprom save/load
float * gain_array;
float _kp; ///< proportional gain
float _ki; ///< integral gain
float _kd; ///< derivative gain
float _imax; ///< integrator magnitude clamp
float _integrator; ///< integrator value
int32_t _last_error; ///< last error for derivative
/// low pass filter cut frequency
/// for derivative calculation,
/// set to 20 Hz becasue anything over that
/// is probably noise, see
/// http://en.wikipedia.org/wiki/Low-pass_filter
static uint8_t _RC = 20;
};
#endif

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

@ -20,10 +20,10 @@ void setup()
//rc.trim();
radio_trim = APM_RC.InputCh(0);
pid.set_P(1);
pid.set_I(0);
pid.set_D(.5);
pid.set_imax(50);
pid.kP() = 1;
pid.kI() = 0;
pid.kD() = 0.5;
pid.imax() = 50;
}
void loop()

17
libraries/PID/keywords.txt
View File

@ -1,8 +1,9 @@
APM_RC KEYWORD1
begin KEYWORD2
InputCh KEYWORD2
OutputCh KEYWORD2
GetState KEYWORD2
Force_Out0_Out1 KEYWORD2
Force_Out2_Out3 KEYWORD2
Force_Out6_Out7 KEYWORD2
PID KEYWORD1
get_pid KEYWORD2
reset_I KEYWORD2
kP KEYWORD2
kD KEYWORD2
kI KEYWORD2
imax KEYWORD2
load_gains KEYWORD2
save_gains KEYWORD2