GCS_MAVLink: added logging of NAMED_VALUE_FLOAT

this is useful when running sensors on a companion computer and wanting values logged in main ArduPilot log.
Icatme · Sep 1, 2021 · ae615de · ae615de
1 parent 8d669f7
commit ae615de
Show file tree

Hide file tree

Showing 2 changed files with 27 additions and 0 deletions.
diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h
@@ -469,6 +469,7 @@ class GCS_MAVLINK
     }  _timesync_request;
 
     void handle_statustext(const mavlink_message_t &msg) const;
+    void handle_named_value(const mavlink_message_t &msg) const;
 
     bool telemetry_delayed() const;
     virtual uint32_t telem_delay() const = 0;

diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -2950,6 +2950,28 @@ void GCS_MAVLINK::handle_statustext(const mavlink_message_t &msg) const
 }
 
 
+/*
+  handle logging of named values from mavlink.
+ */
+void GCS_MAVLINK::handle_named_value(const mavlink_message_t &msg) const
+{
+    auto *logger = AP_Logger::get_singleton();
+    if (logger == nullptr) {
+        return;
+    }
+    mavlink_named_value_float_t p;
+    mavlink_msg_named_value_float_decode(&msg, &p);
+    char s[11] {};
+    strncpy(s, p.name, sizeof(s)-1);
+    logger->Write("NVAL", "TimeUS,TimeBootMS,Name,Value,SSys,SCom", "ss#---", "FC----", "QINfBB",
+                  AP_HAL::micros64(),
+                  p.time_boot_ms,
+                  s,
+                  p.value,
+                  msg.sysid,
+                  msg.compid);
+}
+
 void GCS_MAVLINK::handle_system_time_message(const mavlink_message_t &msg)
 {
     mavlink_system_time_t packet;
@@ -3522,6 +3544,10 @@ void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg)
     case MAVLINK_MSG_ID_LANDING_TARGET:
         handle_landing_target(msg);
         break;
+
+    case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT:
+        handle_named_value(msg);
+        break;
     }
 
 }