Merge pull request #2407 from mavlink/pr-v2.12-system-debugging

[v2.12 BACKPORT] Add some system debugging
mavlink · Sep 26, 2024 · 0a825ac · 0a825ac
2 parents 27a1077 + 74f1f73
commit 0a825ac
Show file tree

Hide file tree

Showing 3 changed files with 20 additions and 2 deletions.
diff --git a/src/mavsdk/core/mavlink_command_receiver.cpp b/src/mavsdk/core/mavlink_command_receiver.cpp
@@ -47,7 +47,7 @@ void MavlinkCommandReceiver::receive_command_int(const mavlink_message_t& messag
         cmd.target_component_id != MAV_COMP_ID_ALL) {
         if (_debugging) {
             LogDebug() << "Ignored command int to component " << (int)cmd.target_component_id
-                       << " instead of " << _server_component_impl.get_own_component_id();
+                       << " instead of " << (int)_server_component_impl.get_own_component_id();
         }
         return;
     }
@@ -96,7 +96,7 @@ void MavlinkCommandReceiver::receive_command_long(const mavlink_message_t& messa
         cmd.target_component_id != MAV_COMP_ID_ALL) {
         if (_debugging) {
             LogDebug() << "Ignored command long to component " << (int)cmd.target_component_id
-                       << " instead of " << _server_component_impl.get_own_component_id();
+                       << " instead of " << (int)_server_component_impl.get_own_component_id();
         }
         return;
     }

diff --git a/src/mavsdk/core/mavsdk_impl.cpp b/src/mavsdk/core/mavsdk_impl.cpp
@@ -39,6 +39,13 @@ MavsdkImpl::MavsdkImpl(const Mavsdk::Configuration& configuration) :
         }
     }
 
+    if (const char* env_p = std::getenv("MAVSDK_SYSTEM_DEBUGGING")) {
+        if (std::string(env_p) == "1") {
+            LogDebug() << "System debugging is on.";
+            _system_debugging = true;
+        }
+    }
+
     set_configuration(configuration);
 
     _work_thread = new std::thread(&MavsdkImpl::work_thread, this);
@@ -389,6 +396,16 @@ void MavsdkImpl::receive_message(mavlink_message_t& message, Connection* connect
     }
 
     if (!found_system) {
+        if (_system_debugging) {
+            LogWarn() << "Create new system/component " << (int)message.sysid << "/"
+                      << (int)message.compid;
+            LogWarn() << "From message " << (int)message.msgid << " with len " << (int)message.len;
+            std::string bytes = "";
+            for (unsigned i = 0; i < 12 + message.len; ++i) {
+                bytes += std::to_string(reinterpret_cast<uint8_t*>(&message)[i]) + ' ';
+            }
+            LogWarn() << "Bytes: " << bytes;
+        }
         make_system_with_component(message.sysid, message.compid);
     }
 

diff --git a/src/mavsdk/core/mavsdk_impl.h b/src/mavsdk/core/mavsdk_impl.h
@@ -173,6 +173,7 @@ class MavsdkImpl {
 
     bool _message_logging_on{false};
     bool _callback_debugging{false};
+    bool _system_debugging{false};
 
     mutable std::mutex _intercept_callback_mutex{};
     std::function<bool(mavlink_message_t&)> _intercept_incoming_messages_callback{nullptr};