diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 7c945a446f..75bc7fa376 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -193,6 +193,7 @@ public: // found. Rover overrides this! virtual void send_rangefinder() const; void send_proximity() const; + virtual void send_nav_controller_output() const = 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 f719402453..40cf3e493e 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -4091,6 +4091,11 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) send_ahrs3(); break; + case MSG_NAV_CONTROLLER_OUTPUT: + CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT); + send_nav_controller_output(); + break; + case MSG_AHRS: CHECK_PAYLOAD_SIZE(AHRS); send_ahrs(); diff --git a/libraries/GCS_MAVLink/GCS_Dummy.h b/libraries/GCS_MAVLink/GCS_Dummy.h index 400eaa6537..2ee10934cd 100644 --- a/libraries/GCS_MAVLink/GCS_Dummy.h +++ b/libraries/GCS_MAVLink/GCS_Dummy.h @@ -36,6 +36,8 @@ protected: bool set_home_to_current_location(bool lock) override { return false; } bool set_home(const Location& loc, bool lock) override { return false; } + + void send_nav_controller_output() const override {}; }; /* diff --git a/libraries/GCS_MAVLink/examples/routing/routing.cpp b/libraries/GCS_MAVLink/examples/routing/routing.cpp index fc93c1890d..8a4f7b9f89 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 get_sensor_status_flags(uint32_t &present, uint32_t &enabled, uint32_t &health) override { present = 0; enabled = 0; health = 0; } + void send_nav_controller_output() const override {}; bool set_home_to_current_location(bool lock) override { return false; } bool set_home(const Location& loc, bool lock) override { return false; }