From faea5d285cf47f1b747a3763671b58b6ca3faaf7 Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Fri, 2 Mar 2018 17:46:46 -0700 Subject: [PATCH] Plane: Consolidate some of the PID logging details --- ArduPlane/GCS_Mavlink.cpp | 88 +++++++++++++-------------------------- ArduPlane/Plane.h | 1 + ArduPlane/defines.h | 11 +++++ 3 files changed, 41 insertions(+), 59 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 308803968c..a51c4a8192 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/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 & TUNING_BITS_STEER) { + send_pid_info(chan, &steerController.get_pid_info(), PID_TUNING_STEER, degrees(gyro.z)); } - 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_LAND) && (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND)) { + send_pid_info(chan, landing.get_pid_info(), PID_TUNING_LANDING, degrees(gyro.z)); } } diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 518d5313ff..bc311d7c8b 100644 --- a/ArduPlane/Plane.h +++ b/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); diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 2ecb2cebee..7da27cfd0f 100644 --- a/ArduPlane/defines.h +++ b/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 {