Copter: added GCS_PID_MASK for mavlink rate tuning

ArduCopter/GCS_Mavlink.pde

@@ -14,6 +14,7 @@ static bool	gcs_out_of_time;
 
 
 // check if a message will fit in the payload space available
+#define HAVE_PAYLOAD_SPACE(chan, id) (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_ ## id ## _LEN)
 #define CHECK_PAYLOAD_SIZE(id) if (txspace < MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_ ## id ## _LEN) return false
 
 static void gcs_send_heartbeat(void)
@@ -421,6 +422,55 @@ static void NOINLINE send_rangefinder(mavlink_channel_t chan)
 }
 #endif
 
+
+/*
+  send PID tuning message
+ */
+static void send_pid_tuning(mavlink_channel_t chan)
+{
+    const Vector3f &gyro = ahrs.get_gyro();
+    if (g.gcs_pid_mask & 1) {
+        const DataFlash_Class::PID_Info &pid_info = g.pid_rate_roll.get_pid_info();
+        mavlink_msg_pid_tuning_send(chan, 1, 
+                                    pid_info.desired*0.01f,
+                                    degrees(gyro.x),
+                                    pid_info.FF*0.01,
+                                    pid_info.P*0.01,
+                                    pid_info.I*0.01,
+                                    pid_info.D*0.01);
+        if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
+            return;
+        }
+    }
+    if (g.gcs_pid_mask & 2) {
+        const DataFlash_Class::PID_Info &pid_info = g.pid_rate_pitch.get_pid_info();
+        mavlink_msg_pid_tuning_send(chan, 2, 
+                                    pid_info.desired*0.01f,
+                                    degrees(gyro.y),
+                                    pid_info.FF*0.01f,
+                                    pid_info.P*0.01f,
+                                    pid_info.I*0.01f,
+                                    pid_info.D*0.01f);
+        if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
+            return;
+        }
+    }
+    if (g.gcs_pid_mask & 4) {
+        const DataFlash_Class::PID_Info &pid_info = g.pid_rate_yaw.get_pid_info();
+        mavlink_msg_pid_tuning_send(chan, 3, 
+                                    pid_info.desired*0.01f,
+                                    degrees(gyro.z),
+                                    pid_info.FF*0.01f,
+                                    pid_info.P*0.01f,
+                                    pid_info.I*0.01f,
+                                    pid_info.D*0.01f);
+        if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
+            return;
+        }
+    }
+}
+
+
 static void NOINLINE send_statustext(mavlink_channel_t chan)
 {
     mavlink_statustext_t *s = &gcs[chan-MAVLINK_COMM_0].pending_status;
@@ -659,6 +709,11 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
         // unused
         break;
 
+    case MSG_PID_TUNING:
+        CHECK_PAYLOAD_SIZE(PID_TUNING);
+        send_pid_tuning(chan);
+        break;
+
     case MSG_RETRY_DEFERRED:
         break; // just here to prevent a warning
     }
@@ -859,6 +914,7 @@ GCS_MAVLINK::data_stream_send(void)
     if (stream_trigger(STREAM_EXTRA1)) {
         send_message(MSG_ATTITUDE);
         send_message(MSG_SIMSTATE);
+        send_message(MSG_PID_TUNING);
     }
 
     if (gcs_out_of_time) return;

ArduCopter/Parameters.h

@@ -194,6 +194,7 @@ public:
         k_param_ch12_option,     // 123
         k_param_takeoff_trigger_dz,
         k_param_gcs3,            // 125
+        k_param_gcs_pid_mask,
 
         //
         // 140: Sensor parameters
@@ -414,6 +415,7 @@ public:
 
     AP_Int8         land_repositioning;
     AP_Float        ekfcheck_thresh;
+    AP_Int8         gcs_pid_mask;
 
 #if FRAME_CONFIG ==     HELI_FRAME
     // Heli

ArduCopter/Parameters.pde

@@ -108,6 +108,13 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @Increment: 1
     GSCALAR(telem_delay,            "TELEM_DELAY",     0),
 
+    // @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:Roll,2:Pitch,4:Yaw
+    GSCALAR(gcs_pid_mask,           "GCS_PID_MASK",     0),
+
     // @Param: RTL_ALT
     // @DisplayName: RTL Altitude
     // @Description: The minimum altitude the model will move to before Returning to Launch.  Set to zero to return at current altitude.