From 35a132f74e0aef97f3360c1c893002d1d69fed3c Mon Sep 17 00:00:00 2001
From: Grant Morphett <grant@gmorph.com>
Date: Mon, 30 May 2016 09:44:23 +1000
Subject: [PATCH] Rover: Added throttle PID logging

---
 APMrover2/GCS_Mavlink.cpp | 26 ++++++++++++++++++++------
 APMrover2/Log.cpp         |  4 +++-
 APMrover2/Parameters.cpp  |  4 ++--
 3 files changed, 25 insertions(+), 9 deletions(-)

diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp
index 93db560f4c..0af2b5d9e0 100644
--- a/APMrover2/GCS_Mavlink.cpp
+++ b/APMrover2/GCS_Mavlink.cpp
@@ -363,15 +363,29 @@ void Rover::send_rangefinder(mavlink_channel_t chan)
 void Rover::send_pid_tuning(mavlink_channel_t chan)
 {
     const Vector3f &gyro = ahrs.get_gyro();
+    const DataFlash_Class::PID_Info *pid_info;
     if (g.gcs_pid_mask & 1) {
-        const DataFlash_Class::PID_Info &pid_info = steerController.get_pid_info();
+        pid_info = &steerController.get_pid_info();
         mavlink_msg_pid_tuning_send(chan, PID_TUNING_STEER, 
-                                    pid_info.desired,
+                                    pid_info->desired,
                                     degrees(gyro.z),
-                                    pid_info.FF,
-                                    pid_info.P,
-                                    pid_info.I,
-                                    pid_info.D);
+                                    pid_info->FF,
+                                    pid_info->P,
+                                    pid_info->I,
+                                    pid_info->D);
+        if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
+            return;
+        }
+    }
+    if (g.gcs_pid_mask & 2) {
+        pid_info = &g.pidSpeedThrottle.get_pid_info();
+        mavlink_msg_pid_tuning_send(chan, PID_TUNING_ACCZ, 
+                                    pid_info->desired,
+                                    0,
+                                    0,
+                                    pid_info->P,
+                                    pid_info->I,
+                                    pid_info->D);
         if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
             return;
         }
diff --git a/APMrover2/Log.cpp b/APMrover2/Log.cpp
index 92627b88f2..b16f3ebe77 100644
--- a/APMrover2/Log.cpp
+++ b/APMrover2/Log.cpp
@@ -293,7 +293,9 @@ void Rover::Log_Write_Attitude()
 #endif
     DataFlash.Log_Write_POS(ahrs);
 
-    DataFlash.Log_Write_PID(LOG_PIDY_MSG, steerController.get_pid_info());
+    DataFlash.Log_Write_PID(LOG_PIDS_MSG, steerController.get_pid_info());
+
+    DataFlash.Log_Write_PID(LOG_PIDA_MSG, g.pidSpeedThrottle.get_pid_info());
 }
 
 struct PACKED log_Sonar {
diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp
index 2f38928679..f659598e57 100644
--- a/APMrover2/Parameters.cpp
+++ b/APMrover2/Parameters.cpp
@@ -88,8 +88,8 @@ const AP_Param::Info Rover::var_info[] = {
     // @DisplayName: GCS PID tuning mask
     // @Description: bitmask of PIDs to send MAVLink PID_TUNING messages for
     // @User: Advanced
-    // @Values: 0:None,1:Steering
-    // @Bitmask: 0:Steering
+    // @Values: 0:None,1:Steering,2:Throttle
+    // @Bitmask: 0:Steering,1:Throttle
     GSCALAR(gcs_pid_mask,           "GCS_PID_MASK",     0),
 
     // @Param: MAG_ENABLED