diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 55913b7d12..48268ea2f5 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -492,6 +492,19 @@ void Plane::send_pid_tuning(mavlink_channel_t chan) return; } } + if (g.gcs_pid_mask & 4) { + const DataFlash_Class::PID_Info &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; + } + } if (g.gcs_pid_mask & 8) { const DataFlash_Class::PID_Info &pid_info = steerController.get_pid_info(); mavlink_msg_pid_tuning_send(chan, PID_TUNING_STEER, diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index 5087cac532..dd27c705f5 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -171,6 +171,7 @@ void Plane::Log_Write_Attitude(void) #if HAL_CPU_CLASS > HAL_CPU_CLASS_16 DataFlash.Log_Write_PID(LOG_PIDR_MSG, rollController.get_pid_info()); DataFlash.Log_Write_PID(LOG_PIDP_MSG, pitchController.get_pid_info()); + DataFlash.Log_Write_PID(LOG_PIDY_MSG, yawController.get_pid_info()); DataFlash.Log_Write_PID(LOG_PIDS_MSG, steerController.get_pid_info()); #endif