Correct misunderstanding about the intent of the external gain array.

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

libraries/PID/PID.cpp

@@ -8,6 +8,12 @@
 
 #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)
 {
@@ -60,32 +66,24 @@ PID::imax(float v)
 void
 PID::load_gains()
 {
-	if (_address) {
+	if ((_gain_array == &_local_gains[0]) && (_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];
-		_ki 	= _gain_array[1];
-		_kd 	= _gain_array[2];
-		_imax 	= _gain_array[3];
 	}
+	// load is a NOP if the gain array is managed externally
 }
 
 void
 PID::save_gains()
 {
-	if (_address) {
+	if ((_gain_array == &_local_gains[0]) && (_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;
-		_gain_array[1] = _ki;
-		_gain_array[2] = _kd;
-		_gain_array[3] = _imax;
 	}
+	// save is a NOP if the gain array is managed externally
 }
 

libraries/PID/PID.h

@@ -15,30 +15,37 @@ public:
 	/// Constructor
 	///
 	/// A PID constructed in this fashion does not support save/restore.
+	/// Gains are managed internally, and must be read/written using the
+	/// accessor functions.
 	///
 	PID() :
-		_address(0)
+		_address(0),
+		_gain_array(&_local_gains[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.  Zero if the PID does not support
-	///					save/restore.
+	///					are stored.
 	///
 	PID(uint16_t address) :
-		_address(address)
+		_address(address),
+		_gain_array(&_local_gains[0])
 	{}
 
 	/// Constructor
 	///
-	/// @param gain_array	Address of an array of floats from which
-	///						gains will be loaded and to which they
-	///						are saved.
+	/// Gain values for the PID are managed externally; load/save are a NOP.
+	///
+	/// @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) :
-		_gain_array(gain_array),
-		_address(0)		// must init address to zero to have gain_array respected
+		_gain_array(gain_array)
 	{}
 
 	/// Iterate the PID, return the new control value
@@ -74,25 +81,26 @@ public:
 
 	/// @name	parameter accessors
 	//@{
-	float	kP()			{ return _kp; }
-	float	kI()			{ return _ki; }
-	float	kD()			{ return _kd; }
-	float	imax()			{ return _imax; }
+	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(float v)		{ _kp = v; }
-	void	kI(float v)		{ _ki = v; }
-	void	kD(float v)		{ _kd = v; }
-	void	imax(float v);
+	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);
+
+	// 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); }
 	//@}
 
 private:
 	uint16_t	_address;			///< EEPROM address for save/restore of P/I/D
 	float		*_gain_array; 		///< pointer to the gains for this pid
 
-	float		_kp;			///< proportional gain
-	float		_ki;			///< integral gain
-	float		_kd;			///< derivative gain
-	float		_imax;			///< integrator magnitude clamp
+	float		_local_gains[4];	///< local storage for gains when not globally managed
 
 	float		_integrator;		///< integrator value
 	int32_t		_last_error;		///< last error for derivative