From b04d084a5022c4e9b1e314f7670cad04fe95412c Mon Sep 17 00:00:00 2001 From: Peter Barker <pbarker@barker.dropbear.id.au> Date: Fri, 1 Mar 2019 10:33:10 +1100 Subject: [PATCH] Sub: move sending of send_pid_tuning up --- ArduSub/GCS_Mavlink.cpp | 13 ++++++------- ArduSub/GCS_Mavlink.h | 2 ++ ArduSub/Sub.h | 1 - 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 6f82cc0ff5..d08dc7825e 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -166,8 +166,12 @@ bool GCS_MAVLINK_Sub::send_info() /* send PID tuning message */ -void Sub::send_pid_tuning(mavlink_channel_t chan) +void GCS_MAVLINK_Sub::send_pid_tuning() { + const Parameters &g = sub.g; + AP_AHRS &ahrs = AP::ahrs(); + AC_AttitudeControl_Sub &attitude_control = sub.attitude_control; + const Vector3f &gyro = ahrs.get_gyro(); if (g.gcs_pid_mask & 1) { const AP_Logger::PID_Info &pid_info = attitude_control.get_rate_roll_pid().get_pid_info(); @@ -209,7 +213,7 @@ void Sub::send_pid_tuning(mavlink_channel_t chan) } } if (g.gcs_pid_mask & 8) { - const AP_Logger::PID_Info &pid_info = pos_control.get_accel_z_pid().get_pid_info(); + const AP_Logger::PID_Info &pid_info = sub.pos_control.get_accel_z_pid().get_pid_info(); mavlink_msg_pid_tuning_send(chan, PID_TUNING_ACCZ, pid_info.desired*0.01f, -(ahrs.get_accel_ef_blended().z + GRAVITY_MSS), @@ -263,11 +267,6 @@ bool GCS_MAVLINK_Sub::try_send_message(enum ap_message id) #endif break; - case MSG_PID_TUNING: - CHECK_PAYLOAD_SIZE(PID_TUNING); - sub.send_pid_tuning(chan); - break; - default: return GCS_MAVLINK::try_send_message(id); } diff --git a/ArduSub/GCS_Mavlink.h b/ArduSub/GCS_Mavlink.h index 850cb97733..c06bc74321 100644 --- a/ArduSub/GCS_Mavlink.h +++ b/ArduSub/GCS_Mavlink.h @@ -36,6 +36,8 @@ protected: bool set_home(const Location& loc, bool lock) override WARN_IF_UNUSED; void send_nav_controller_output() const override; + void send_pid_tuning() override; + uint64_t capabilities() const override; private: diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index c045a33e42..a612140252 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -483,7 +483,6 @@ private: void send_rpm(mavlink_channel_t chan); void rpm_update(); #endif - void send_pid_tuning(mavlink_channel_t chan); void Log_Write_Control_Tuning(); void Log_Write_Performance(); void Log_Write_Attitude();