From f33ba585493e1f573eb2b5b7d82116534cb6810a Mon Sep 17 00:00:00 2001
From: IamPete1 <33176108+IamPete1@users.noreply.github.com>
Date: Thu, 28 Feb 2019 21:38:28 +0000
Subject: [PATCH] Tracker: add PID_TUNING message

---
 AntennaTracker/GCS_Mavlink.cpp | 56 ++++++++++++++++++++++++++++++++++
 AntennaTracker/Parameters.cpp  |  8 +++++
 AntennaTracker/Parameters.h    |  2 ++
 AntennaTracker/Tracker.h       |  1 +
 4 files changed, 67 insertions(+)

diff --git a/AntennaTracker/GCS_Mavlink.cpp b/AntennaTracker/GCS_Mavlink.cpp
index 9789879e00..86ca4b129c 100644
--- a/AntennaTracker/GCS_Mavlink.cpp
+++ b/AntennaTracker/GCS_Mavlink.cpp
@@ -89,6 +89,44 @@ void GCS_MAVLINK_Tracker::send_nav_controller_output() const
         0);
 }
 
+/*
+  send PID tuning message
+ */
+void Tracker::send_pid_tuning(mavlink_channel_t chan)
+{
+
+    // Pitch PID
+    if (g.gcs_pid_mask & 1) {
+        const AP_Logger::PID_Info *pid_info;
+        pid_info = &g.pidPitch2Srv.get_pid_info();
+        mavlink_msg_pid_tuning_send(chan, PID_TUNING_PITCH,
+                                    pid_info->desired,
+                                    pid_info->actual,
+                                    pid_info->FF,
+                                    pid_info->P,
+                                    pid_info->I,
+                                    pid_info->D);
+        if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
+            return;
+        }
+    }
+
+    // Yaw PID
+    if (g.gcs_pid_mask & 2) {
+        const AP_Logger::PID_Info *pid_info;
+        pid_info = &g.pidYaw2Srv.get_pid_info();
+        mavlink_msg_pid_tuning_send(chan, PID_TUNING_YAW,
+                                    pid_info->desired,
+                                    pid_info->actual,
+                                    pid_info->FF,
+                                    pid_info->P,
+                                    pid_info->I,
+                                    pid_info->D);
+        if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
+            return;
+        }
+    }
+}
 
 bool GCS_MAVLINK_Tracker::handle_guided_request(AP_Mission::Mission_Command&)
 {
@@ -101,6 +139,23 @@ void GCS_MAVLINK_Tracker::handle_change_alt_request(AP_Mission::Mission_Command&
     // do nothing
 }
 
+
+// try to send a message, return false if it won't fit in the serial tx buffer
+bool GCS_MAVLINK_Tracker::try_send_message(enum ap_message id)
+{
+    switch (id) {
+
+    case MSG_PID_TUNING:
+        CHECK_PAYLOAD_SIZE(PID_TUNING);
+        tracker.send_pid_tuning(chan);
+        break;
+
+    default:
+        return GCS_MAVLINK::try_send_message(id);
+    }
+    return true;
+}
+
 /*
   default stream rates to 1Hz
  */
@@ -219,6 +274,7 @@ static const ap_message STREAM_RC_CHANNELS_msgs[] = {
 };
 static const ap_message STREAM_EXTRA1_msgs[] = {
     MSG_ATTITUDE,
+    MSG_PID_TUNING,
 };
 static const ap_message STREAM_EXTRA3_msgs[] = {
     MSG_AHRS,
diff --git a/AntennaTracker/Parameters.cpp b/AntennaTracker/Parameters.cpp
index f16046b080..c27bf4df54 100644
--- a/AntennaTracker/Parameters.cpp
+++ b/AntennaTracker/Parameters.cpp
@@ -386,6 +386,14 @@ const AP_Param::Info Tracker::var_info[] = {
     // @Path: ../libraries/AP_BattMonitor/AP_BattMonitor.cpp
     GOBJECT(battery,                "BATT", AP_BattMonitor),
 
+    // @Param: GCS_PID_MASK
+    // @DisplayName: GCS PID tuning mask
+    // @Description: bitmask of PIDs to send MAVLink PID_TUNING messages for
+    // @User: Advanced
+    // @Values: 0:None,1:Pitch,2:Yaw
+    // @Bitmask: 0:Pitch,1:Yaw
+    GSCALAR(gcs_pid_mask,           "GCS_PID_MASK",     0),
+
     AP_VAREND
 };
 
diff --git a/AntennaTracker/Parameters.h b/AntennaTracker/Parameters.h
index 530c7fb788..198c660df4 100644
--- a/AntennaTracker/Parameters.h
+++ b/AntennaTracker/Parameters.h
@@ -116,6 +116,7 @@ public:
         k_param_command_total = 220,
 
         // 254,255: reserved
+        k_param_gcs_pid_mask = 225,
     };
 
     AP_Int16 format_version;
@@ -151,6 +152,7 @@ public:
     AP_Int16 distance_min;          // target's must be at least this distance from tracker to be tracked
     AP_Int16 pitch_min;
     AP_Int16 pitch_max;
+    AP_Int16 gcs_pid_mask;
 
     // Waypoints
     //
diff --git a/AntennaTracker/Tracker.h b/AntennaTracker/Tracker.h
index f748b61a4a..7c642e3e50 100644
--- a/AntennaTracker/Tracker.h
+++ b/AntennaTracker/Tracker.h
@@ -227,6 +227,7 @@ private:
 
     // GCS_Mavlink.cpp
     void send_nav_controller_output(mavlink_channel_t chan);
+    void send_pid_tuning(mavlink_channel_t chan);
 
     // Log.cpp
     void Log_Write_Attitude();