Added D term to APM_PI - need to refactor this as a parent class and two child classes to save code space, remove dupes

libraries/AC_PID/AC_PID.cpp

@@ -0,0 +1,118 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+
+/// @file	AC_PID.cpp
+/// @brief	Generic PID algorithm
+
+#include <math.h>
+#include "AC_PID.h"
+
+
+int32_t AC_PID::get_p(int32_t error)
+{
+	return (float)error * _kp;
+}
+
+int32_t AC_PID::get_i(int32_t error, float dt)
+{
+	if((_ki != 0) && (dt != 0)){
+		_integrator += ((float)error * _ki) * dt;
+		if (_integrator < -_imax) {
+			_integrator = -_imax;
+		} else if (_integrator > _imax) {
+			_integrator = _imax;
+		}
+	}
+	return _integrator;
+}
+
+int32_t AC_PID::get_d(int32_t input, float dt)
+{
+	if ((_kd != 0) && (dt != 0)) {
+		_derivative = (input - _last_input) / dt;
+
+		// discrete low pass filter, cuts out the
+		// high frequency noise that can drive the controller crazy
+		_derivative = _last_derivative +
+		        (dt / ( _filter + dt)) * (_derivative - _last_derivative);
+
+		// update state
+		_last_input 		= input;
+		_last_derivative    = _derivative;
+
+		// add in derivative component
+		return _kd * _derivative;
+	}
+}
+
+int32_t AC_PID::get_pi(int32_t error, float dt)
+{
+	return get_p(error) + get_i(error, dt);
+}
+
+
+int32_t AC_PID::get_pid(int32_t error, float dt)
+{
+	return get_p(error) + get_i(error, dt) + get_d(error, dt);
+}
+
+
+
+/*
+int32_t AC_PID::get_pid(int32_t error, float dt)
+{
+	// Compute proportional component
+	_output = error * _kp;
+
+	// Compute derivative component if time has elapsed
+	if ((fabs(_kd) > 0) && (dt > 0)) {
+
+		_derivative = (error - _last_input) / dt;
+
+		// discrete low pass filter, cuts out the
+		// high frequency noise that can drive the controller crazy
+		_derivative = _last_derivative +
+		        (dt / ( _filter + dt)) * (_derivative - _last_derivative);
+
+		// update state
+		_last_input 		= error;
+		_last_derivative    = _derivative;
+
+		// add in derivative component
+		_output 	+= _kd * _derivative;
+	}
+
+	// Compute integral component if time has elapsed
+	if ((fabs(_ki) > 0) && (dt > 0)) {
+		_integrator 		+= (error * _ki) * dt;
+		if (_integrator < -_imax) {
+			_integrator = -_imax;
+		} else if (_integrator > _imax) {
+			_integrator = _imax;
+		}
+		_output 	+= _integrator;
+	}
+
+	return _output;
+}
+*/
+
+
+void
+AC_PID::reset_I()
+{
+	_integrator = 0;
+	_last_input = 0;
+	_last_derivative = 0;
+}
+
+void
+AC_PID::load_gains()
+{
+    _group.load();
+}
+
+void
+AC_PID::save_gains()
+{
+    _group.save();
+}

libraries/AC_PID/AC_PID.h

@@ -0,0 +1,152 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+
+/// @file	AC_PID.h
+/// @brief	Generic PID algorithm, with EEPROM-backed storage of constants.
+
+#ifndef AC_PID_h
+#define AC_PID_h
+
+#include <AP_Common.h>
+#include <math.h>		// for fabs()
+
+/// @class	AC_PID
+/// @brief	Object managing one PID control
+class AC_PID {
+public:
+
+	/// Constructor for PID that saves its settings to EEPROM
+	///
+	/// @note	PIDs must be named to avoid either multiple parameters with the
+	///			same name, or an overly complex constructor.
+	///
+	/// @param	key 			Storage key assigned to this PID.  Should be unique.
+	/// @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
+	///
+	AC_PID(AP_Var::Key key,
+	    const prog_char_t *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) :
+
+		_group(key, name),
+		// 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"))
+	{
+		// no need for explicit load, assuming that the main code uses AP_Var::load_all.
+	}
+
+	/// Constructor for PID that does not save its settings.
+	///
+    /// @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
+	///
+    AC_PID(const prog_char_t *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) :
+
+        _group(AP_Var::k_key_none, name),
+        // 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
+	///
+	/// Positive error produces positive output.
+	///
+	/// @param error	The measured error value
+	/// @param dt		The time delta in milliseconds (note
+	///					that update interval cannot be more
+	///					than 65.535 seconds due to limited range
+	///					of the data type).
+	/// @param scaler	An arbitrary scale factor
+	///
+	/// @returns		The updated control output.
+	///
+	int32_t get_pid(int32_t error, float dt);
+	int32_t get_pi(int32_t error, float dt);
+	int32_t get_p(int32_t error);
+	int32_t get_i(int32_t error, float dt);
+	int32_t get_d(int32_t error, float dt);
+
+
+	/// Reset the PID integrator
+	///
+	void	reset_I();
+
+	/// Load gain properties
+	///
+	void 	load_gains();
+
+	/// Save gain properties
+	///
+	void 	save_gains();
+
+	/// @name	parameter accessors
+	//@{
+
+	/// Overload the function call operator to permit relatively easy initialisation
+	void operator() (const float p,
+	                 const float i,
+	                 const float d,
+	                 const int16_t imaxval) {
+		_kp = p; _ki = i; _kd = d; _imax = imaxval;
+	}
+
+	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(); }
+
+	void	kP(const float v)		{ _kp.set(v); }
+	void	kI(const float v)		{ _ki.set(v); }
+	void	kD(const float v)		{ _kd.set(v); }
+	void	imax(const int16_t v)	{ _imax.set(abs(v)); }
+
+	float	get_integrator() const	{ return _integrator; }
+
+private:
+	AP_Var_group	    _group;
+	AP_Float16			_kp;
+	AP_Float16			_ki;
+	AP_Float16			_kd;
+	AP_Int16			_imax;
+
+	float				_integrator;		///< integrator value
+	int32_t				_last_input;		///< last input for derivative
+	float				_last_derivative; 	///< last derivative for low-pass filter
+	float				_output;
+	float				_derivative;
+
+	/// Low pass filter cut frequency for derivative calculation.
+	///
+	static const float _filter = 7.9577e-3; // Set to  "1 / ( 2 * PI * f_cut )";
+	// Examples for _filter:
+	// f_cut = 10 Hz -> _filter = 15.9155e-3
+	// f_cut = 15 Hz -> _filter = 10.6103e-3
+	// f_cut = 20 Hz -> _filter =  7.9577e-3
+	// f_cut = 25 Hz -> _filter =  6.3662e-3
+	// f_cut = 30 Hz -> _filter =  5.3052e-3
+};
+
+#endif

libraries/AC_PID/keywords.txt

@@ -0,0 +1,9 @@
+PID		KEYWORD1
+get_pid		KEYWORD2
+reset_I		KEYWORD2
+kP		KEYWORD2
+kD		KEYWORD2
+kI		KEYWORD2
+imax		KEYWORD2
+load_gains	KEYWORD2
+save_gains	KEYWORD2