From 3452b87e96d3fea32e233fe66789083c7ff39df2 Mon Sep 17 00:00:00 2001
From: Knut Hjorth <knut.hjorth@aviant.no>
Date: Mon, 28 Oct 2024 10:18:50 +0100
Subject: [PATCH] fix(ulog): the index of the timestamp may be non-zero

The previous parsing assumed that the timestamp for a ulog data series
was always at index 0, which is often, but not necessarily the case. The
parser now store the correct index when parsing the definition.
---
 .../DataLoadULog/ulog_parser.cpp               | 18 +++++++++++++-----
 plotjuggler_plugins/DataLoadULog/ulog_parser.h |  3 ++-
 2 files changed, 15 insertions(+), 6 deletions(-)

diff --git a/plotjuggler_plugins/DataLoadULog/ulog_parser.cpp b/plotjuggler_plugins/DataLoadULog/ulog_parser.cpp
index dd838eb58..0933855e4 100644
--- a/plotjuggler_plugins/DataLoadULog/ulog_parser.cpp
+++ b/plotjuggler_plugins/DataLoadULog/ulog_parser.cpp
@@ -153,10 +153,6 @@ void ULogParser::parseDataMessage(const ULogParser::Subscription& sub, char* mes
   }
   Timeseries& timeseries = ts_it->second;
 
-  uint64_t time_val = *reinterpret_cast<uint64_t*>(message);
-  timeseries.timestamps.push_back(time_val);
-  message += sizeof(uint64_t);
-
   size_t index = 0;
   parseSimpleDataMessage(timeseries, sub.format, message, &index);
 }
@@ -173,8 +169,15 @@ char* ULogParser::parseSimpleDataMessage(Timeseries& timeseries, const Format* f
       continue;
     }
 
+    bool timestamp_done = false;
     for (int array_pos = 0; array_pos < field.array_size; array_pos++)
     {
+      if (*index == format->timestamp_idx && !timestamp_done) {
+        timestamp_done = true;
+        uint64_t time_val = *reinterpret_cast<uint64_t*>(message);
+        timeseries.timestamps.push_back(time_val);
+        message += sizeof(uint64_t);
+      }
       double value = 0;
       switch (field.type)
       {
@@ -619,7 +622,7 @@ bool ULogParser::readFormat(DataStream& datastream, uint16_t msg_size)
 
     if (field.type == UINT64 && field_name == StringView("timestamp"))
     {
-      // skip
+      format.timestamp_idx = format.fields.size();
     }
     else
     {
@@ -628,6 +631,11 @@ bool ULogParser::readFormat(DataStream& datastream, uint16_t msg_size)
     }
   }
 
+  if (format.timestamp_idx < 0) {
+      // Required timestamp is not found in definition
+      return false;
+  }
+
   format.name = name;
   _formats[name] = std::move(format);
 
diff --git a/plotjuggler_plugins/DataLoadULog/ulog_parser.h b/plotjuggler_plugins/DataLoadULog/ulog_parser.h
index b6b15b3b4..c2c475f75 100644
--- a/plotjuggler_plugins/DataLoadULog/ulog_parser.h
+++ b/plotjuggler_plugins/DataLoadULog/ulog_parser.h
@@ -79,12 +79,13 @@ class ULogParser
 
   struct Format
   {
-    Format() : padding(0)
+    Format() : padding(0), timestamp_idx(-1)
     {
     }
     std::string name;
     std::vector<Field> fields;
     int padding;
+    int timestamp_idx;
   };
 
   struct MessageLog