Add compononent information server

src/mavsdk/core/CMakeLists.txt

@@ -23,6 +23,7 @@ target_sources(mavsdk
     mavlink_mission_transfer.cpp
     mavlink_parameters.cpp
     mavlink_receiver.cpp
+    mavlink_request_message_handler.cpp
     mavlink_statustext_handler.cpp
     mavlink_message_handler.cpp
     ping.cpp

src/mavsdk/core/mavlink_request_message_handler.cpp

@@ -0,0 +1,126 @@
+#include "mavlink_request_message_handler.h"
+#include "system_impl.h"
+
+namespace mavsdk {
+
+MavlinkRequestMessageHandler::MavlinkRequestMessageHandler(SystemImpl& system_impl) :
+    _system_impl(system_impl)
+{
+    _system_impl.register_mavlink_command_handler(
+        MAV_CMD_REQUEST_MESSAGE,
+        [this](const MavlinkCommandReceiver::CommandLong& command) {
+            return handle_command_long(command);
+        },
+        this);
+
+    _system_impl.register_mavlink_command_handler(
+        MAV_CMD_REQUEST_MESSAGE,
+        [this](const MavlinkCommandReceiver::CommandLong& command) {
+            return handle_command_long(command);
+        },
+        this);
+}
+
+MavlinkRequestMessageHandler::~MavlinkRequestMessageHandler()
+{
+    _system_impl.unregister_all_mavlink_message_handlers(this);
+}
+
+bool MavlinkRequestMessageHandler::register_handler(
+    uint32_t message_id, const Callback& callback, const void* cookie)
+{
+    std::lock_guard<std::mutex> lock(_table_mutex);
+
+    if (std::find_if(_table.begin(), _table.end(), [message_id](const Entry& entry) {
+            return entry.message_id == message_id;
+        }) != _table.end()) {
+        LogErr() << "message id " << message_id << " already registered, registration ignored";
+        return false;
+    }
+
+    _table.emplace_back(Entry{message_id, callback, cookie});
+    return true;
+}
+
+void MavlinkRequestMessageHandler::unregister_handler(uint32_t message_id, const void* cookie)
+{
+    std::lock_guard<std::mutex> lock(_table_mutex);
+
+    _table.erase(
+        std::remove_if(
+            _table.begin(),
+            _table.end(),
+            [&message_id, &cookie](const Entry& entry) {
+                return entry.message_id == message_id && entry.cookie == cookie;
+            }),
+        _table.end());
+}
+
+void MavlinkRequestMessageHandler::unregister_all_handlers(const void* cookie)
+{
+    std::lock_guard<std::mutex> lock(_table_mutex);
+
+    _table.erase(
+        std::remove_if(
+            _table.begin(),
+            _table.end(),
+            [&cookie](const Entry& entry) { return entry.cookie == cookie; }),
+        _table.end());
+}
+
+std::optional<mavlink_message_t> MavlinkRequestMessageHandler::handle_command_long(
+    const MavlinkCommandReceiver::CommandLong& command)
+{
+    std::lock_guard<std::mutex> lock(_table_mutex);
+
+    for (auto& entry : _table) {
+        if (entry.message_id == static_cast<uint32_t>(std::round(command.params.param1))) {
+            if (entry.callback != nullptr) {
+                const auto result = entry.callback(
+                    {command.params.param2,
+                     command.params.param3,
+                     command.params.param4,
+                     command.params.param5,
+                     command.params.param6});
+
+                if (result.has_value()) {
+                    return _system_impl.make_command_ack_message(command, result.value());
+                }
+            }
+            return {};
+        }
+    }
+
+    // We could respond with MAV_RESULT_UNSUPPORTED here, however, it's not clear if maybe someone
+    // else might be answering the command.
+
+    return {};
+}
+
+std::optional<mavlink_message_t>
+MavlinkRequestMessageHandler::handle_command_int(const MavlinkCommandReceiver::CommandInt& command)
+{
+    std::lock_guard<std::mutex> lock(_table_mutex);
+
+    for (auto& entry : _table) {
+        if (entry.message_id == static_cast<uint32_t>(std::round(command.params.param1))) {
+            if (entry.callback != nullptr) {
+                const auto result = entry.callback(
+                    {command.params.param2,
+                     command.params.param3,
+                     command.params.param4,
+                     static_cast<float>(command.params.x),
+                     static_cast<float>(command.params.y)});
+
+                if (result.has_value()) {
+                    return _system_impl.make_command_ack_message(command, result.value());
+                }
+            }
+            return {};
+        }
+    }
+
+    return {};
+}
+
+} // namespace mavsdk

src/mavsdk/core/mavlink_request_message_handler.h

@@ -0,0 +1,46 @@
+#pragma once
+
+#include <array>
+#include <functional>
+#include <mutex>
+#include <optional>
+#include <vector>
+#include "mavlink_include.h"
+#include "mavlink_command_receiver.h"
+
+namespace mavsdk {
+
+class SystemImpl;
+
+class MavlinkRequestMessageHandler {
+public:
+    MavlinkRequestMessageHandler() = delete;
+    explicit MavlinkRequestMessageHandler(SystemImpl& system_impl);
+    ~MavlinkRequestMessageHandler();
+
+    using Params = std::array<float, 5>;
+    using Callback = std::function<std::optional<MAV_RESULT>(Params)>;
+
+    bool register_handler(uint32_t message_id, const Callback& callback, const void* cookie);
+    void unregister_handler(uint32_t message_id, const void* cookie);
+    void unregister_all_handlers(const void* cookie);
+
+private:
+    std::optional<mavlink_message_t>
+    handle_command_long(const MavlinkCommandReceiver::CommandLong& command);
+    std::optional<mavlink_message_t>
+    handle_command_int(const MavlinkCommandReceiver::CommandInt& command);
+
+    struct Entry {
+        uint32_t message_id;
+        Callback callback;
+        const void* cookie;
+    };
+
+    std::mutex _table_mutex{};
+    std::vector<Entry> _table{};
+
+    SystemImpl& _system_impl;
+};
+
+} // namespace mavsdk

src/mavsdk/core/system_impl.cpp

@@ -22,6 +22,7 @@ SystemImpl::SystemImpl(MavsdkImpl& parent) :
     _params(*this),
     _command_sender(*this),
     _command_receiver(*this),
+    _request_message_handler(*this),
     _timesync(*this),
     _ping(*this),
     _mission_transfer(
@@ -80,14 +81,6 @@ void SystemImpl::init(uint8_t system_id, uint8_t comp_id, bool connected)
         },
         this);
 
-    // TO-DO!
-    register_mavlink_command_handler(
-        MAV_CMD_REQUEST_MESSAGE,
-        [this](const MavlinkCommandReceiver::CommandLong& command) {
-            return make_command_ack_message(command, MAV_RESULT::MAV_RESULT_UNSUPPORTED);
-        },
-        this);
-
     add_new_component(comp_id);
 }
 
@@ -1605,6 +1598,22 @@ void SystemImpl::unregister_all_mavlink_command_handlers(const void* cookie)
     _command_receiver.unregister_all_mavlink_command_handlers(cookie);
 }
 
+bool SystemImpl::register_mavlink_request_message_handler(
+    uint32_t message_id, const MavlinkRequestMessageHandler::Callback& callback, const void* cookie)
+{
+    return _request_message_handler.register_handler(message_id, callback, cookie);
+}
+
+void SystemImpl::unregister_mavlink_request_message_handler(uint32_t message_id, const void* cookie)
+{
+    _request_message_handler.unregister_handler(message_id, cookie);
+}
+
+void SystemImpl::unregister_mavlink_request_message_handler(const void* cookie)
+{
+    _request_message_handler.unregister_all_handlers(cookie);
+}
+
 void SystemImpl::set_server_armed(bool armed)
 {
     uint8_t base_mode = _parent.get_base_mode();

src/mavsdk/core/system_impl.h

@@ -8,6 +8,7 @@
 #include "mavlink_command_sender.h"
 #include "mavlink_message_handler.h"
 #include "mavlink_mission_transfer.h"
+#include "mavlink_request_message_handler.h"
 #include "mavlink_statustext_handler.h"
 #include "request_message.h"
 #include "ardupilot_custom_mode.h"
@@ -276,6 +277,13 @@ class SystemImpl : public Sender {
     void unregister_mavlink_command_handler(uint16_t cmd_id, const void* cookie);
     void unregister_all_mavlink_command_handlers(const void* cookie);
 
+    bool register_mavlink_request_message_handler(
+        uint32_t message_id,
+        const MavlinkRequestMessageHandler::Callback& callback,
+        const void* cookie);
+    void unregister_mavlink_request_message_handler(uint32_t message_id, const void* cookie);
+    void unregister_mavlink_request_message_handler(const void* cookie);
+
     double timeout_s() const;
 
     // Autopilot version data
@@ -386,6 +394,7 @@ class SystemImpl : public Sender {
     MAVLinkParameters _params;
     MavlinkCommandSender _command_sender;
     MavlinkCommandReceiver _command_receiver;
+    MavlinkRequestMessageHandler _request_message_handler;
 
     Timesync _timesync;
     Ping _ping;

src/mavsdk/plugins/CMakeLists.txt

@@ -24,6 +24,7 @@ add_subdirectory(tracking_server)
 add_subdirectory(server_utility)
 add_subdirectory(transponder)
 add_subdirectory(telemetry_server)
+add_subdirectory(component_information_server)
 add_subdirectory(tune)
 
 set(UNIT_TEST_SOURCES ${UNIT_TEST_SOURCES} PARENT_SCOPE)

src/mavsdk/plugins/component_information_server/CMakeLists.txt

@@ -0,0 +1,15 @@
+target_sources(mavsdk
+    PRIVATE
+    component_information_server.cpp
+    component_information_server_impl.cpp
+)
+
+target_include_directories(mavsdk PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include/mavsdk>
+    )
+
+install(FILES
+    include/plugins/component_information_server/component_information_server.h
+    DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/mavsdk/plugins/component_information_server
+)

src/mavsdk/plugins/component_information_server/component_information_server.cpp

@@ -0,0 +1,53 @@
+// WARNING: THIS FILE IS AUTOGENERATED! As such, it should not be edited.
+// Edits need to be made to the proto files
+// (see
+// https://github.com/mavlink/MAVSDK-Proto/blob/master/protos/component_information_server/component_information_server.proto)
+
+#include <iomanip>
+
+#include "component_information_server_impl.h"
+#include "plugins/component_information_server/component_information_server.h"
+
+namespace mavsdk {
+
+ComponentInformationServer::ComponentInformationServer(System& system) :
+    PluginBase(),
+    _impl{std::make_unique<ComponentInformationServerImpl>(system)}
+{}
+
+ComponentInformationServer::ComponentInformationServer(std::shared_ptr<System> system) :
+    PluginBase(),
+    _impl{std::make_unique<ComponentInformationServerImpl>(system)}
+{}
+
+ComponentInformationServer::~ComponentInformationServer() {}
+
+ComponentInformationServer::Result
+ComponentInformationServer::provide_peripheral_file(std::string path) const
+{
+    return _impl->provide_peripheral_file(path);
+}
+
+std::ostream& operator<<(std::ostream& str, ComponentInformationServer::Result const& result)
+{
+    switch (result) {
+        case ComponentInformationServer::Result::Unknown:
+            return str << "Unknown";
+        case ComponentInformationServer::Result::Success:
+            return str << "Success";
+        case ComponentInformationServer::Result::NotFound:
+            return str << "Not Found";
+        case ComponentInformationServer::Result::OpenFailure:
+            return str << "Open Failure";
+        case ComponentInformationServer::Result::ReadFailure:
+            return str << "Read Failure";
+        case ComponentInformationServer::Result::FailedJsonParsing:
+            return str << "Failed Json Parsing";
+        case ComponentInformationServer::Result::FailedJsonSchema:
+            return str << "Failed Json Schema";
+        default:
+            return str << "Unknown";
+    }
+}
+
+} // namespace mavsdk