APM_Control: added get_pid_info() interface

libraries/APM_Control/AP_PitchController.cpp

@@ -141,17 +141,17 @@ int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool
 				// prevent the integrator from decreasing if surface defln demand  is below the lower limit
 				integrator_delta = min(integrator_delta , 0);
 			}
-			_integrator += integrator_delta;
+			_pid_info.I += integrator_delta;
 		}
 	} else {
-		_integrator = 0;
+		_pid_info.I = 0;
 	}
 
     // Scale the integration limit
     float intLimScaled = gains.imax * 0.01f;
 
     // Constrain the integrator state
-    _integrator = constrain_float(_integrator, -intLimScaled, intLimScaled);
+    _pid_info.I = constrain_float(_pid_info.I, -intLimScaled, intLimScaled);
 
 	// Calculate equivalent gains so that values for K_P and K_I can be taken across from the old PID law
     // No conversion is required for K_D
@@ -161,7 +161,10 @@ int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool
 	// Note the scaler is applied again. We want a 1/speed scaler applied to the feed-forward
 	// path, but want a 1/speed^2 scaler applied to the rate error path. 
 	// This is because acceleration scales with speed^2, but rate scales with speed.
-	_last_out = ( (rate_error * gains.D) + (desired_rate * kp_ff) ) * scaler;
+    _pid_info.FF = desired_rate * kp_ff * scaler;
+    _pid_info.D = rate_error * gains.D * scaler;
+	_last_out = _pid_info.D + _pid_info.FF;
+    _pid_info.desired = desired_rate;
 
     if (autotune.running && aspeed > aparm.airspeed_min) {
         // let autotune have a go at the values 
@@ -174,7 +177,7 @@ int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool
         _max_rate_neg.set_and_save_ifchanged(gains.rmax);
     }
 
-	_last_out += _integrator;
+	_last_out += _pid_info.I;
 	
 	// Convert to centi-degrees and constrain
 	return constrain_float(_last_out * 100, -4500, 4500);
@@ -290,5 +293,5 @@ int32_t AP_PitchController::get_servo_out(int32_t angle_err, float scaler, bool
 
 void AP_PitchController::reset_I()
 {
-	_integrator = 0;
+	_pid_info.I = 0;
 }

libraries/APM_Control/AP_PitchController.h

@@ -28,6 +28,8 @@ public:
     void autotune_start(void) { autotune.start(); }
     void autotune_restore(void) { autotune.stop(); }
 
+    const DataFlash_Class::PID_Info& get_pid_info(void) const { return _pid_info; }
+
 	static const struct AP_Param::GroupInfo var_info[];
 
 private:
@@ -39,7 +41,7 @@ private:
 	uint32_t _last_t;
 	float _last_out;
 	
-	float _integrator;
+    DataFlash_Class::PID_Info _pid_info;
 
 	int32_t _get_rate_out(float desired_rate, float scaler, bool disable_integrator, float aspeed);
     float   _get_coordination_rate_offset(float &aspeed, bool &inverted) const;

libraries/APM_Control/AP_RollController.cpp

@@ -133,23 +133,27 @@ int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool
                 // prevent the integrator from decreasing if surface defln demand  is below the lower limit
                  integrator_delta = min(integrator_delta, 0);
             }
-			_integrator += integrator_delta;
+			_pid_info.I += integrator_delta;
 		}
 	} else {
-		_integrator = 0;
+		_pid_info.I = 0;
 	}
 	
     // Scale the integration limit
     float intLimScaled = gains.imax * 0.01f;
 
     // Constrain the integrator state
-    _integrator = constrain_float(_integrator, -intLimScaled, intLimScaled);
+    _pid_info.I = constrain_float(_pid_info.I, -intLimScaled, intLimScaled);
 	
 	// Calculate the demanded control surface deflection
 	// Note the scaler is applied again. We want a 1/speed scaler applied to the feed-forward
 	// path, but want a 1/speed^2 scaler applied to the rate error path. 
 	// This is because acceleration scales with speed^2, but rate scales with speed.
-	_last_out = ( (rate_error * gains.D) + (desired_rate * kp_ff) ) * scaler;
+    _pid_info.D = rate_error * gains.D * scaler;
+    _pid_info.FF = desired_rate * kp_ff * scaler;
+    _pid_info.desired = desired_rate;
+
+	_last_out = _pid_info.FF + _pid_info.D;
 
     if (autotune.running && aspeed > aparm.airspeed_min) {
         // let autotune have a go at the values 
@@ -159,7 +163,7 @@ int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool
         autotune.update(desired_rate, achieved_rate, _last_out);
     }
 
-	_last_out += _integrator;
+	_last_out += _pid_info.I;
 	
 	// Convert to centi-degrees and constrain
 	return constrain_float(_last_out * 100, -4500, 4500);
@@ -201,6 +205,6 @@ int32_t AP_RollController::get_servo_out(int32_t angle_err, float scaler, bool d
 
 void AP_RollController::reset_I()
 {
-	_integrator = 0;
+	_pid_info.I = 0;
 }
 

libraries/APM_Control/AP_RollController.h

@@ -9,6 +9,7 @@
 #include <AP_AutoTune.h>
 #include <DataFlash.h>
 #include <AP_Math.h>
+#include <DataFlash.h>
 
 class AP_RollController {
 public:
@@ -28,6 +29,8 @@ public:
     void autotune_start(void) { autotune.start(); }
     void autotune_restore(void) { autotune.stop(); }
 
+    const       DataFlash_Class::PID_Info& get_pid_info(void) const { return _pid_info; }
+
 	static const struct AP_Param::GroupInfo var_info[];
 
 private:
@@ -37,7 +40,7 @@ private:
 	uint32_t _last_t;
 	float _last_out;
 
-	float _integrator;
+    DataFlash_Class::PID_Info _pid_info;
 
 	int32_t _get_rate_out(float desired_rate, float scaler, bool disable_integrator);