#382: Lighthouse: Implement Sync0 calculation from multiple sensors

src/utils/interface/lighthouse/pulseProcessor.h

@@ -2,10 +2,13 @@
 
 #include <stdbool.h>
 #include <stdint.h>
+#include <stdlib.h>
 
 #define PULSE_PROCESSOR_N_SENSORS 8
 #define PULSE_PROCESSOR_HISTORY_LENGTH 8
 #define TIMESTAMP_BITWIDTH 29
+#define TIMESTAMP_MAX ((1<<TIMESTAMP_BITWIDTH)-1)
+
 
 enum pulseClass_e {unknown, sync0, sync1, sweep};
 
@@ -56,3 +59,19 @@ bool processPulse(pulseProcessor_t *state, unsigned int timestamp, unsigned int
  * @return false if no Sync0 found
  */
 bool findSyncTime(const pulseProcessorPulse_t pulseHistory[], uint32_t *foundSyncTime);
+
+/**
+ * @brief Get the System Sync time from sampled Sync0 time from multiple sensors
+ * 
+ * This function places the Sync0 timestamps modulo the lighthouse V1 frame length
+ * to estimate if they can possibly be coming from the same lighthouse system.
+ * This allows to check that the sampling done on multiple receiving sensor is
+ * consistent.
+ * 
+ * @param syncTimes Array of Sync0 timestamps
+ * @param nSyncTimes Number of timestamps in syncTimes array
+ * @param[out] syncTime Pointer to the variable where the resulting sync time is written
+ * @return true If an acceptable sync time could be calculated
+ * @return false If the sampled Sync0 timestamps do not make sense
+ */
+bool getSystemSyncTime(const uint32_t syncTimes[], int nSyncTimes, uint32_t *syncTime);

src/utils/src/lighthouse/pulseProcessor.c

@@ -103,4 +103,61 @@ bool findSyncTime(const pulseProcessorPulse_t pulseHistory[], uint32_t *foundSyn
   uint32_t delta = TS_DIFF(foundTimes[1], foundTimes[0]);
 
   return (nFound == 2 && delta < (FRAME_LENGTH + MAX_FRAME_LENGTH_NOISE) && delta > (FRAME_LENGTH - MAX_FRAME_LENGTH_NOISE));
+}
+
+bool getSystemSyncTime(const uint32_t syncTimes[], int nSyncTimes, uint32_t *syncTime)
+{
+  if (nSyncTimes == 0) {
+    return false;
+  }
+
+  // Detect if samples are wrapping
+  // If the samples are wrapping, all samples bellow TIMESTAMP_MAX/2 will
+  // be pushed by (1<<TIMESTAMP_BITWIDTH) to correct the wrapping
+  bool isWrapping = false;
+  int wref = syncTimes[0];
+  for (int i=0; i<nSyncTimes; i++) {
+    if (abs(wref - syncTimes[i]) > (TIMESTAMP_MAX/2)) {
+      isWrapping = true;
+    }
+  }
+
+  int32_t differenceSum = 0;
+  int32_t reference = syncTimes[0] % FRAME_LENGTH;
+  if (isWrapping && syncTimes[0] < (TIMESTAMP_MAX/2)) {
+    reference = (syncTimes[0] + (1<<TIMESTAMP_BITWIDTH)) % FRAME_LENGTH;
+  }
+
+  int minDiff = INT32_MAX;
+  int maxDiff = INT32_MIN;
+
+  for (size_t i=1; i<nSyncTimes; i++) {
+
+    int diff;
+
+    if (isWrapping && (syncTimes[i] < (TIMESTAMP_MAX/2))) {
+      diff = ((syncTimes[i] + (1<<TIMESTAMP_BITWIDTH)) % FRAME_LENGTH) - reference;
+    } else {
+      diff = (syncTimes[i] % FRAME_LENGTH) - reference;
+    }
+
+
+    if (diff < minDiff) {
+      minDiff = diff;
+    }
+    if (diff > maxDiff) {
+      maxDiff = diff;
+    }
+
+    differenceSum += diff;
+  }
+
+  if ((maxDiff - minDiff) > MAX_FRAME_LENGTH_NOISE) {
+    return false;
+  }
+
+  *syncTime = ((int)syncTimes[0] + (differenceSum / (nSyncTimes))) & (TIMESTAMP_MAX);
+
+
+  return true;
 }

test/utils/src/lighthouse/TestPulseProcessor.c

@@ -250,14 +250,89 @@ void testThatFindSyncCanDetectSync0FromTwoBasestationsWithTimestampWrapping()
   assertSyncTimeIsMultipleOfFrameLength(expectedSyncTime, actualSyncTime);
 }
 
+void testThatGetSystemSyncTimeReturnsTheAverageForGoodSyncData()
+{
+  // Fixture
+  uint32_t actualSyncTime = 0;
+  uint32_t expectedSyncTime = 2;
+  uint32_t syncTimes[8] = {1, 2, 3};
+  size_t nSyncTimes = 3;
+
+  // Test
+  bool found = getSystemSyncTime(syncTimes, nSyncTimes, &actualSyncTime);
+
+  // Assert
+  TEST_ASSERT_TRUE(found);
+  TEST_ASSERT_EQUAL(expectedSyncTime, actualSyncTime);
+}
+
+void testThatGetSystemSyncTimeHandlesTimestampsFromMultipleFrames()
+{
+  // Fixture
+  uint32_t actualSyncTime = 0;
+  uint32_t expectedSyncTime = 2;
+  uint32_t syncTimes[8] = {1 + FRAME_LENGTH, 2 + (2*FRAME_LENGTH), 3, 2 + (2*FRAME_LENGTH)};
+  size_t nSyncTimes = 4;
+
+  // Test
+  bool found = getSystemSyncTime(syncTimes, nSyncTimes, &actualSyncTime);
+
+  // Assert
+  TEST_ASSERT_TRUE(found);
+  assertSyncTimeIsMultipleOfFrameLength(expectedSyncTime, actualSyncTime);
+}
+
+void testThatGetSystemSyncTimeDoesNotReturnTimestampFor0Samples()
+{
+  // Fixture
+  uint32_t unused = 0;
+  uint32_t syncTimes[8] = {};
+  size_t nSyncTimes = 0;
+
+  // Test
+  bool found = getSystemSyncTime(syncTimes, nSyncTimes, &unused);
+
+  // Assert
+  TEST_ASSERT_FALSE(found);
+}
+
+void testThatGetSystemSyncTimeDoesNotReturnTimestampIfTooMuchTimestampsSpread()
+{
+  // Fixture
+  uint32_t unused = 0;
+  uint32_t syncTimes[8] = {1, 50, 3};
+  size_t nSyncTimes = 3;
+
+  // Test
+  bool found = getSystemSyncTime(syncTimes, nSyncTimes, &unused);
+
+  // Assert
+  TEST_ASSERT_FALSE(found);
+}
+
+void testThatGetSystemSyncTimeHandlesTimestampsWithWrapping()
+{
+  // Fixture
+  uint32_t actualSyncTime = 0;
+  uint32_t expectedSyncTime = TIMESTAMP_MAX;
+  uint32_t syncTimes[8] = {0, TIMESTAMP_MAX - 1, TIMESTAMP_MAX};
+  size_t nSyncTimes = 3;
+
+  // Test
+  bool found = getSystemSyncTime(syncTimes, nSyncTimes, &actualSyncTime);
+
+  // Assert
+  TEST_ASSERT_TRUE(found);
+  TEST_ASSERT_EQUAL(expectedSyncTime, actualSyncTime);
+}
 
 // Test helpers
 
 void assertSyncTimeIsMultipleOfFrameLength(uint32_t expectedSyncTime, uint32_t actualSyncTime)
 {
   uint32_t diff = actualSyncTime - expectedSyncTime;
 
-  TEST_ASSERT_TRUE_MESSAGE(diff % FRAME_LENGTH < MAX_FRAME_LENGTH_NOISE, "Sync time out of bound");
+  TEST_ASSERT_LESS_THAN_MESSAGE(MAX_FRAME_LENGTH_NOISE, diff % FRAME_LENGTH, "Sync time out of bound");
 }
 
 void limitTimestamps(pulseProcessorPulse_t history[])