From 74702b86883d72133db11abde889cef0bf1c543d Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 1 Mar 2019 10:24:13 +1100 Subject: [PATCH] GCS_MAVLink: make sending of send_pid_tuning up --- libraries/GCS_MAVLink/GCS.h | 1 + libraries/GCS_MAVLink/GCS_Common.cpp | 5 +++++ libraries/GCS_MAVLink/GCS_Dummy.h | 1 + libraries/GCS_MAVLink/examples/routing/routing.cpp | 1 + 4 files changed, 8 insertions(+) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index fd122ea348..adc325f6dc 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -196,6 +196,7 @@ public: virtual void send_rangefinder() const; void send_proximity() const; virtual void send_nav_controller_output() const = 0; + virtual void send_pid_tuning() = 0; void send_ahrs2(); void send_ahrs3(); void send_system_time(); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 160dc52668..e924c32f25 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -4137,6 +4137,11 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) send_ahrs3(); break; + case MSG_PID_TUNING: + CHECK_PAYLOAD_SIZE(PID_TUNING); + send_pid_tuning(); + break; + case MSG_NAV_CONTROLLER_OUTPUT: CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT); send_nav_controller_output(); diff --git a/libraries/GCS_MAVLink/GCS_Dummy.h b/libraries/GCS_MAVLink/GCS_Dummy.h index 20d9e138bb..001db936cb 100644 --- a/libraries/GCS_MAVLink/GCS_Dummy.h +++ b/libraries/GCS_MAVLink/GCS_Dummy.h @@ -37,6 +37,7 @@ protected: bool set_home(const Location& loc, bool lock) override { return false; } void send_nav_controller_output() const override {}; + void send_pid_tuning() override {}; }; /* diff --git a/libraries/GCS_MAVLink/examples/routing/routing.cpp b/libraries/GCS_MAVLink/examples/routing/routing.cpp index 9d044f90fb..38e10f32b7 100644 --- a/libraries/GCS_MAVLink/examples/routing/routing.cpp +++ b/libraries/GCS_MAVLink/examples/routing/routing.cpp @@ -39,6 +39,7 @@ protected: uint32_t custom_mode() const override { return 3; } // magic number MAV_STATE system_status() const override { return MAV_STATE_CALIBRATING; } void send_nav_controller_output() const override {}; + void send_pid_tuning() override {}; bool set_home_to_current_location(bool lock) override { return false; } bool set_home(const Location& loc, bool lock) override { return false; }