From 8c3b90007cbfa1717c0f18e068a4d7193148733b Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lorenz@px4.io>
Date: Sat, 3 Jun 2017 11:46:29 +0200
Subject: [PATCH] MAVLink app: Enable protocol version handshaking

This allows the ground control station or any other communication partner to query the supported versions. The key aspect is to send the response in MAVLink 2 framing to test the link with a MAVLink 2 framed message.
---
 src/modules/mavlink/mavlink_main.cpp     | 21 +++++++++++++++++++++
 src/modules/mavlink/mavlink_main.h       |  9 +++++++++
 src/modules/mavlink/mavlink_receiver.cpp |  7 +++++++
 3 files changed, 37 insertions(+)

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 17eff600e65e..aa61edf11cbe 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -1302,6 +1302,27 @@ void Mavlink::send_autopilot_capabilites()
 	}
 }
 
+void Mavlink::send_protocol_version()
+{
+	mavlink_protocol_version_t msg = {};
+
+	msg.version = _protocol_version * 100;
+	msg.min_version = 100;
+	msg.max_version = 200;
+	uint64_t mavlink_lib_git_version_binary = px4_mavlink_lib_version_binary();
+	// TODO add when available
+	//memcpy(&msg.spec_version_hash, &mavlink_spec_git_version_binary, sizeof(msg.spec_version_hash));
+	memcpy(&msg.library_version_hash, &mavlink_lib_git_version_binary, sizeof(msg.library_version_hash));
+
+	// Switch to MAVLink 2
+	int curr_proto_ver = _protocol_version;
+	set_proto_version(2);
+	// Send response - if it passes through the link its fine to use MAVLink 2
+	mavlink_msg_protocol_version_send_struct(get_channel(), &msg);
+	// Reset to previous value
+	set_proto_version(curr_proto_ver);
+}
+
 MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic, int instance)
 {
 	/* check if already subscribed to this topic */
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 1f791e1e28ee..5498dea466ae 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -344,8 +344,17 @@ class Mavlink
 	 * @param severity the log level
 	 */
 	void			send_statustext(unsigned char severity, const char *string);
+
+	/**
+	 * Send the capabilities of this autopilot in terms of the MAVLink spec
+	 */
 	void 			send_autopilot_capabilites();
 
+	/**
+	 * Send the protocol version of MAVLink
+	 */
+	void			send_protocol_version();
+
 	MavlinkStream 		*get_streams() const { return _streams; }
 
 	float			get_rate_mult();
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index d2012f01189c..32b27a4dda75 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -362,6 +362,9 @@ MavlinkReceiver::evaluate_target_ok(int command, int target_system, int target_c
 	switch (command) {
 
 	case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES:
+
+	/* fallthrough */
+	case MAV_CMD_REQUEST_PROTOCOL_VERSION:
 		/* broadcast and ignore component */
 		target_ok = (target_system == 0) || (target_system == mavlink_system.sysid);
 		break;
@@ -406,6 +409,10 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
 		/* send autopilot version message */
 		_mavlink->send_autopilot_capabilites();
 
+	} else if (cmd_mavlink.command == MAV_CMD_REQUEST_PROTOCOL_VERSION) {
+		/* send protocol version message */
+		_mavlink->send_protocol_version();
+
 	} else if (cmd_mavlink.command == MAV_CMD_GET_HOME_POSITION) {
 		_mavlink->configure_stream_threadsafe("HOME_POSITION", 0.5f);