Tracker: add PID_TUNING message

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,

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
 };
 

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
     //

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();