core: add mavlink statustexts as debug printfs

By printing all mavlink statustexts we should get a bit more context when something goes wrong on the firmware side.
mavlink · Oct 23, 2017 · 8df5d52 · 8df5d52
1 parent bf80f02
commit 8df5d52
Show file tree

Hide file tree

Showing 2 changed files with 49 additions and 0 deletions.
diff --git a/core/device_impl.cpp b/core/device_impl.cpp
@@ -24,6 +24,10 @@ DeviceImpl::DeviceImpl(DroneCoreImpl *parent,
     register_mavlink_message_handler(
         MAVLINK_MSG_ID_AUTOPILOT_VERSION,
         std::bind(&DeviceImpl::process_autopilot_version, this, _1), this);
+
+    register_mavlink_message_handler(
+        MAVLINK_MSG_ID_STATUSTEXT,
+        std::bind(&DeviceImpl::process_statustext, this, _1), this);
 }
 
 DeviceImpl::~DeviceImpl()
@@ -155,6 +159,50 @@ void DeviceImpl::process_autopilot_version(const mavlink_message_t &message)
     unregister_timeout_handler(_autopilot_version_timed_out_cookie);
 }
 
+void DeviceImpl::process_statustext(const mavlink_message_t &message)
+{
+    mavlink_statustext_t statustext;
+    mavlink_msg_statustext_decode(&message, &statustext);
+
+    std::string debug_str = "mavlink ";
+
+    switch (statustext.severity) {
+        case MAV_SEVERITY_EMERGENCY:
+            debug_str += "emergency";
+            break;
+        case MAV_SEVERITY_ALERT:
+            debug_str += "alert";
+            break;
+        case MAV_SEVERITY_CRITICAL:
+            debug_str += "critical";
+            break;
+        case MAV_SEVERITY_ERROR:
+            debug_str += "error";
+            break;
+        case MAV_SEVERITY_WARNING:
+            debug_str += "warning";
+            break;
+        case MAV_SEVERITY_NOTICE:
+            debug_str += "notice";
+            break;
+        case MAV_SEVERITY_INFO:
+            debug_str += "info";
+            break;
+        case MAV_SEVERITY_DEBUG:
+            debug_str += "debug";
+            break;
+        default:
+            break;
+    }
+
+    // statustext.text is not null terminated, therefore we copy it first to
+    // an array big enough that is zeroed.
+    char text_with_null[sizeof(statustext.text) + 1] {};
+    memcpy(text_with_null, statustext.text, sizeof(statustext.text));
+
+    LogDebug() << debug_str << ": " << text_with_null;
+}
+
 void DeviceImpl::heartbeats_timed_out()
 {
     LogInfo() << "heartbeats timed out";

diff --git a/core/device_impl.h b/core/device_impl.h
@@ -97,6 +97,7 @@ class DeviceImpl
 
     void process_heartbeat(const mavlink_message_t &message);
     void process_autopilot_version(const mavlink_message_t &message);
+    void process_statustext(const mavlink_message_t &message);
     void heartbeats_timed_out();
     void set_connected();
     void set_disconnected();