Use AP_Var to store PID scaling values.

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

libraries/PID/PID.cpp

@@ -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)
+    _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
 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;
-	}
 }
-

libraries/PID/PID.h

@@ -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.
+	/// @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
+    ///                         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
+    PID(const prog_char *name,
-	///						array is used as kP, kI, kD and imax
+        const float &initial_p = 0.0,
-	///						respectively.
+        const float &initial_i = 0.0,
-	///
+        const float &initial_d = 0.0,
-	
+        const int16_t &initial_imax = 0.0) :
-	PID(float *gain_array) :
+
-		_storage(STORE_OFF),
+        _group(AP_Var::k_key_none, name),
-		_gain_array(gain_array)
+        // 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() {	
+	void	reset_I();
-		_integrator = 0; 
-		_last_error = 0; 
-		_last_derivative = 0;
-	}
 
 	/// 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; }
+	/// 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) {
-	void	address(const uint16_t v)	{ _address = v; }
+		_kp = p; _ki = i; _kd = d; _imax = imax;
-	
+	}
-	// 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)
+	float	kP() const				{ return _kp.get(); }
-	//{ kP(p); kI(i); kD(d); imax(max); }
+	float	kI() const 				{ return _ki.get(); }
-	//@}
+	float	kD() const 				{ return _kd.get(); }
-	
+	int16_t	imax() const			{ return _imax.get(); }
-	float	get_integrator() 		{ return _integrator; }
+
+	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
+	AP_Var_group	    _group;
-	float		*_gain_array; 		///< pointer to the gains for this pid
+	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		_integrator;		///< integrator value
+	float				_last_derivative; 	///< last derivative for low-pass filter
-	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

libraries/PID/examples/pid/pid.pde

@@ -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);
-}
+}