AC_PID: Add new AC_HELI_PID as a child of AC_PID

libraries/AC_PID/AC_HELI_PID.cpp

@@ -0,0 +1,55 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/// @file	AC_HELI_PID.cpp
+/// @brief	Generic PID algorithm
+
+#include <AP_Math.h>
+#include "AC_HELI_PID.h"
+
+const AP_Param::GroupInfo AC_HELI_PID::var_info[] PROGMEM = {
+    // @Param: P
+    // @DisplayName: PID Proportional Gain
+    // @Description: P Gain which produces an output value that is proportional to the current error value
+    AP_GROUPINFO("P",    0, AC_HELI_PID, _kp, 0),
+    // @Param: I
+    // @DisplayName: PID Integral Gain
+    // @Description: I Gain which produces an output that is proportional to both the magnitude and the duration of the error
+    AP_GROUPINFO("I",    1, AC_HELI_PID, _ki, 0),
+    // @Param: D
+    // @DisplayName: PID Derivative Gain
+    // @Description: D Gain which produces an output that is proportional to the rate of change of the error
+    AP_GROUPINFO("D",    2, AC_HELI_PID, _kd, 0),
+    // @Param: IMAX
+    // @DisplayName: PID Integral Maximum
+    // @Description: The maximum/minimum value that the I term can output
+    AP_GROUPINFO("IMAX", 3, AC_HELI_PID, _imax, 0),
+    // @Param: FC
+    // @DisplayName: PID+FF FeedForward Gain
+    // @Description: FF Gain which produces an output value that is proportional to the current error value
+    AP_GROUPINFO("FF",    4, AC_HELI_PID, _ff, 0),
+    AP_GROUPEND
+};
+
+float AC_HELI_PID::get_ff(float requested_rate) const
+{
+    return (float)requested_rate * _ff;
+}
+
+// This is an integrator which tends to decay to zero naturally
+// if the error is zero.
+
+float AC_HELI_PID::get_leaky_i(float error, float dt, float leak_rate)
+{
+	if((_ki != 0) && (dt != 0)){
+		_integrator -= (float)_integrator * leak_rate;
+		_integrator += ((float)error * _ki) * dt;
+		if (_integrator < -_imax) {
+			_integrator = -_imax;
+		} else if (_integrator > _imax) {
+			_integrator = _imax;
+		}
+
+		return _integrator;
+	}
+	return 0;
+}

libraries/AC_PID/AC_HELI_PID.h

@@ -0,0 +1,62 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/// @file	AC_HELI_PID.h
+/// @brief	Helicopter Specific Rate PID algorithm, with EEPROM-backed storage of constants.
+
+#ifndef __AC_HELI_PID_H__
+#define __AC_HELI_PID_H__
+
+#include <AP_Common.h>
+#include <AP_Param.h>
+#include <stdlib.h>
+#include <math.h>
+#include <AC_PID.h>
+
+// 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
+#define AC_HELI_PID_D_TERM_FILTER 0.00795770f    // 20hz filter on D term
+
+/// @class	AC_HELI_PID
+/// @brief	Object managing one PID control
+class AC_HELI_PID : public 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  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_HELI_PID(
+        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,
+        const float &   initial_ff = 0.0) :
+        AC_PID(initial_p, initial_i, initial_d, initial_imax)
+    {
+		 _ff = initial_ff;
+    }
+    
+    /// get_ff - return FeedForward Term 
+    float       get_ff(float requested_rate) const;
+    
+    /// get_leaky_i - replacement for get_i but output is leaded at leak_rate
+    float       get_leaky_i(float error, float dt, float leak_rate);
+
+    static const struct AP_Param::GroupInfo        var_info[];
+    
+private:
+    AP_Float        _ff;
+    
+};
+
+#endif // __AC_HELI_PID_H__