Merge pull request #2192 from mavlink/pr-fix-sysid-filter

core: fix incoming messages by sysid

src/mavsdk/core/mavlink_message_handler.cpp

@@ -1,8 +1,19 @@
 #include <mutex>
 #include "mavlink_message_handler.h"
+#include "log.h"
 
 namespace mavsdk {
 
+MavlinkMessageHandler::MavlinkMessageHandler()
+{
+    if (const char* env_p = std::getenv("MAVSDK_MESSAGE_HANDLER_DEBUGGING")) {
+        if (std::string(env_p) == "1") {
+            LogDebug() << "Mavlink message handler debugging is on.";
+            _debugging = true;
+        }
+    }
+}
+
 void MavlinkMessageHandler::register_one(
     uint16_t msg_id, const Callback& callback, const void* cookie)
 {
@@ -12,11 +23,8 @@ void MavlinkMessageHandler::register_one(
     _table.push_back(entry);
 }
 
-void MavlinkMessageHandler::register_one(
-    uint16_t msg_id,
-    std::optional<uint8_t> component_id,
-    const Callback& callback,
-    const void* cookie)
+void MavlinkMessageHandler::register_one_with_component_id(
+    uint16_t msg_id, uint8_t component_id, const Callback& callback, const void* cookie)
 {
     std::lock_guard<std::mutex> lock(_mutex);
 
@@ -56,26 +64,34 @@ void MavlinkMessageHandler::process_message(const mavlink_message_t& message)
 {
     std::lock_guard<std::mutex> lock(_mutex);
 
-#if MESSAGE_DEBUGGING == 1
     bool forwarded = false;
-#endif
+
+    if (_debugging) {
+        LogDebug() << "Table entries: ";
+    }
+
     for (auto& entry : _table) {
+        if (_debugging) {
+            LogDebug() << "Msg id: " << entry.msg_id << ", component id: "
+                       << (entry.component_id.has_value() ?
+                               std::to_string(entry.component_id.value()) :
+                               "none");
+        }
+
         if (entry.msg_id == message.msgid &&
-            (!entry.cmp_id.has_value() || entry.cmp_id == message.compid)) {
-#if MESSAGE_DEBUGGING == 1
-            LogDebug() << "Forwarding msg " << int(message.msgid) << " to "
-                       << size_t(entry->cookie);
+            (!entry.component_id.has_value() || entry.component_id.value() == message.compid)) {
+            if (_debugging) {
+                LogDebug() << "Using msg " << int(message.msgid) << " to " << size_t(entry.cookie);
+            }
+
             forwarded = true;
-#endif
             entry.callback(message);
         }
     }
 
-#if MESSAGE_DEBUGGING == 1
-    if (!forwarded) {
+    if (_debugging && !forwarded) {
         LogDebug() << "Ignoring msg " << int(message.msgid);
     }
-#endif
 }
 
 void MavlinkMessageHandler::update_component_id(
@@ -85,7 +101,7 @@ void MavlinkMessageHandler::update_component_id(
 
     for (auto& entry : _table) {
         if (entry.msg_id == msg_id && entry.cookie == cookie) {
-            entry.cmp_id = component_id;
+            entry.component_id = component_id;
         }
     }
 }

src/mavsdk/core/mavlink_message_handler.h

@@ -11,21 +11,20 @@ namespace mavsdk {
 
 class MavlinkMessageHandler {
 public:
+    MavlinkMessageHandler();
+
     using Callback = std::function<void(const mavlink_message_t&)>;
 
     struct Entry {
         uint32_t msg_id;
-        std::optional<uint8_t> cmp_id;
+        std::optional<uint8_t> component_id;
         Callback callback;
         const void* cookie; // This is the identification to unregister.
     };
 
     void register_one(uint16_t msg_id, const Callback& callback, const void* cookie);
-    void register_one(
-        uint16_t msg_id,
-        std::optional<uint8_t> cmp_id,
-        const Callback& callback,
-        const void* cookie);
+    void register_one_with_component_id(
+        uint16_t msg_id, uint8_t component_id, const Callback& callback, const void* cookie);
     void unregister_one(uint16_t msg_id, const void* cookie);
     void unregister_all(const void* cookie);
     void process_message(const mavlink_message_t& message);
@@ -34,6 +33,8 @@ class MavlinkMessageHandler {
 private:
     std::mutex _mutex{};
     std::vector<Entry> _table{};
+
+    bool _debugging{false};
 };
 
 } // namespace mavsdk

src/mavsdk/core/mavsdk_impl.cpp

@@ -391,10 +391,11 @@ void MavsdkImpl::receive_message(mavlink_message_t& message, Connection* connect
         return;
     }
 
+    mavlink_message_handler.process_message(message);
+
     for (auto& system : _systems) {
         if (system.first == message.sysid) {
-            // system.second->system_impl()->process_mavlink_message(message);
-            mavlink_message_handler.process_message(message);
+            system.second->system_impl()->process_mavlink_message(message);
             break;
         }
     }

src/mavsdk/core/server_component_impl.cpp

@@ -102,12 +102,6 @@ void ServerComponentImpl::register_mavlink_message_handler(
     _mavsdk_impl.mavlink_message_handler.register_one(msg_id, callback, cookie);
 }
 
-void ServerComponentImpl::register_mavlink_message_handler(
-    uint16_t msg_id, uint8_t cmp_id, const MavlinkMessageHandler& callback, const void* cookie)
-{
-    _mavsdk_impl.mavlink_message_handler.register_one(msg_id, cmp_id, callback, cookie);
-}
-
 void ServerComponentImpl::unregister_mavlink_message_handler(uint16_t msg_id, const void* cookie)
 {
     _mavsdk_impl.mavlink_message_handler.unregister_one(msg_id, cookie);

src/mavsdk/core/server_component_impl.h

@@ -85,8 +85,6 @@ class ServerComponentImpl {
 
     void register_mavlink_message_handler(
         uint16_t msg_id, const MavlinkMessageHandler& callback, const void* cookie);
-    void register_mavlink_message_handler(
-        uint16_t msg_id, uint8_t cmp_id, const MavlinkMessageHandler& callback, const void* cookie);
 
     void unregister_mavlink_message_handler(uint16_t msg_id, const void* cookie);
     void unregister_all_mavlink_message_handlers(const void* cookie);

src/mavsdk/core/system_impl.cpp

@@ -28,11 +28,11 @@ SystemImpl::SystemImpl(MavsdkImpl& mavsdk_impl) :
     _ping(*this),
     _mission_transfer_client(
         _mavsdk_impl.default_server_component_impl().sender(),
-        _mavsdk_impl.mavlink_message_handler,
+        _mavlink_message_handler,
         _mavsdk_impl.timeout_handler,
         [this]() { return timeout_s(); }),
     _request_message(
-        *this, _command_sender, _mavsdk_impl.mavlink_message_handler, _mavsdk_impl.timeout_handler),
+        *this, _command_sender, _mavlink_message_handler, _mavsdk_impl.timeout_handler),
     _mavlink_ftp_client(*this)
 {
     _system_thread = new std::thread(&SystemImpl::system_thread, this);
@@ -41,7 +41,7 @@ SystemImpl::SystemImpl(MavsdkImpl& mavsdk_impl) :
 SystemImpl::~SystemImpl()
 {
     _should_exit = true;
-    _mavsdk_impl.mavlink_message_handler.unregister_all(this);
+    _mavlink_message_handler.unregister_all(this);
 
     unregister_timeout_handler(_heartbeat_timeout_cookie);
 
@@ -58,17 +58,17 @@ void SystemImpl::init(uint8_t system_id, uint8_t comp_id)
     // We use this as a default.
     _target_address.component_id = MAV_COMP_ID_AUTOPILOT1;
 
-    _mavsdk_impl.mavlink_message_handler.register_one(
+    _mavlink_message_handler.register_one(
         MAVLINK_MSG_ID_HEARTBEAT,
         [this](const mavlink_message_t& message) { process_heartbeat(message); },
         this);
 
-    _mavsdk_impl.mavlink_message_handler.register_one(
+    _mavlink_message_handler.register_one(
         MAVLINK_MSG_ID_STATUSTEXT,
         [this](const mavlink_message_t& message) { process_statustext(message); },
         this);
 
-    _mavsdk_impl.mavlink_message_handler.register_one(
+    _mavlink_message_handler.register_one(
         MAVLINK_MSG_ID_AUTOPILOT_VERSION,
         [this](const mavlink_message_t& message) { process_autopilot_version(message); },
         this);
@@ -97,31 +97,34 @@ bool SystemImpl::is_connected() const
 }
 
 void SystemImpl::register_mavlink_message_handler(
-    uint16_t msg_id, const MavlinkMessageHandler& callback, const void* cookie)
+    uint16_t msg_id, const MavlinkMessageHandler::Callback& callback, const void* cookie)
 {
-    _mavsdk_impl.mavlink_message_handler.register_one(msg_id, callback, cookie);
+    _mavlink_message_handler.register_one(msg_id, callback, cookie);
 }
 
-void SystemImpl::register_mavlink_message_handler(
-    uint16_t msg_id, uint8_t cmp_id, const MavlinkMessageHandler& callback, const void* cookie)
+void SystemImpl::register_mavlink_message_handler_with_compid(
+    uint16_t msg_id,
+    uint8_t component_id,
+    const MavlinkMessageHandler::Callback& callback,
+    const void* cookie)
 {
-    _mavsdk_impl.mavlink_message_handler.register_one(msg_id, cmp_id, callback, cookie);
+    _mavlink_message_handler.register_one_with_component_id(msg_id, component_id, callback, cookie);
 }
 
 void SystemImpl::unregister_mavlink_message_handler(uint16_t msg_id, const void* cookie)
 {
-    _mavsdk_impl.mavlink_message_handler.unregister_one(msg_id, cookie);
+    _mavlink_message_handler.unregister_one(msg_id, cookie);
 }
 
 void SystemImpl::unregister_all_mavlink_message_handlers(const void* cookie)
 {
-    _mavsdk_impl.mavlink_message_handler.unregister_all(cookie);
+    _mavlink_message_handler.unregister_all(cookie);
 }
 
-void SystemImpl::update_componentid_messages_handler(
-    uint16_t msg_id, uint8_t cmp_id, const void* cookie)
+void SystemImpl::update_component_id_messages_handler(
+    uint16_t msg_id, uint8_t component_id, const void* cookie)
 {
-    _mavsdk_impl.mavlink_message_handler.update_component_id(msg_id, cmp_id, cookie);
+    _mavlink_message_handler.update_component_id(msg_id, component_id, cookie);
 }
 
 void SystemImpl::register_timeout_handler(
@@ -162,6 +165,11 @@ void SystemImpl::unsubscribe_is_connected(System::IsConnectedHandle handle)
     _is_connected_callbacks.unsubscribe(handle);
 }
 
+void SystemImpl::process_mavlink_message(mavlink_message_t& message)
+{
+    _mavlink_message_handler.process_message(message);
+}
+
 void SystemImpl::add_call_every(std::function<void()> callback, float interval_s, void** cookie)
 {
     _mavsdk_impl.call_every_handler.add(
@@ -1337,7 +1345,7 @@ MavlinkParameterClient* SystemImpl::param_sender(uint8_t component_id, bool exte
     _mavlink_parameter_clients.push_back(
         {std::make_unique<MavlinkParameterClient>(
              _mavsdk_impl.default_server_component_impl().sender(),
-             _mavsdk_impl.mavlink_message_handler,
+             _mavlink_message_handler,
              _mavsdk_impl.timeout_handler,
              [this]() { return timeout_s(); },
              get_system_id(),

src/mavsdk/core/system_impl.h

@@ -49,19 +49,21 @@ class SystemImpl {
     System::IsConnectedHandle subscribe_is_connected(const System::IsConnectedCallback& callback);
     void unsubscribe_is_connected(System::IsConnectedHandle handle);
 
-    // void process_mavlink_message(mavlink_message_t& message);
+    void process_mavlink_message(mavlink_message_t& message);
 
-    using MavlinkMessageHandler = std::function<void(const mavlink_message_t&)>;
-
-    void register_mavlink_message_handler(
-        uint16_t msg_id, const MavlinkMessageHandler& callback, const void* cookie);
     void register_mavlink_message_handler(
-        uint16_t msg_id, uint8_t cmp_id, const MavlinkMessageHandler& callback, const void* cookie);
+        uint16_t msg_id, const MavlinkMessageHandler::Callback& callback, const void* cookie);
+    void register_mavlink_message_handler_with_compid(
+        uint16_t msg_id,
+        uint8_t cmp_id,
+        const MavlinkMessageHandler::Callback& callback,
+        const void* cookie);
 
     void unregister_mavlink_message_handler(uint16_t msg_id, const void* cookie);
     void unregister_all_mavlink_message_handlers(const void* cookie);
 
-    void update_componentid_messages_handler(uint16_t msg_id, uint8_t cmp_id, const void* cookie);
+    void
+    update_component_id_messages_handler(uint16_t msg_id, uint8_t component_id, const void* cookie);
 
     void register_timeout_handler(
         const std::function<void()>& callback, double duration_s, void** cookie);
@@ -348,6 +350,8 @@ class SystemImpl {
 
     AutopilotTime _autopilot_time{};
 
+    MavlinkMessageHandler _mavlink_message_handler{};
+
     MavlinkStatustextHandler _statustext_handler{};
 
     MavlinkParameterClient* param_sender(uint8_t component_id, bool extended);

src/mavsdk/plugins/camera/camera_impl.cpp

@@ -37,43 +37,43 @@ CameraImpl::~CameraImpl()
 
 void CameraImpl::init()
 {
-    _system_impl->register_mavlink_message_handler(
+    _system_impl->register_mavlink_message_handler_with_compid(
         MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS,
         _camera_id + MAV_COMP_ID_CAMERA,
         [this](const mavlink_message_t& message) { process_camera_capture_status(message); },
         this);
 
-    _system_impl->register_mavlink_message_handler(
+    _system_impl->register_mavlink_message_handler_with_compid(
         MAVLINK_MSG_ID_STORAGE_INFORMATION,
         _camera_id + MAV_COMP_ID_CAMERA,
         [this](const mavlink_message_t& message) { process_storage_information(message); },
         this);
 
-    _system_impl->register_mavlink_message_handler(
+    _system_impl->register_mavlink_message_handler_with_compid(
         MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED,
         _camera_id + MAV_COMP_ID_CAMERA,
         [this](const mavlink_message_t& message) { process_camera_image_captured(message); },
         this);
 
-    _system_impl->register_mavlink_message_handler(
+    _system_impl->register_mavlink_message_handler_with_compid(
         MAVLINK_MSG_ID_CAMERA_SETTINGS,
         _camera_id + MAV_COMP_ID_CAMERA,
         [this](const mavlink_message_t& message) { process_camera_settings(message); },
         this);
 
-    _system_impl->register_mavlink_message_handler(
+    _system_impl->register_mavlink_message_handler_with_compid(
         MAVLINK_MSG_ID_CAMERA_INFORMATION,
         _camera_id + MAV_COMP_ID_CAMERA,
         [this](const mavlink_message_t& message) { process_camera_information(message); },
         this);
 
-    _system_impl->register_mavlink_message_handler(
+    _system_impl->register_mavlink_message_handler_with_compid(
         MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION,
         _camera_id + MAV_COMP_ID_CAMERA,
         [this](const mavlink_message_t& message) { process_video_information(message); },
         this);
 
-    _system_impl->register_mavlink_message_handler(
+    _system_impl->register_mavlink_message_handler_with_compid(
         MAVLINK_MSG_ID_VIDEO_STREAM_STATUS,
         _camera_id + MAV_COMP_ID_CAMERA,
         [this](const mavlink_message_t& message) { process_video_stream_status(message); },
@@ -246,24 +246,25 @@ void CameraImpl::manual_disable()
 void CameraImpl::update_component()
 {
     uint8_t cmp_id = _camera_id + MAV_COMP_ID_CAMERA;
-    _system_impl->update_componentid_messages_handler(
+    _system_impl->update_component_id_messages_handler(
         MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS, cmp_id, this);
 
-    _system_impl->update_componentid_messages_handler(
+    _system_impl->update_component_id_messages_handler(
         MAVLINK_MSG_ID_STORAGE_INFORMATION, cmp_id, this);
 
-    _system_impl->update_componentid_messages_handler(
+    _system_impl->update_component_id_messages_handler(
         MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED, cmp_id, this);
 
-    _system_impl->update_componentid_messages_handler(MAVLINK_MSG_ID_CAMERA_SETTINGS, cmp_id, this);
+    _system_impl->update_component_id_messages_handler(
+        MAVLINK_MSG_ID_CAMERA_SETTINGS, cmp_id, this);
 
-    _system_impl->update_componentid_messages_handler(
+    _system_impl->update_component_id_messages_handler(
         MAVLINK_MSG_ID_CAMERA_INFORMATION, cmp_id, this);
 
-    _system_impl->update_componentid_messages_handler(
+    _system_impl->update_component_id_messages_handler(
         MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION, cmp_id, this);
 
-    _system_impl->update_componentid_messages_handler(
+    _system_impl->update_component_id_messages_handler(
         MAVLINK_MSG_ID_VIDEO_STREAM_STATUS, cmp_id, this);
 }