Merge pull request #6396 from DonLakeFlyer/Ping

Support PING response

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) {

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);