From 71df552d27f69cba36f5c37b3dd3c7a49d335b0e Mon Sep 17 00:00:00 2001
From: DonLakeFlyer <don@thegagnes.com>
Date: Mon, 30 Apr 2018 16:01:08 -0700
Subject: [PATCH] Support PING response

---
 src/Vehicle/Vehicle.cc | 20 ++++++++++++++++++++
 src/Vehicle/Vehicle.h  |  1 +
 2 files changed, 21 insertions(+)

diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc
index a6adf2804..e91bba69b 100644
--- a/src/Vehicle/Vehicle.cc
+++ b/src/Vehicle/Vehicle.cc
@@ -726,6 +726,9 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
     case MAVLINK_MSG_ID_DISTANCE_SENSOR:
         _handleDistanceSensor(message);
         break;
+    case MAVLINK_MSG_ID_PING:
+        _handlePing(link, message);
+        break;
 
     case MAVLINK_MSG_ID_SERIAL_CONTROL:
     {
@@ -1377,6 +1380,23 @@ void Vehicle::_updateArmed(bool armed)
     }
 }
 
+void Vehicle::_handlePing(LinkInterface* link, mavlink_message_t& message)
+{
+    mavlink_ping_t      ping;
+    mavlink_message_t   msg;
+
+    mavlink_msg_ping_decode(&message, &ping);
+    mavlink_msg_ping_pack_chan(_mavlink->getSystemId(),
+                               _mavlink->getComponentId(),
+                               priorityLink()->mavlinkChannel(),
+                               &msg,
+                               ping.time_usec,
+                               ping.seq,
+                               message.sysid,
+                               message.compid);
+    sendMessageOnLink(link, msg);
+}
+
 void Vehicle::_handleHeartbeat(mavlink_message_t& message)
 {
     if (message.compid != _defaultComponentId) {
diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h
index f410e13fb..51ea096fe 100644
--- a/src/Vehicle/Vehicle.h
+++ b/src/Vehicle/Vehicle.h
@@ -1010,6 +1010,7 @@ private:
     void _loadSettings(void);
     void _saveSettings(void);
     void _startJoystick(bool start);
+    void _handlePing(LinkInterface* link, mavlink_message_t& message);
     void _handleHomePosition(mavlink_message_t& message);
     void _handleHeartbeat(mavlink_message_t& message);
     void _handleRadioStatus(mavlink_message_t& message);
-- 
2.22.0