Add bare-metal TDK IMU ESKF fusion

soc/software/gps_system/header/fusion_bus.h

@@ -0,0 +1,42 @@
+#ifndef FUSION_BUS_H
+#define FUSION_BUS_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "fusion_measurement.h"
+
+#ifndef FUSION_IMU_QUEUE_CAPACITY
+#define FUSION_IMU_QUEUE_CAPACITY 256u
+#endif
+
+#ifndef FUSION_GPS_QUEUE_CAPACITY
+#define FUSION_GPS_QUEUE_CAPACITY 8u
+#endif
+
+typedef struct {
+	fusion_imu_measurement_t imu_queue[FUSION_IMU_QUEUE_CAPACITY];
+	fusion_gps_measurement_t gps_queue[FUSION_GPS_QUEUE_CAPACITY];
+	volatile unsigned int imu_head;
+	volatile unsigned int imu_tail;
+	volatile unsigned int gps_head;
+	volatile unsigned int gps_tail;
+	volatile uint64_t imu_overflow_count;
+	volatile uint64_t gps_overflow_count;
+} fusion_bus_t;
+
+void fusion_bus_init(fusion_bus_t* bus);
+
+/* Single producer (TDK ISR), single consumer (foreground). */
+bool fusion_bus_publish_imu(fusion_bus_t* bus,
+	                        const fusion_imu_measurement_t* measurement);
+bool fusion_bus_take_imu(fusion_bus_t* bus,
+	                     fusion_imu_measurement_t* measurement_out);
+
+/* Single producer (GPS WLS foreground), single fusion consumer. */
+bool fusion_bus_publish_gps(fusion_bus_t* bus,
+	                        const fusion_gps_measurement_t* measurement);
+bool fusion_bus_take_gps(fusion_bus_t* bus,
+	                     fusion_gps_measurement_t* measurement_out);
+
+#endif