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

git-svn-id: https://arducopter.googlecode.com/svn/trunk@930 f9c3cf11-9bcb-44bc-f272-b75c42450872

libraries/PID/PID.cpp

@@ -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		= 0;
-	float output;
+ 	float delta_time	= (float)dt / 1000;
- 	float error = (float)err;
- 	float delta_time = (float)dt/1000.0;
- 	
-	// Compute proportional component
-	if(_kp != 0){
-		output 				+= (_kp * error);
-	}
 
-	if(_kd != 0 && dt > 0){
+	// Compute proportional component
-		// Compute derivative component
+	output += error * _kp;
-		//derivative = (error - previous_error)/delta_time
+
-		float derivative 	= (error - _last_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/
+		derivative = _last_derivative + 
-			(RC + delta_time)*(derivative - _last_derivative);
+			((delta_time / (RC + delta_time)) * (derivative - _last_derivative));
-		
-		//Serial.print("d: ");
-		//Serial.println(derivative,DEC);
 
+		// 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
+	// Compute integral component if time has elapsed
-	if(_ki != 0 && dt > 0){
+	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;
+	if (_address) {
-	_ki 	= gain_array[1]/ 1000.0;
+		_kp 	= (float)(eeprom_read_word((uint16_t *)	 _address))      / 1000.0;
-	_kd 	= gain_array[2]/ 1000.0;
+		_ki 	= (float)(eeprom_read_word((uint16_t *)	(_address + 2))) / 1000.0;
-	_imax 	= gain_array[3]/ 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;
+	if (_address) {
-	gain_array[1] = _ki * 1000;
+		eeprom_write_word((uint16_t *)	 _address, 		(int)(_kp * 1000));
-	gain_array[2] = _kd * 1000;
+		eeprom_write_word((uint16_t *)	(_address + 2), (int)(_ki * 1000));
-	gain_array[3] = _imax * 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));
-	*/
-}

libraries/PID/PID.h

@@ -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
+	/// Constructor
-	// it is not necessary to pass a gain_array pointer, instead
+	///
-	// you need to pass address to load_gains/ save_gains functions
+	/// @param address	EEPROM base address at which PID parameters
-	PID(float * gain_array = 0);
+	///					are stored.  Zero if the PID does not support
-	long 	get_pid(long err, long dt, float scaler);
+	///					save/restore.
-	void	reset_I();
+	///
-	void 	load_gains(int address);
+	PID(uint16_t address = 0) :
-	void 	save_gains(int address);
+		_gain_array(0),
-	void 	load_gains();
+		_address(address),
-	void 	save_gains();
+		_integrator(0),
-	void 	set_P(float p);
+		_last_error(0),
-	void 	set_I(float i);
+		_last_error_derivative(0)
-	void 	set_D(float d);
+	{}
-	void 	set_imax(int imax);
-	void 	test(void);
 
-	float 	get_P(void);
+	/// Constructor
-	float 	get_I(void);
+	///
-	float 	get_D(void);
+	/// @param gain_array	Address of an array of floats from which
-	int 	get_imax(void);
+	///						gains will be loaded and to which they
-	float _imax;		// integrator max
+	///						are saved.
-	float _integrator;	// integrator value
+	///
-	float _last_error;	// last error for derivative
+	PID(float *gain_array) :
-	float _kp;
+		_gain_array(gain_array),
-	float _ki;
+		_address(0),
-	float _kd;
+		_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);
+
+	/// Reset the PID integrator
+	///
+	void	reset_I() { _integrator = 0; _last_error = 0; }
+
+	/// Load gain properties
+	///
+	void 	load_gains();
+
+	/// Save gain properties
+	///
+	void 	save_gains();
+
+	/// @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
+	uint16_t	_address		///< EEPROM address for save/restore of P/I/D
-	// for derivative calculation,
+	float		*_gain_array; 	///< pointer to the gains for this pid
-	// set to 20 Hz becasue anything over that
+
-	// is probably noise, see
+	float		_kp;			///< proportional gain
-	// http://en.wikipedia.org/wiki/Low-pass_filter
+	float		_ki;			///< integral gain
-	static uint8_t RC = 20; 
+	float		_kd;			///< derivative gain
-	
+	float		_imax;			///< integrator magnitude clamp
-	// pointer to the gains for this pid,
+
-	// if not using eeprom save/load
+	float		_integrator;	///< integrator value
-	float * gain_array; 
+	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

libraries/PID/examples/pid/pid.pde

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

libraries/PID/keywords.txt

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