From 944541b2879ae22a2c9853cd91cb01c8f9d8d2e1 Mon Sep 17 00:00:00 2001
From: Grant Morphett <grant@gmorph.com>
Date: Mon, 30 May 2016 16:48:40 +1000
Subject: [PATCH] PID: Added PID_Info to class for PID logging

---
 libraries/PID/PID.cpp | 11 +++++++++--
 libraries/PID/PID.h   |  5 +++++
 2 files changed, 14 insertions(+), 2 deletions(-)

diff --git a/libraries/PID/PID.cpp b/libraries/PID/PID.cpp
index f61e55a81f..5e374dcde3 100644
--- a/libraries/PID/PID.cpp
+++ b/libraries/PID/PID.cpp
@@ -57,7 +57,8 @@ float PID::get_pid(float error, float scaler)
     delta_time = (float)dt / 1000.0f;
 
     // Compute proportional component
-    output += error * _kp;
+    _pid_info.P = error * _kp;
+    output += _pid_info.P;
 
     // Compute derivative component if time has elapsed
     if ((fabsf(_kd) > 0) && (dt > 0)) {
@@ -85,11 +86,14 @@ float PID::get_pid(float error, float scaler)
         _last_derivative    = derivative;
 
         // add in derivative component
-        output                          += _kd * derivative;
+        _pid_info.D = _kd * derivative;
+        output                          += _pid_info.D;
     }
 
     // scale the P and D components
     output *= scaler;
+    _pid_info.D *= scaler;
+    _pid_info.P *= scaler;
 
     // Compute integral component if time has elapsed
     if ((fabsf(_ki) > 0) && (dt > 0)) {
@@ -99,9 +103,11 @@ float PID::get_pid(float error, float scaler)
         } else if (_integrator > _imax) {
             _integrator = _imax;
         }
+        _pid_info.I = _integrator;
         output                          += _integrator;
     }
 
+    _pid_info.desired = output;
     return output;
 }
 
@@ -118,6 +124,7 @@ PID::reset_I()
 	// we use NAN (Not A Number) to indicate that the last 
 	// derivative value is not valid
     _last_derivative = NAN;
+    _pid_info.I = 0;
 }
 
 void
diff --git a/libraries/PID/PID.h b/libraries/PID/PID.h
index 239db265aa..8c398e411b 100644
--- a/libraries/PID/PID.h
+++ b/libraries/PID/PID.h
@@ -6,6 +6,7 @@
 
 #include <AP_Common/AP_Common.h>
 #include <AP_Param/AP_Param.h>
+#include <DataFlash/DataFlash.h>
 #include <stdlib.h>
 #include <cmath>
 
@@ -98,6 +99,8 @@ public:
 
     static const struct AP_Param::GroupInfo        var_info[];
 
+    const DataFlash_Class::PID_Info& get_pid_info(void) const { return _pid_info; }
+
 private:
     AP_Float        _kp;
     AP_Float        _ki;
@@ -111,6 +114,8 @@ private:
 
     float           _get_pid(float error, uint16_t dt, float scaler);
 
+    DataFlash_Class::PID_Info _pid_info {};
+
     /// Low pass filter cut frequency for derivative calculation.
     ///
     /// 20 Hz because anything over that is probably noise, see