AC_PID: Integrate PID Logging functionality.

libraries/AC_PID/AC_HELI_PID.cpp

@@ -46,9 +46,10 @@ AC_HELI_PID::AC_HELI_PID(float initial_p, float initial_i, float initial_d, floa
     _ff = initial_ff;
 }
 
-float AC_HELI_PID::get_ff(float requested_rate) const
+float AC_HELI_PID::get_ff(float requested_rate)
 {
-    return (float)requested_rate * _ff;
+    _pid_info.FF = (float)requested_rate * _ff;
+    return _pid_info.FF;
 }
 
 // This is an integrator which tends to decay to zero naturally
@@ -56,16 +57,17 @@ float AC_HELI_PID::get_ff(float requested_rate) const
 
 float AC_HELI_PID::get_leaky_i(float leak_rate)
 {
-	if(!is_zero(_ki) && !is_zero(_dt)){
-		_integrator -= (float)_integrator * leak_rate;
-		_integrator += ((float)_input * _ki) * _dt;
-		if (_integrator < -_imax) {
-			_integrator = -_imax;
-		} else if (_integrator > _imax) {
-			_integrator = _imax;
-		}
+    if(!is_zero(_ki) && !is_zero(_dt)){
+        _integrator -= (float)_integrator * leak_rate;
+        _integrator += ((float)_input * _ki) * _dt;
+        if (_integrator < -_imax) {
+            _integrator = -_imax;
+            } else if (_integrator > _imax) {
+            _integrator = _imax;
+        }
 
-		return _integrator;
-	}
-	return 0;
+        _pid_info.I = _integrator;
+        return _integrator;
+    }
+    return 0;
 }

libraries/AC_PID/AC_HELI_PID.h

@@ -21,7 +21,7 @@ public:
     AC_HELI_PID(float initial_p, float initial_i, float initial_d, float initial_imax, float initial_filt_hz, float dt, float initial_ff);
 
     /// get_ff - return FeedForward Term 
-    float       get_ff(float requested_rate) const;
+    float       get_ff(float requested_rate);
     
     /// get_leaky_i - replacement for get_i but output is leaded at leak_rate
     float       get_leaky_i(float leak_rate);

libraries/AC_PID/AC_PID.cpp

@@ -57,6 +57,8 @@ AC_PID::AC_PID(float initial_p, float initial_i, float initial_d, float initial_
 
     // reset input filter to first value received
     _flags._reset_filter = true;
+
+    memset(&_pid_info, 0, sizeof(_pid_info));
 }
 
 // set_dt - set time step in seconds
@@ -125,9 +127,10 @@ void AC_PID::set_input_filter_d(float input)
     _input = input;
 }
 
-float AC_PID::get_p() const
+float AC_PID::get_p()
 {
-    return (_input * _kp);
+    _pid_info.P = (_input * _kp);
+    return _pid_info.P;
 }
 
 float AC_PID::get_i()
@@ -139,15 +142,17 @@ float AC_PID::get_i()
         } else if (_integrator > _imax) {
             _integrator = _imax;
         }
+        _pid_info.I = _integrator;
         return _integrator;
     }
     return 0;
 }
 
-float AC_PID::get_d() const
+float AC_PID::get_d()
 {
-    // add in derivative component
-    return (_kd * _derivative);
+    // derivative component
+    _pid_info.D = (_kd * _derivative);
+    return _pid_info.D;
 }
 
 float AC_PID::get_pi()

libraries/AC_PID/AC_PID.h

@@ -10,6 +10,7 @@
 #include <AP_Param.h>
 #include <stdlib.h>
 #include <math.h>
+#include <DataFlash.h>
 
 #define AC_PID_FILT_HZ_DEFAULT  20.0f   // default input filter frequency
 #define AC_PID_FILT_HZ_MIN      0.01f   // minimum input filter frequency
@@ -38,9 +39,9 @@ public:
     // get_pid - get results from pid controller
     float       get_pid();
     float       get_pi();
-    float       get_p() const;
+    float       get_p();
     float       get_i();
-    float       get_d() const;
+    float       get_d();
 
     // reset_I - reset the integrator
     void        reset_I();
@@ -75,6 +76,8 @@ public:
     float       get_integrator() const { return _integrator; }
     void        set_integrator(float i) { _integrator = i; }
 
+    const       DataFlash_Class::PID_Info& get_pid_info(void) const { return _pid_info; }
+
     // parameter var table
     static const struct AP_Param::GroupInfo        var_info[];
 
@@ -93,10 +96,12 @@ protected:
     } _flags;
 
     // internal variables
-    float           _dt;                        // timestep in seconds
-    float           _integrator;                // integrator value
-    float           _input;                // last input for derivative
-    float           _derivative;           // last derivative for low-pass filter
+    float           _dt;                    // timestep in seconds
+    float           _integrator;            // integrator value
+    float           _input;                 // last input for derivative
+    float           _derivative;            // last derivative for low-pass filter
+
+    DataFlash_Class::PID_Info        _pid_info;
 };
 
 #endif // __AC_PID_H__