Plane: Consolidate some of the PID logging details

ArduPlane/GCS_Mavlink.cpp

@@ -317,6 +317,25 @@ void NOINLINE Plane::send_rpm(mavlink_channel_t chan)
     }
 }
 
+// sends a single pid info over the provided channel
+void Plane::send_pid_info(const mavlink_channel_t chan, const DataFlash_Class::PID_Info *pid_info,
+                          const uint8_t axis, const float achieved)
+{
+    if (pid_info == nullptr) {
+        return;
+    }
+    if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
+        return;
+    }
+     mavlink_msg_pid_tuning_send(chan, axis,
+                                 pid_info->desired,
+                                 achieved,
+                                 pid_info->FF,
+                                 pid_info->P,
+                                 pid_info->I,
+                                 pid_info->D);
+}
+
 /*
   send PID tuning message
  */
@@ -324,84 +343,35 @@ void Plane::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) {
+    if (g.gcs_pid_mask & TUNING_BITS_ROLL) {
         if (quadplane.in_vtol_mode()) {
             pid_info = &quadplane.attitude_control->get_rate_roll_pid().get_pid_info();
         } else {
             pid_info = &rollController.get_pid_info();
         }
-        mavlink_msg_pid_tuning_send(chan, PID_TUNING_ROLL, 
-                                    pid_info->desired,
-                                    degrees(gyro.x),
-                                    pid_info->FF,
-                                    pid_info->P,
-                                    pid_info->I,
-                                    pid_info->D);
-        if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
-            return;
-        }
+        send_pid_info(chan, pid_info, PID_TUNING_ROLL, degrees(gyro.x));
     }
-    if (g.gcs_pid_mask & 2) {
+    if (g.gcs_pid_mask & TUNING_BITS_PITCH) {
         if (quadplane.in_vtol_mode()) {
             pid_info = &quadplane.attitude_control->get_rate_pitch_pid().get_pid_info();
         } else {
             pid_info = &pitchController.get_pid_info();
         }
-        mavlink_msg_pid_tuning_send(chan, PID_TUNING_PITCH, 
-                                    pid_info->desired,
-                                    degrees(gyro.y),
-                                    pid_info->FF,
-                                    pid_info->P,
-                                    pid_info->I,
-                                    pid_info->D);
-        if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
-            return;
-        }
+        send_pid_info(chan, pid_info, PID_TUNING_PITCH, degrees(gyro.y));
     }
-    if (g.gcs_pid_mask & 4) {
+    if (g.gcs_pid_mask & TUNING_BITS_YAW) {
         if (quadplane.in_vtol_mode()) {
             pid_info = &quadplane.attitude_control->get_rate_yaw_pid().get_pid_info();
         } else {
             pid_info = &yawController.get_pid_info();
         }
-        mavlink_msg_pid_tuning_send(chan, PID_TUNING_YAW,
-                                    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;
-        }
+        send_pid_info(chan, pid_info, PID_TUNING_YAW, degrees(gyro.z));
     }
-    if (g.gcs_pid_mask & 8) {
-        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;
-        }
-    }
-    if ((g.gcs_pid_mask & 0x10) && (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND)) {
-        pid_info = landing.get_pid_info();
-        if (pid_info != nullptr) {
-            mavlink_msg_pid_tuning_send(chan, PID_TUNING_LANDING,
-                                        pid_info->desired,
-                                        gyro.z,
-                                        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 & TUNING_BITS_STEER) {
+        send_pid_info(chan, &steerController.get_pid_info(), PID_TUNING_STEER, degrees(gyro.z));
     }
+    if ((g.gcs_pid_mask & TUNING_BITS_LAND) && (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND)) {
+        send_pid_info(chan, landing.get_pid_info(), PID_TUNING_LANDING, degrees(gyro.z));
     }
 }
 

ArduPlane/Plane.h

@@ -792,6 +792,7 @@ private:
     void send_vfr_hud(mavlink_channel_t chan);
     void send_simstate(mavlink_channel_t chan);
     void send_wind(mavlink_channel_t chan);
+    void send_pid_info(const mavlink_channel_t chan, const DataFlash_Class::PID_Info *pid_info, const uint8_t axis, const float achieved);
     void send_pid_tuning(mavlink_channel_t chan);
     void send_rpm(mavlink_channel_t chan);
 

ArduPlane/defines.h

@@ -116,6 +116,17 @@ typedef enum GeofenceEnableReason {
     GCS_TOGGLED          //Fence enabled/disabled by the GCS via Mavlink
 } GeofenceEnableReason;
 
+// PID broadcast bitmask
+enum tuning_pid_bits {
+    TUNING_BITS_ROLL  = (1 <<  0),
+    TUNING_BITS_PITCH = (1 <<  1),
+    TUNING_BITS_YAW   = (1 <<  2),
+    TUNING_BITS_STEER = (1 <<  3),
+    TUNING_BITS_LAND  = (1 <<  4),
+    TUNING_BITS_END // dummy just used for static checking
+};
+
+static_assert(TUNING_BITS_END <= (1 << 24) + 1, "Tuning bit mask is to large to be set by MAVLink");
 
 // Logging message types
 enum log_messages {