Rover: added the PID logging for steering into the mavlink message.

Copter/Plane already have this PID logging in the mavlink stream to
the GCS and now Rover does too.

APMrover2/GCS_Mavlink.cpp

@@ -6,6 +6,7 @@
 #define MAVLINK_SENSOR_PRESENT_DEFAULT (MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL | MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL | MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | MAV_SYS_STATUS_SENSOR_YAW_POSITION | MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL | MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS | MAV_SYS_STATUS_AHRS)
 
 // 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
 
 void Rover::send_heartbeat(mavlink_channel_t chan)
@@ -350,6 +351,27 @@ void Rover::send_rangefinder(mavlink_channel_t chan)
         voltage);
 }
 
+/*
+  send PID tuning message
+ */
+void Rover::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 = steerController.get_pid_info();
+        mavlink_msg_pid_tuning_send(chan, PID_TUNING_STEER, 
+                                    pid_info.desired,
+                                    degrees(gyro.z),
+                                    pid_info.FF,
+                                    pid_info.P,
+                                    pid_info.I,
+                                    pid_info.D);
+        if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
+            return;
+        }
+    }
+}
+
 void Rover::send_current_waypoint(mavlink_channel_t chan)
 {
     mavlink_msg_mission_current_send(chan, mission.get_current_nav_index());
@@ -530,7 +552,6 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
     case MSG_LIMITS_STATUS:
     case MSG_FENCE_STATUS:
     case MSG_WIND:
-    case MSG_PID_TUNING:
     case MSG_VIBRATION:
         // unused
         break;
@@ -554,6 +575,11 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
 #endif
         break;
 
+    case MSG_PID_TUNING:
+        CHECK_PAYLOAD_SIZE(PID_TUNING);
+        rover.send_pid_tuning(chan);
+        break;
+
     case MSG_RETRY_DEFERRED:
     case MSG_TERRAIN:
     case MSG_OPTICAL_FLOW:
@@ -765,8 +791,11 @@ GCS_MAVLINK::data_stream_send(void)
     if (stream_trigger(STREAM_EXTRA1)) {
         send_message(MSG_ATTITUDE);
         send_message(MSG_SIMSTATE);
+        if (rover.control_mode != MANUAL) {
+            send_message(MSG_PID_TUNING);
+        }
     }
-
+    
     if (rover.gcs_out_of_time) return;
 
     if (stream_trigger(STREAM_EXTRA2)) {

APMrover2/Parameters.cpp

@@ -76,6 +76,14 @@ const AP_Param::Info Rover::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:Steering
+    // @Bitmask: 0:Steering
+    GSCALAR(gcs_pid_mask,           "GCS_PID_MASK",     0),
+    
     // @Param: SKIP_GYRO_CAL
     // @DisplayName: Skip gyro calibration
     // @Description: When enabled this tells the APM to skip the normal gyroscope calibration at startup, and instead use the saved gyro calibration from the last flight. You should only enable this if you are careful to check that your aircraft has good attitude control before flying, as some boards may have significantly different gyro calibration between boots, especially if the temperature changes a lot. If gyro calibration is skipped then APM relies on using the gyro drift detection code to get the right gyro calibration in the few minutes after it boots. This option is mostly useful where the requirement to hold the vehicle still while it is booting is a significant problem.

APMrover2/Parameters.h

@@ -72,6 +72,7 @@ public:
         k_param_serial_manager,     // serial manager library
         k_param_cli_enabled,
         k_param_gcs3,
+        k_param_gcs_pid_mask,
 
         //
         // 130: Sensor parameters
@@ -232,6 +233,7 @@ public:
     AP_Float    auto_kickstart;
     AP_Float    turn_max_g;
     AP_Int16    pivot_turn_angle;
+    AP_Int16    gcs_pid_mask;
 
     // RC channels
     RC_Channel      rc_1;

APMrover2/Rover.h

@@ -393,6 +393,7 @@ private:
     void send_vfr_hud(mavlink_channel_t chan);
     void send_simstate(mavlink_channel_t chan);
     void send_hwstatus(mavlink_channel_t chan);
+    void send_pid_tuning(mavlink_channel_t chan);
     void send_rangefinder(mavlink_channel_t chan);
     void send_current_waypoint(mavlink_channel_t chan);
     void send_statustext(mavlink_channel_t chan);