Merge pull request #2449 from jonathanreeves/main

Respond to MAVLINK 2 PROTOCOL_VERSION request
mavlink · Nov 25, 2024 · 5a124d9 · 5a124d9
2 parents c963c51 + 1609d29
commit 5a124d9
Show file tree

Hide file tree

Showing 2 changed files with 37 additions and 0 deletions.
diff --git a/src/mavsdk/core/server_component_impl.cpp b/src/mavsdk/core/server_component_impl.cpp
@@ -18,6 +18,8 @@ ServerComponentImpl::ServerComponentImpl(MavsdkImpl& mavsdk_impl, uint8_t compon
     _mavlink_request_message_handler(mavsdk_impl, *this, _mavlink_command_receiver),
     _mavlink_ftp_server(*this)
 {
+    _autopilot_version.capabilities |= MAV_PROTOCOL_CAPABILITY_MAVLINK2;
+
     if (!MavlinkChannels::Instance().checkout_free_channel(_channel)) {
         // We use a default of channel 0 which will still work but not track
         // seq correctly.
@@ -41,6 +43,14 @@ ServerComponentImpl::ServerComponentImpl(MavsdkImpl& mavsdk_impl, uint8_t compon
         },
         this);
 
+    _mavlink_request_message_handler.register_handler(
+        MAVLINK_MSG_ID_PROTOCOL_VERSION,
+        [this](uint8_t, uint8_t, const MavlinkRequestMessageHandler::Params&) {
+            send_protocol_version();
+            return MAV_RESULT_ACCEPTED;
+        },
+        this);
+
     _mavlink_request_message_handler.register_handler(
         MAVLINK_MSG_ID_AUTOPILOT_VERSION,
         [this](uint8_t, uint8_t, const MavlinkRequestMessageHandler::Params&) {
@@ -377,6 +387,24 @@ void ServerComponentImpl::send_autopilot_version()
     });
 }
 
+void ServerComponentImpl::send_protocol_version()
+{
+    queue_message([&, this](MavlinkAddress mavlink_address, uint8_t channel) {
+        mavlink_message_t message;
+        mavlink_msg_protocol_version_pack_chan(
+            mavlink_address.system_id,
+            mavlink_address.component_id,
+            channel,
+            &message,
+            MAVLINK_VERSION_INFO.version,
+            MAVLINK_VERSION_INFO.min_version,
+            MAVLINK_VERSION_INFO.max_version,
+            MAVLINK_VERSION_INFO.spec_version_hash,
+            MAVLINK_VERSION_INFO.library_version_hash);
+        return message;
+    });
+}
+
 ServerComponentImpl::OurSender::OurSender(ServerComponentImpl& server_component_impl) :
     _server_component_impl(server_component_impl)
 {}

diff --git a/src/mavsdk/core/server_component_impl.h b/src/mavsdk/core/server_component_impl.h
@@ -25,6 +25,14 @@ class ServerPluginImplBase;
 
 class ServerComponentImpl {
 public:
+    static constexpr mavlink_protocol_version_t MAVLINK_VERSION_INFO{
+        MAVLINK_VERSION * 100, // currently active mavlink version
+        100, // min supported version
+        MAVLINK_VERSION * 100, // max supported version
+        {}, // spec version hash (unused for now)
+        {}, // library version hash (unused for now)
+    };
+
     ServerComponentImpl(MavsdkImpl& mavsdk_impl, uint8_t component_id);
     ~ServerComponentImpl();
 
@@ -140,6 +148,7 @@ class ServerComponentImpl {
     void set_product_id(uint16_t product_id);
     bool set_uid2(std::string uid2);
     void send_autopilot_version();
+    void send_protocol_version();
 
     MavlinkMissionTransferServer& mission_transfer_server() { return _mission_transfer_server; }
     MavlinkParameterServer& mavlink_parameter_server() { return _mavlink_parameter_server; }