From 840b425c820c5d3e7157cdb44ff8d6ef64c77075 Mon Sep 17 00:00:00 2001
From: Don Gagne <don@thegagnes.com>
Date: Tue, 24 May 2016 08:12:09 -0700
Subject: [PATCH] Add MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES support

---
 src/comm/MockLink.cc | 29 +++++++++++++++++++++++++++++
 src/comm/MockLink.h  |  1 +
 2 files changed, 30 insertions(+)

diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc
index a0aa47def..ad45267cc 100644
--- a/src/comm/MockLink.cc
+++ b/src/comm/MockLink.cc
@@ -803,6 +803,10 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg)
     case MAV_CMD_PREFLIGHT_STORAGE:
         commandResult = MAV_RESULT_ACCEPTED;
         break;
+    case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES:
+        commandResult = MAV_RESULT_ACCEPTED;
+        _respondWithAutopilotVersion();
+        break;
     }
 
     mavlink_message_t commandAck;
@@ -814,6 +818,31 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg)
     respondWithMavlinkMessage(commandAck);
 }
 
+void MockLink::_respondWithAutopilotVersion(void)
+{
+    mavlink_message_t msg;
+
+    uint8_t customVersion[8];
+    memset(customVersion, 0, sizeof(customVersion));
+
+    // Only flight_sw_version is supported a this point
+    mavlink_msg_autopilot_version_pack(_vehicleSystemId,
+                                       _vehicleComponentId,
+                                       &msg,
+                                       0,                                           // capabilities,
+                                       (1 << (8*3)) | FIRMWARE_VERSION_TYPE_DEV,    // flight_sw_version,
+                                       0,                                           // middleware_sw_version,
+                                       0,                                           // os_sw_version,
+                                       0,                                           // board_version,
+                                       (uint8_t *)&customVersion,                   // flight_custom_version,
+                                       (uint8_t *)&customVersion,                   // middleware_custom_version,
+                                       (uint8_t *)&customVersion,                   // os_custom_version,
+                                       0,                                           // vendor_id,
+                                       0,                                           // product_id,
+                                       0);                                          // uid
+    respondWithMavlinkMessage(msg);
+}
+
 void MockLink::setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode_t failureMode)
 {
     _missionItemHandler.setMissionItemFailureMode(failureMode);
diff --git a/src/comm/MockLink.h b/src/comm/MockLink.h
index 395907e8e..917dece5c 100644
--- a/src/comm/MockLink.h
+++ b/src/comm/MockLink.h
@@ -185,6 +185,7 @@ private:
     void _sendGpsRawInt(void);
     void _sendVibration(void);
     void _sendStatusTextMessages(void);
+    void _respondWithAutopilotVersion(void);
 
     static MockLink* _startMockLink(MockConfiguration* mockConfig);
 
-- 
2.22.0