From 94899bd2ed8316d86bb30c8b684bf744ab1fe137 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 1 Mar 2019 10:33:00 +1100 Subject: [PATCH] Plane: move sending of send_pid_tuning up --- ArduPlane/GCS_Mavlink.cpp | 55 ++++++++++++++++++++------------------- ArduPlane/GCS_Mavlink.h | 3 +++ ArduPlane/Plane.h | 2 -- 3 files changed, 31 insertions(+), 29 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index b4ff191827..09dcba97f1 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -324,7 +324,7 @@ 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 AP_Logger::PID_Info *pid_info, +void GCS_MAVLINK_Plane::send_pid_info(const AP_Logger::PID_Info *pid_info, const uint8_t axis, const float achieved) { if (pid_info == nullptr) { @@ -345,44 +345,52 @@ void Plane::send_pid_info(const mavlink_channel_t chan, const AP_Logger::PID_Inf /* send PID tuning message */ -void Plane::send_pid_tuning(mavlink_channel_t chan) +void GCS_MAVLINK_Plane::send_pid_tuning() { + if (plane.control_mode == MANUAL) { + // no PIDs should be used in manual + return; + } + + const Parameters &g = plane.g; + AP_AHRS &ahrs = AP::ahrs(); + const Vector3f &gyro = ahrs.get_gyro(); const AP_Logger::PID_Info *pid_info; 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(); + if (plane.quadplane.in_vtol_mode()) { + pid_info = &plane.quadplane.attitude_control->get_rate_roll_pid().get_pid_info(); } else { - pid_info = &rollController.get_pid_info(); + pid_info = &plane.rollController.get_pid_info(); } - send_pid_info(chan, pid_info, PID_TUNING_ROLL, degrees(gyro.x)); + send_pid_info(pid_info, PID_TUNING_ROLL, degrees(gyro.x)); } 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(); + if (plane.quadplane.in_vtol_mode()) { + pid_info = &plane.quadplane.attitude_control->get_rate_pitch_pid().get_pid_info(); } else { - pid_info = &pitchController.get_pid_info(); + pid_info = &plane.pitchController.get_pid_info(); } - send_pid_info(chan, pid_info, PID_TUNING_PITCH, degrees(gyro.y)); + send_pid_info(pid_info, PID_TUNING_PITCH, degrees(gyro.y)); } 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(); + if (plane.quadplane.in_vtol_mode()) { + pid_info = &plane.quadplane.attitude_control->get_rate_yaw_pid().get_pid_info(); } else { - pid_info = &yawController.get_pid_info(); + pid_info = &plane.yawController.get_pid_info(); } - send_pid_info(chan, pid_info, PID_TUNING_YAW, degrees(gyro.z)); + send_pid_info(pid_info, PID_TUNING_YAW, degrees(gyro.z)); } if (g.gcs_pid_mask & TUNING_BITS_STEER) { - send_pid_info(chan, &steerController.get_pid_info(), PID_TUNING_STEER, degrees(gyro.z)); + send_pid_info(&plane.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)); + if ((g.gcs_pid_mask & TUNING_BITS_LAND) && (plane.flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND)) { + send_pid_info(plane.landing.get_pid_info(), PID_TUNING_LANDING, degrees(gyro.z)); } - if (g.gcs_pid_mask & TUNING_BITS_ACCZ && quadplane.in_vtol_mode()) { + if (g.gcs_pid_mask & TUNING_BITS_ACCZ && plane.quadplane.in_vtol_mode()) { const Vector3f &accel = ahrs.get_accel_ef(); - pid_info = &quadplane.pos_control->get_accel_z_pid().get_pid_info(); - send_pid_info(chan, pid_info, PID_TUNING_ACCZ, -accel.z); + pid_info = &plane.quadplane.pos_control->get_accel_z_pid().get_pid_info(); + send_pid_info(pid_info, PID_TUNING_ACCZ, -accel.z); } } @@ -443,13 +451,6 @@ bool GCS_MAVLINK_Plane::try_send_message(enum ap_message id) plane.send_wind(chan); break; - case MSG_PID_TUNING: - if (plane.control_mode != MANUAL) { - CHECK_PAYLOAD_SIZE(PID_TUNING); - plane.send_pid_tuning(chan); - } - break; - case MSG_RPM: CHECK_PAYLOAD_SIZE(RPM); plane.send_rpm(chan); diff --git a/ArduPlane/GCS_Mavlink.h b/ArduPlane/GCS_Mavlink.h index f4c183718c..d122a40cdc 100644 --- a/ArduPlane/GCS_Mavlink.h +++ b/ArduPlane/GCS_Mavlink.h @@ -42,9 +42,12 @@ protected: uint64_t capabilities() const override; void send_nav_controller_output() const override; + void send_pid_tuning() override; private: + void send_pid_info(const AP_Logger::PID_Info *pid_info, const uint8_t axis, const float achieved); + void handleMessage(mavlink_message_t * msg) override; bool handle_guided_request(AP_Mission::Mission_Command &cmd) override; void handle_change_alt_request(AP_Mission::Mission_Command &cmd) override; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 07a0e39957..0e998f47f7 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -792,8 +792,6 @@ private: void send_fence_status(mavlink_channel_t chan); void send_servo_out(mavlink_channel_t chan); void send_wind(mavlink_channel_t chan); - void send_pid_info(const mavlink_channel_t chan, const AP_Logger::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); void send_aoa_ssa(mavlink_channel_t chan);