Clock correction tests

Makefile

@@ -72,6 +72,7 @@ VPATH_CF2 += $(LIB)/CMSIS/STM32F4xx/Source/
 VPATH_CF2 += $(LIB)/STM32_USB_Device_Library/Core/src
 VPATH_CF2 += $(LIB)/STM32_USB_OTG_Driver/src
 VPATH_CF2 += src/deck/api src/deck/core src/deck/drivers/src src/deck/drivers/src/test
+VPATH_CF2 += src/deck/drivers/src/clockCorrection
 CRT0_CF2 = startup_stm32f40xx.o system_stm32f4xx.o
 
 # Should maybe be in separate file?
@@ -181,6 +182,7 @@ PROJ_OBJ_CF2 += cppmdeck.o
 PROJ_OBJ_CF2 += usddeck.o
 PROJ_OBJ_CF2 += zranger.o zranger2.o
 PROJ_OBJ_CF2 += locodeck.o
+PROJ_OBJ_CF2 += clockCorrectionFunctions.o clockCorrectionEngine.o
 PROJ_OBJ_CF2 += lpsTwrTag.o
 PROJ_OBJ_CF2 += lpsTdoa2Tag.o
 PROJ_OBJ_CF2 += lpsTdoa3Tag.o lpsTdoaTagEngine.o lpsTdoaTagStats.o
@@ -238,6 +240,7 @@ INCLUDES_CF2 += -I$(LIB)/CMSIS/STM32F4xx/Include
 INCLUDES_CF2 += -I$(LIB)/STM32_USB_Device_Library/Core/inc
 INCLUDES_CF2 += -I$(LIB)/STM32_USB_OTG_Driver/inc
 INCLUDES_CF2 += -Isrc/deck/interface -Isrc/deck/drivers/interface
+INCLUDES_CF2 += -Isrc/deck/drivers/interface/clockCorrection
 INCLUDES_CF2 += -Ivendor/libdw1000/inc
 INCLUDES_CF2 += -I$(LIB)/FatFS
 INCLUDES_CF2 += -I$(LIB)/vl53l1
@@ -257,6 +260,8 @@ STFLAGS_CF2 = -DSTM32F4XX -DSTM32F40_41xxx -DHSE_VALUE=8000000 -DUSE_STDPERIPH_D
 
 ifeq ($(DEBUG), 1)
   CFLAGS += -O0 -g3 -DDEBUG
+  # Prevent silent errors when converting between types (requires explicit casting)
+  CFLAGS += -Wconversion
 else
 	# Fail on warnings
   CFLAGS += -Os -g3 -Werror
@@ -408,4 +413,5 @@ include tools/make/targets.mk
 -include $(DEPS)
 
 unit:
-	rake unit "DEFINES=$(CFLAGS)" "FILES=$(FILES)"
+# The flag "-DUNITY_INCLUDE_DOUBLE" allows comparison of double values in Unity. See: https://stackoverflow.com/a/37790196
+	rake unit "DEFINES=$(CFLAGS) -DUNITY_INCLUDE_DOUBLE" "FILES=$(FILES)"

src/deck/drivers/interface/clockCorrection/clockCorrectionEngine.h

@@ -0,0 +1,20 @@
+/**
+ Exposes the public API used for clock correction in an Object Oriented way (and hides the functions that are only part of the internal implementation).
+ */
+
+#ifndef clockCorrectionEngine_h
+#define clockCorrectionEngine_h
+
+#include <stdbool.h>
+#include "clockCorrectionStorage.h"
+
+typedef struct {
+  double (*getClockCorrection)(clockCorrectionStorage_t* storage);
+  double (*calculateClockCorrection)(const uint64_t new_t_in_cl_reference, const uint64_t old_t_in_cl_reference, const uint64_t new_t_in_cl_x, const uint64_t old_t_in_cl_x);
+  bool (*updateClockCorrection)(clockCorrectionStorage_t* storage, const double clockCorrectionCandidate);
+
+} clockCorrectionEngine_t;
+
+extern clockCorrectionEngine_t clockCorrectionEngine;
+
+#endif /* clockCorrectionEngine_h */

src/deck/drivers/interface/clockCorrection/clockCorrectionFunctions.h

@@ -0,0 +1,18 @@
+/**
+Exposes all the functions used for clock correction, for testing purposes.
+ */
+
+#ifndef clockCorrectionFunctions_h
+#define clockCorrectionFunctions_h
+
+#include <stdbool.h>
+#include "clockCorrectionStorage.h"
+
+double getClockCorrection(clockCorrectionStorage_t* storage);
+uint64_t truncateTimeStampFromDW1000(uint64_t timeStamp);
+void fillClockCorrectionBucket(clockCorrectionStorage_t* storage);
+bool emptyClockCorrectionBucket(clockCorrectionStorage_t* storage);
+double calculateClockCorrection(const uint64_t new_t_in_cl_reference, const uint64_t old_t_in_cl_reference, const uint64_t new_t_in_cl_x, const uint64_t old_t_in_cl_x);
+bool updateClockCorrection(clockCorrectionStorage_t* storage, const double clockCorrectionCandidate);
+
+#endif /* clockCorrectionFunctions_h */

src/deck/drivers/interface/clockCorrection/clockCorrectionStorage.h

@@ -0,0 +1,16 @@
+/**
+ Exposes the storage required by the clock correction engine.
+ */
+
+#ifndef clockCorrectionStorage_h
+#define clockCorrectionStorage_h
+
+#include <stdint.h>
+
+typedef struct {
+  double clockCorrection;
+  unsigned int clockCorrectionBucket;
+} clockCorrectionStorage_t;
+
+
+#endif /* clockCorrectionStorage_h */

src/deck/drivers/src/clockCorrection/clockCorrectionEngine.c

@@ -0,0 +1,8 @@
+#include "clockCorrectionEngine.h"
+#include "clockCorrectionFunctions.h"
+
+clockCorrectionEngine_t clockCorrectionEngine = {
+  .getClockCorrection = getClockCorrection,
+  .calculateClockCorrection = calculateClockCorrection,
+  .updateClockCorrection = updateClockCorrection
+};

src/deck/drivers/src/clockCorrection/clockCorrectionFunctions.c

@@ -0,0 +1,97 @@
+#include "clockCorrectionFunctions.h"
+
+#define MAX_CLOCK_DEVIATION_SPEC 10e-6
+#define CLOCK_CORRECTION_SPEC_MIN (1.0 - MAX_CLOCK_DEVIATION_SPEC * 2)
+#define CLOCK_CORRECTION_SPEC_MAX (1.0 + MAX_CLOCK_DEVIATION_SPEC * 2)
+
+#define CLOCK_CORRECTION_ACCEPTED_NOISE 0.03e-6
+#define CLOCK_CORRECTION_FILTER 0.1
+#define CLOCK_CORRECTION_BUCKET_MAX 4
+
+/**
+ Obtains the clock correction from a clockCorrectionStorage_t object. This is the recommended public API to obtan the clock correction, instead of getting it directly from the storage object.
+ */
+double getClockCorrection(clockCorrectionStorage_t* storage) {
+  return storage->clockCorrection;
+}
+
+/**
+ Truncates a timestamp to 40 bits, which is the number of bits used to timestamp events in the DW1000 chip. This truncation ensures that the value returned is a valid time event, even if the DW1000 wrapped around at some point.
+ */
+uint64_t truncateTimeStampFromDW1000(uint64_t timeStamp) {
+  uint64_t mask = 0xFFFFFFFFFF; // 40 bits
+  return timeStamp & mask;
+}
+
+/**
+ Implementation of the leaky bucket algorithm. See: https://en.wikipedia.org/wiki/Leaky_bucket
+ */
+void fillClockCorrectionBucket(clockCorrectionStorage_t* storage) {
+  if (storage->clockCorrectionBucket < CLOCK_CORRECTION_BUCKET_MAX) {
+    storage->clockCorrectionBucket++;
+  }
+}
+
+/**
+ Implementation of the leaky bucket algorithm. See: https://en.wikipedia.org/wiki/Leaky_bucket
+ */
+bool emptyClockCorrectionBucket(clockCorrectionStorage_t* storage) {
+  if (storage->clockCorrectionBucket > 0) {
+    storage->clockCorrectionBucket--;
+    return false;
+  }
+
+  return true;
+}
+
+/**
+ Calculates the clock correction between a reference clock and another clock (x).
+
+ @param new_t_in_cl_reference The newest time of occurrence for an event (t), meassured by the clock of reference
+ @param old_t_in_cl_reference An older time of occurrence for an event (t), meassured by the clock of reference
+ @param new_t_in_cl_x The newest time of occurrence for an event (t), meassured by clock x
+ @param old_t_in_cl_x The previous time of occurrence for an event (t), meassured by clock x
+ @return The necessary clock correction to apply to timestamps meassured by clock x, in order to obtain their value like if the meassurement was done by the reference clock. Or -1 if it was not possible to perform the computation. Example: timestamp_in_cl_reference = clockCorrection * timestamp_in_cl_x
+ */
+double calculateClockCorrection(const uint64_t new_t_in_cl_reference, const uint64_t old_t_in_cl_reference, const uint64_t new_t_in_cl_x, const uint64_t old_t_in_cl_x) {
+  const uint64_t tickCount_in_cl_reference = truncateTimeStampFromDW1000(new_t_in_cl_reference - old_t_in_cl_reference);
+  const uint64_t tickCount_in_cl_x = truncateTimeStampFromDW1000(new_t_in_cl_x - old_t_in_cl_x);
+
+  if (tickCount_in_cl_x == 0) {
+    return -1;
+  }
+
+  return (double)tickCount_in_cl_reference / (double)tickCount_in_cl_x;
+}
+
+/**
+ Updates the clock correction only if the provided value follows certain conditions. This is used to discard wrong clock correction meassurements.
+ @return True if the provided clock correction was valid and accepted, false otherwise.
+ */
+bool updateClockCorrection(clockCorrectionStorage_t* storage, const double clockCorrectionCandidate) {
+  bool sampleIsAccepted = false;
+
+  if (CLOCK_CORRECTION_SPEC_MIN < clockCorrectionCandidate && clockCorrectionCandidate < CLOCK_CORRECTION_SPEC_MAX) {
+    const double currentClockCorrection = storage->clockCorrection;
+    const double difference = clockCorrectionCandidate - currentClockCorrection;
+
+    if (-CLOCK_CORRECTION_ACCEPTED_NOISE < difference && difference < CLOCK_CORRECTION_ACCEPTED_NOISE) {
+      // Simple low pass filter
+      const double newClockCorrection = currentClockCorrection * CLOCK_CORRECTION_FILTER + clockCorrectionCandidate * (1.0 - CLOCK_CORRECTION_FILTER);
+
+      fillClockCorrectionBucket(storage);
+
+      storage->clockCorrection = newClockCorrection;
+      sampleIsAccepted = true;
+    } else {
+      const bool shouldAcceptANewClockReference = emptyClockCorrectionBucket(storage);
+      if (shouldAcceptANewClockReference) {
+        fillClockCorrectionBucket(storage);
+        storage->clockCorrection = clockCorrectionCandidate;
+        sampleIsAccepted = true;
+      }
+    }
+  }
+
+  return sampleIsAccepted;
+}