From 273c80116a187a9970792a72ba43235ca9b63ee7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 17 May 2016 08:36:49 +1000 Subject: [PATCH] GCS_MAVLink: added a wrapper for sending HEARTBEAT will be used to cope with old radios with MAVLink2 --- libraries/GCS_MAVLink/GCS.h | 1 + libraries/GCS_MAVLink/GCS_Common.cpp | 14 ++++++++++++++ 2 files changed, 15 insertions(+) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index dd9fa08603..08827ae0bb 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -163,6 +163,7 @@ public: void send_vibration(const AP_InertialSensor &ins) const; void send_home(const Location &home) const; static void send_home_all(const Location &home); + void send_heartbeat(uint8_t type, uint8_t base_mode, uint32_t custom_mode, uint8_t system_status); // return a bitmap of active channels. Used by libraries to loop // over active channels to send to all active channels diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 6fb62464e0..f6c761421b 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1580,3 +1580,17 @@ void GCS_MAVLINK::send_home_all(const Location &home) } } } + +/* + wrapper for sending heartbeat + */ +void GCS_MAVLINK::send_heartbeat(uint8_t type, uint8_t base_mode, uint32_t custom_mode, uint8_t system_status) +{ + mavlink_msg_heartbeat_send( + chan, + type, + MAV_AUTOPILOT_ARDUPILOTMEGA, + base_mode, + custom_mode, + system_status); +}