Handle MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES requests as an autopi…

…lot (#1494)

* Add autopilot_version struct

* add autopilot_version setters and getter

* Remove unused vars

* Add capabilities function & tidy

* Add AutopilotVersion api to system.h

* Set cmd progress to 255

* Use UID2 for autopilot_version, del uid

* Fix style

* Add missing docs

* Resend capabilities and listen for  MAV_CMD_SET_MESSAGE_INTERVAL MAV_CMD_REQUEST_MESSAGE

* Fix bad merge

* Add array header for non-gcc builds

Co-authored-by: Seb Horsewell <seb.horsewell@seebyte.com>

src/core/mavsdk_impl.h

@@ -83,11 +83,6 @@ class MavsdkImpl {
 
     double timeout_s() const { return _timeout_s; };
 
-    void set_base_mode(uint8_t base_mode);
-    void set_custom_mode(uint32_t custom_mode);
-    uint8_t get_base_mode() const;
-    uint32_t get_custom_mode() const;
-
     MAVLinkAddress own_address{};
 
     void set_base_mode(uint8_t base_mode);

src/core/system.cpp

@@ -69,4 +69,49 @@ void System::enable_timesync()
     _system_impl->enable_timesync();
 }
 
+void System::add_capabilities(uint64_t add_capabilities)
+{
+    _system_impl->add_capabilities(add_capabilities);
+}
+
+void System::set_flight_sw_version(uint32_t flight_sw_version)
+{
+    _system_impl->set_flight_sw_version(flight_sw_version);
+}
+
+void System::set_middleware_sw_version(uint32_t middleware_sw_version)
+{
+    _system_impl->set_middleware_sw_version(middleware_sw_version);
+}
+
+void System::set_os_sw_version(uint32_t os_sw_version)
+{
+    _system_impl->set_os_sw_version(os_sw_version);
+}
+
+void System::set_board_version(uint32_t board_version)
+{
+    _system_impl->set_board_version(board_version);
+}
+
+void System::set_vendor_id(uint16_t vendor_id)
+{
+    _system_impl->set_vendor_id(vendor_id);
+}
+
+void System::set_product_id(uint16_t product_id)
+{
+    _system_impl->set_product_id(product_id);
+}
+
+bool System::set_uid2(std::string uid2)
+{
+    return _system_impl->set_uid2(uid2);
+}
+
+System::AutopilotVersion System::get_autopilot_version_data()
+{
+    return _system_impl->get_autopilot_version_data();
+}
+
 } // namespace mavsdk

src/core/system.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <memory>
+#include <array>
 #include <functional>
 
 #include "deprecated.h"
@@ -21,6 +22,31 @@ class PluginImplBase;
  */
 class System {
 public:
+    /**
+     * @brief This struct represents Autopilot version information. This is only used when
+     * MAVSDK is configured as an autopilot
+     *
+     * Other MAVLink systems can use this to identify and match software and capabilities.
+     */
+    struct AutopilotVersion {
+        /** @brief MAVLink autopilot_version capabilities. */
+        uint64_t capabilities{};
+        /** @brief MAVLink autopilot_version flight_sw_version */
+        uint32_t flight_sw_version{0};
+        /** @brief MAVLink autopilot_version middleware_sw_version */
+        uint32_t middleware_sw_version{0};
+        /** @brief MAVLink autopilot_version os_sw_version */
+        uint32_t os_sw_version{0};
+        /** @brief MAVLink autopilot_version board_version */
+        uint32_t board_version{0};
+        /** @brief MAVLink autopilot_version vendor_id */
+        uint16_t vendor_id{0};
+        /** @brief MAVLink autopilot_version product_id */
+        uint16_t product_id{0};
+        /** @brief MAVLink autopilot_version uid2 */
+        std::array<uint8_t, 18> uid2{0};
+    };
+
     /** @private Constructor, used internally
      *
      * This constructor is not (and should not be) directly called by application code.
@@ -136,6 +162,72 @@ class System {
      */
     const System& operator=(const System&) = delete;
 
+    /**
+     * @brief Register capabilities to the system (only used if MAVSDK is autopilot)
+     *
+     * @note Plugins should register additional capabilities they provide using this.
+     *
+     * @param capabilities MAVLink capability flag to bitwise OR
+     */
+    void add_capabilities(uint64_t capabilities);
+
+    /**
+     * @brief Set flight sw version (only used if MAVSDK is autopilot)
+     *
+     * @param flight_sw_version version number of flight control software
+     */
+    void set_flight_sw_version(uint32_t flight_sw_version);
+
+    /**
+     * @brief Set middleware sw version (only used if MAVSDK is autopilot)
+     *
+     * @param middleware_sw_version version number of middleware software
+     */
+    void set_middleware_sw_version(uint32_t middleware_sw_version);
+
+    /**
+     * @brief Set OS sw version (only used if MAVSDK is autopilot)
+     *
+     * @param os_sw_version version number of operating system
+     */
+    void set_os_sw_version(uint32_t os_sw_version);
+
+    /**
+     * @brief Set hardware board version (only used if MAVSDK is autopilot)
+     *
+     * @param board_version version number of hardware board
+     */
+    void set_board_version(uint32_t board_version);
+
+    /**
+     * @brief Set vendor id (only used if MAVSDK is autopilot)
+     *
+     * @param vendor_id number of vendor id
+     */
+    void set_vendor_id(uint16_t vendor_id);
+
+    /**
+     * @brief Set product id (only used if MAVSDK is autopilot)
+     *
+     * @param product_id number of product id
+     */
+    void set_product_id(uint16_t product_id);
+
+    /**
+     * @brief Set uid2, 18 chars max (only used if MAVSDK is autopilot)
+     *
+     * @param uid2 unique hardware id
+     * @return true if valid size, false if too large,
+     */
+    bool set_uid2(std::string uid2);
+
+    /**
+     * @brief Get autopilot version data
+     *
+     * @return AutopilotVersion struct containing autopilot version ids
+     */
+    AutopilotVersion get_autopilot_version_data();
+
 private:
     std::shared_ptr<SystemImpl> system_impl() { return _system_impl; };
 

src/core/system_impl.cpp

@@ -69,6 +69,49 @@ void SystemImpl::init(uint8_t system_id, uint8_t comp_id, bool connected)
         std::bind(&SystemImpl::process_autopilot_version, this, _1),
         this);
 
+    register_mavlink_command_handler(
+        MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES,
+        std::bind(&SystemImpl::process_autopilot_version_request, this, std::placeholders::_1),
+        this);
+
+    // TO-DO!
+    register_mavlink_command_handler(
+        MAV_CMD_SET_MESSAGE_INTERVAL,
+        [this](const MavlinkCommandReceiver::CommandLong& command) {
+            mavlink_message_t msg;
+            mavlink_msg_command_ack_pack(
+                _parent.get_own_system_id(),
+                _parent.get_own_component_id(),
+                &msg,
+                command.command,
+                MAV_RESULT::MAV_RESULT_UNSUPPORTED,
+                100,
+                0,
+                command.origin_system_id,
+                command.origin_component_id);
+            return msg;
+        },
+        this);
+
+    // TO-DO!
+    register_mavlink_command_handler(
+        MAV_CMD_REQUEST_MESSAGE,
+        [this](const MavlinkCommandReceiver::CommandLong& command) {
+            mavlink_message_t msg;
+            mavlink_msg_command_ack_pack(
+                _parent.get_own_system_id(),
+                _parent.get_own_component_id(),
+                &msg,
+                command.command,
+                MAV_RESULT::MAV_RESULT_UNSUPPORTED,
+                100,
+                0,
+                command.origin_system_id,
+                command.origin_component_id);
+            return msg;
+        },
+        this);
+
     add_new_component(comp_id);
 }
 
@@ -260,6 +303,27 @@ void SystemImpl::system_thread()
     }
 }
 
+std::optional<mavlink_message_t>
+SystemImpl::process_autopilot_version_request(const MavlinkCommandReceiver::CommandLong& command)
+{
+    LogDebug() << "Autopilot Capabilities Request";
+
+    send_autopilot_version();
+
+    mavlink_message_t msg{};
+    mavlink_msg_command_ack_pack(
+        _parent.get_own_system_id(),
+        _parent.get_own_component_id(),
+        &msg,
+        command.command,
+        MAV_RESULT::MAV_RESULT_ACCEPTED,
+        255,
+        0,
+        command.origin_system_id,
+        command.origin_component_id);
+    return msg;
+}
+
 std::string SystemImpl::component_name(uint8_t component_id)
 {
     switch (component_id) {
@@ -419,6 +483,32 @@ void SystemImpl::send_autopilot_version_request()
     send_command_async(command, nullptr);
 }
 
+void SystemImpl::send_autopilot_version()
+{
+    std::lock_guard<std::mutex> lock(_autopilot_version_mutex);
+    const uint8_t custom_values[8] = {0}; // TO-DO: maybe?
+
+    mavlink_message_t msg;
+    mavlink_msg_autopilot_version_pack(
+        _parent.get_own_system_id(),
+        _parent.get_own_component_id(),
+        &msg,
+        _autopilot_version.capabilities,
+        _autopilot_version.flight_sw_version,
+        _autopilot_version.middleware_sw_version,
+        _autopilot_version.os_sw_version,
+        _autopilot_version.board_version,
+        custom_values,
+        custom_values,
+        custom_values,
+        _autopilot_version.vendor_id,
+        _autopilot_version.product_id,
+        0,
+        _autopilot_version.uid2.data());
+
+    _parent.send_message(msg);
+}
+
 void SystemImpl::send_flight_information_request()
 {
     // We don't care about an answer, we mostly care about receiving FLIGHT_INFORMATION.
@@ -1008,6 +1098,69 @@ void SystemImpl::receive_int_param(
     }
 }
 
+void SystemImpl::add_capabilities(uint64_t add_capabilities)
+{
+    std::unique_lock<std::mutex> lock(_autopilot_version_mutex);
+    _autopilot_version.capabilities |= add_capabilities;
+
+    // We need to resend capabilities...
+    lock.unlock();
+    send_autopilot_version();
+}
+
+void SystemImpl::set_flight_sw_version(uint32_t flight_sw_version)
+{
+    std::lock_guard<std::mutex> lock(_autopilot_version_mutex);
+    _autopilot_version.flight_sw_version = flight_sw_version;
+}
+
+void SystemImpl::set_middleware_sw_version(uint32_t middleware_sw_version)
+{
+    std::lock_guard<std::mutex> lock(_autopilot_version_mutex);
+    _autopilot_version.middleware_sw_version = middleware_sw_version;
+}
+
+void SystemImpl::set_os_sw_version(uint32_t os_sw_version)
+{
+    std::lock_guard<std::mutex> lock(_autopilot_version_mutex);
+    _autopilot_version.os_sw_version = os_sw_version;
+}
+
+void SystemImpl::set_board_version(uint32_t board_version)
+{
+    std::lock_guard<std::mutex> lock(_autopilot_version_mutex);
+    _autopilot_version.board_version = board_version;
+}
+
+void SystemImpl::set_vendor_id(uint16_t vendor_id)
+{
+    std::lock_guard<std::mutex> lock(_autopilot_version_mutex);
+    _autopilot_version.vendor_id = vendor_id;
+}
+
+void SystemImpl::set_product_id(uint16_t product_id)
+{
+    std::lock_guard<std::mutex> lock(_autopilot_version_mutex);
+    _autopilot_version.product_id = product_id;
+}
+
+bool SystemImpl::set_uid2(std::string uid2)
+{
+    std::lock_guard<std::mutex> lock(_autopilot_version_mutex);
+    if (uid2.size() > _autopilot_version.uid2.size()) {
+        return false;
+    }
+    _autopilot_version.uid2 = {0};
+    std::copy(uid2.begin(), uid2.end(), _autopilot_version.uid2.data());
+    return true;
+}
+
+System::AutopilotVersion SystemImpl::get_autopilot_version_data()
+{
+    std::lock_guard<std::mutex> lock(_autopilot_version_mutex);
+    return _autopilot_version;
+}
+
 uint8_t SystemImpl::get_autopilot_id() const
 {
     for (auto compid : _components)