MAVLink app: Enable protocol version handshaking

Tools/px_update_git_header.py

@@ -17,12 +17,16 @@
                                   stderr=subprocess.STDOUT).decode('utf-8').strip()
 git_version = subprocess.check_output('git rev-parse --verify HEAD'.split(),
                                       stderr=subprocess.STDOUT).decode('utf-8').strip()
+git_version_short = git_version[0:16]
 nuttx_git_tag = subprocess.check_output('git describe --always --tags'.split(),
                                   cwd='NuttX/nuttx', stderr=subprocess.STDOUT).decode('utf-8').strip().replace("nuttx-","v")
 nuttx_git_tag = re.sub('-.*','.0',nuttx_git_tag)
 nuttx_git_version = subprocess.check_output('git rev-parse --verify HEAD'.split(),
                                       cwd='NuttX/nuttx', stderr=subprocess.STDOUT).decode('utf-8').strip()
-git_version_short = git_version[0:16]
+nuttx_git_version_short = nuttx_git_version[0:16]
+mavlink_git_version = subprocess.check_output('git rev-parse --verify HEAD'.split(),
+                                      cwd='mavlink/include/mavlink/v2.0', stderr=subprocess.STDOUT).decode('utf-8').strip()
+mavlink_git_version_short = mavlink_git_version[0:16]
 
 # Generate the header file content
 header = """
@@ -33,12 +37,18 @@
 #define PX4_GIT_VERSION_BINARY 0x{git_version_short}
 #define PX4_GIT_TAG_STR  "{git_tag}"
 #define NUTTX_GIT_VERSION_STR  "{nuttx_git_version}"
+#define NUTTX_GIT_VERSION_BINARY 0x{nuttx_git_version_short}
 #define NUTTX_GIT_TAG_STR  "{nuttx_git_tag}"
+#define MAVLINK_LIB_GIT_VERSION_STR  "{mavlink_git_version}"
+#define MAVLINK_LIB_GIT_VERSION_BINARY 0x{mavlink_git_version_short}
 """.format(git_tag=git_tag,
            git_version=git_version,
            git_version_short=git_version_short,
            nuttx_git_version=nuttx_git_version,
-           nuttx_git_tag=nuttx_git_tag)
+           nuttx_git_version_short=nuttx_git_version_short,
+           nuttx_git_tag=nuttx_git_tag,
+           mavlink_git_version=mavlink_git_version,
+           mavlink_git_version_short=mavlink_git_version_short)
 
 if old_header != header:
     print('Updating header {}'.format(sys.argv[1]))

src/lib/version/version.c

@@ -254,6 +254,11 @@ uint64_t px4_firmware_version_binary(void)
 	return PX4_GIT_VERSION_BINARY;
 }
 
+uint64_t px4_mavlink_lib_version_binary(void)
+{
+	return MAVLINK_LIB_GIT_VERSION_BINARY;
+}
+
 uint64_t px4_os_version_binary(void)
 {
 	//TODO: get NuttX version via git tag

src/lib/version/version.h

@@ -123,6 +123,11 @@ __EXPORT const char *px4_firmware_version_string(void);
  */
 __EXPORT uint64_t px4_firmware_version_binary(void);
 
+/**
+ * MAVLink lib version in binary form (first part of the git tag)
+ */
+__EXPORT uint64_t px4_mavlink_lib_version_binary(void);
+
 /**
  * Operating system version in binary form (first part of the git tag)
  * @return this is not available on all OSes and can return 0

src/modules/mavlink/mavlink_main.cpp

@@ -1303,6 +1303,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 */

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

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