From 17d86bfc0e263f80a853b48142668c93d72a9889 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 3 Jun 2017 11:08:59 +0200 Subject: [PATCH 1/4] Update MAVLink 2.0 headers to latest --- mavlink/include/mavlink/v2.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v2.0 b/mavlink/include/mavlink/v2.0 index 2faa6d49834a..b69eccedfcbe 160000 --- a/mavlink/include/mavlink/v2.0 +++ b/mavlink/include/mavlink/v2.0 @@ -1 +1 @@ -Subproject commit 2faa6d49834aa203e2a3eeeeed588728fb14c431 +Subproject commit b69eccedfcbec1f64a0340afa0c2294ca3559cfb From c166bb9a55b2e0e87fffa4c3fa1adc910ccf3881 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 3 Jun 2017 11:44:27 +0200 Subject: [PATCH 2/4] Tools: Add MAVLink libary git hash to exported symbols --- Tools/px_update_git_header.py | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/Tools/px_update_git_header.py b/Tools/px_update_git_header.py index d07062847746..366a69020afb 100755 --- a/Tools/px_update_git_header.py +++ b/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])) From 2759f474ef67a59362bb63e386ab1db6f28857c9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 3 Jun 2017 11:44:45 +0200 Subject: [PATCH 3/4] Version: Add MAVLink git hash to available versions --- src/lib/version/version.c | 5 +++++ src/lib/version/version.h | 5 +++++ 2 files changed, 10 insertions(+) diff --git a/src/lib/version/version.c b/src/lib/version/version.c index f3ad70bb9d49..836cca3ebf8d 100644 --- a/src/lib/version/version.c +++ b/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 diff --git a/src/lib/version/version.h b/src/lib/version/version.h index 81bc12d52f23..8a28f336e96f 100644 --- a/src/lib/version/version.h +++ b/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 From 72dfef823f13b9f26e47601c6c188387335730fc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 3 Jun 2017 11:46:29 +0200 Subject: [PATCH 4/4] 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 ac9456d06405..52601644b6e4 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/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 */ diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 5310bb601be4..92007c145c77 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);