Add bare-metal TDK IMU ESKF fusion

soc/software/gps_system/Makefile

@@ -7,6 +7,14 @@ BSP_UART_CLOCK_HZ ?= 50000000
 
 APP_SRCS := \
 	gps_test.c \
+	src/tdk_icm42688.c \
+	src/eskf.c \
+	src/fusion_motion.c \
+	src/fusion_engine.c \
+	src/fusion_bus.c \
+	src/fusion_gps_adapter.c \
+	src/navigation_fusion.c \
+	src/gps_fusion_publisher.c \
 	src/gps_ip.c \
 	src/gps_interrupt_handlers.c \
 	src/gps_time_anchor.c \
@@ -15,9 +23,16 @@ APP_SRCS := \
 	src/NavEquations.c \
 	src/matrixLib.c
 
+BSP_SRCS := init.c uart.c plic.c irq.c spi_flash.c pmod_spi.c
+
+TDK_IMU_ODR_HZ ?= 500
+GPS_NAV_SOLVE_TIMER_TICKS ?= 50
+
 include ../common/app.mk
 
 CFLAGS += -Iheader
+CFLAGS += -DTDK_IMU_ODR_HZ=$(TDK_IMU_ODR_HZ) \
+	-DGPS_NAV_SOLVE_TIMER_TICKS=$(GPS_NAV_SOLVE_TIMER_TICKS)
 LDLIBS += -lm
 
 ifeq ($(GPS_DEBUG),1)

soc/software/gps_system/gps_test.c

@@ -5,21 +5,54 @@
 #include <float.h>
 #include "matrixLib.h"
 #include "gps_macros.h"
+#include "navigation_fusion.h"
+#include "tdk_icm42688.h"
 
 int main()
 {
-		
-	gps_startup_initialization(15);
+	tdk_icm42688_config_t imu_config;
+	tdk_icm42688_default_config(&imu_config);
+	bool imu_initialized = tdk_icm42688_init(&imu_config);
+	bool fusion_initialized = navigation_fusion_init(BSP_CLINT_TIMEBASE_HZ);
+
+	gps_startup_initialization(GPS_NAV_SOLVE_TIMER_TICKS);
+	bool imu_running = imu_initialized && fusion_initialized && tdk_icm42688_start();
+	GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_FUSION,
+	        "init imu=%u running=%u odr_hz=%u timebase_hz=%lu\r\n",
+	        imu_initialized ? 1u : 0u, imu_running ? 1u : 0u,
+	        imu_config.odr_hz, (unsigned long)BSP_CLINT_TIMEBASE_HZ);
 
 	acq_start_loop();
+	uint64_t last_fusion_status_tick = 0u;
 
 	while(1)
 	{
 		asm volatile("wfi"); 
 		gps_background_task();
 		calculate_position();
-		// TODO: implement calculate position nav solve
-		//calculate_position_nav_solve();
+		if(fusion_initialized)
+		{
+			(void)navigation_fusion_service();
+			uint64_t now = gps_get_systick();
+			if((now - last_fusion_status_tick) >= GPS_SYSTICK_HZ)
+			{
+				navigation_fusion_output_t output;
+				if(navigation_fusion_get_output(&output))
+				{
+					GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_FUSION,
+					        "state=%s lat=%.10lf lon=%.10lf height=%.3lf "
+					        "speed=%.3lf nis=%.3lf gps=%lu/%lu sigma=%.2lf,%.2lf,%.2lf\r\n",
+					        fusion_motion_state_name(output.motion_state),
+					        output.latitude_deg, output.longitude_deg, output.height_m,
+					        output.speed_mps, output.last_position_nis,
+					        (unsigned long)output.gps_accepted,
+					        (unsigned long)output.gps_nis_rejected,
+					        output.position_std_m[0], output.position_std_m[1],
+					        output.position_std_m[2]);
+				}
+				last_fusion_status_tick = now;
+			}
+		}
 	}
 
 	return 0;

soc/software/gps_system/header/eskf.h

@@ -0,0 +1,85 @@
+#ifndef ESKF_H
+#define ESKF_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#define ESKF_VECTOR_DIM 3u
+#define ESKF_QUATERNION_DIM 4u
+#define ESKF_ERROR_STATE_DIM 15u
+
+typedef enum {
+	ESKF_UPDATE_INVALID = 0,
+	ESKF_UPDATE_ACCEPTED,
+	ESKF_UPDATE_REJECTED_NIS
+} eskf_update_result_t;
+
+typedef struct {
+	double accel_noise_density[ESKF_VECTOR_DIM];
+	double gyro_noise_density[ESKF_VECTOR_DIM];
+	double accel_bias_random_walk[ESKF_VECTOR_DIM];
+	double gyro_bias_random_walk[ESKF_VECTOR_DIM];
+	double initial_position_std_m[ESKF_VECTOR_DIM];
+	double initial_velocity_std_mps[ESKF_VECTOR_DIM];
+	double initial_attitude_std_rad[ESKF_VECTOR_DIM];
+	double initial_accel_bias_std_mps2[ESKF_VECTOR_DIM];
+	double initial_gyro_bias_std_rps[ESKF_VECTOR_DIM];
+	double gravity_mps2;
+	double max_propagation_step_s;
+	double position_nis_gate;
+	double covariance_diagonal_floor;
+} eskf_config_t;
+
+typedef struct {
+	double position_m[ESKF_VECTOR_DIM];
+	double velocity_mps[ESKF_VECTOR_DIM];
+	double body_to_nav_quaternion[ESKF_QUATERNION_DIM];
+	double accel_bias_mps2[ESKF_VECTOR_DIM];
+	double gyro_bias_rps[ESKF_VECTOR_DIM];
+	double covariance[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM];
+	double last_linear_accel_nav_mps2[ESKF_VECTOR_DIM];
+	double last_unbiased_gyro_rps[ESKF_VECTOR_DIM];
+	double last_position_nis;
+	uint64_t propagation_count;
+	uint64_t position_update_accepted_count;
+	uint64_t position_update_rejected_count;
+	bool initialized;
+} eskf_t;
+
+void eskf_default_config(eskf_config_t* config);
+
+bool eskf_quaternion_align_vectors(const double source[ESKF_VECTOR_DIM],
+	                               const double target[ESKF_VECTOR_DIM],
+	                               double quaternion_out[ESKF_QUATERNION_DIM]);
+
+bool eskf_initialize(eskf_t* filter,
+	                 const eskf_config_t* config,
+	                 const double initial_position_m[ESKF_VECTOR_DIM],
+	                 const double body_to_nav_quaternion[ESKF_QUATERNION_DIM],
+	                 const double accel_bias_mps2[ESKF_VECTOR_DIM],
+	                 const double gyro_bias_rps[ESKF_VECTOR_DIM]);
+
+bool eskf_propagate(eskf_t* filter,
+	                const eskf_config_t* config,
+	                const double accel_mps2[ESKF_VECTOR_DIM],
+	                const double gyro_rps[ESKF_VECTOR_DIM],
+	                double dt_s);
+
+eskf_update_result_t eskf_position_update(
+	eskf_t* filter,
+	const eskf_config_t* config,
+	const double position_m[ESKF_VECTOR_DIM],
+	const double covariance_m2[ESKF_VECTOR_DIM * ESKF_VECTOR_DIM],
+	double* nis_out);
+
+bool eskf_zero_velocity_update(eskf_t* filter, double velocity_std_mps);
+
+bool eskf_zero_angular_rate_update(
+	eskf_t* filter,
+	const double measured_gyro_rps[ESKF_VECTOR_DIM],
+	const double measurement_std_rps[ESKF_VECTOR_DIM]);
+
+double eskf_speed_mps(const eskf_t* filter);
+bool eskf_state_is_finite(const eskf_t* filter);
+
+#endif

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

soc/software/gps_system/header/fusion_engine.h

@@ -0,0 +1,123 @@
+#ifndef FUSION_ENGINE_H
+#define FUSION_ENGINE_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "eskf.h"
+#include "fusion_measurement.h"
+#include "fusion_motion.h"
+
+#ifndef FUSION_IMU_HISTORY_CAPACITY
+#define FUSION_IMU_HISTORY_CAPACITY 2048u
+#endif
+
+#ifndef FUSION_POSITION_PENDING_CAPACITY
+#define FUSION_POSITION_PENDING_CAPACITY 8u
+#endif
+
+typedef enum {
+	FUSION_ENGINE_CALIBRATING = 0,
+	FUSION_ENGINE_RUNNING,
+	FUSION_ENGINE_FAULT
+} fusion_engine_state_t;
+
+typedef struct {
+	eskf_config_t eskf;
+	fusion_motion_config_t motion;
+	uint64_t timebase_hz;
+	double calibration_duration_s;
+	unsigned int calibration_min_samples;
+	double calibration_accel_norm_tolerance_mps2;
+	double calibration_gyro_limit_rps;
+	double calibration_accel_std_limit_mps2;
+	double calibration_gyro_std_limit_rps;
+	double zero_update_period_s;
+	double zero_velocity_std_mps;
+	double zero_gyro_std_rps[3];
+	double gnss_motion_speed_threshold_mps;
+	double gnss_displacement_nis_gate;
+	unsigned int gnss_moving_confirmations;
+	unsigned int gnss_still_confirmations;
+} fusion_engine_config_t;
+
+typedef struct {
+	eskf_t filter;
+	fusion_motion_detector_t motion;
+	fusion_imu_measurement_t held_imu;
+	fusion_gnss_motion_evidence_t gnss_evidence;
+	double last_accepted_position_m[3];
+	double last_accepted_position_covariance_m2[9];
+	uint64_t filter_timestamp_ticks;
+	uint64_t last_zero_update_ticks;
+	uint64_t last_accepted_position_ticks;
+	unsigned int gnss_moving_count;
+	unsigned int gnss_still_count;
+	bool have_held_imu;
+	bool have_accepted_position;
+} fusion_runtime_t;
+
+typedef struct {
+	uint64_t imu_received;
+	uint64_t imu_rejected;
+	uint64_t imu_history_overflow;
+	uint64_t checkpoint_advance_count;
+	uint64_t calibration_reset_count;
+	uint64_t position_received;
+	uint64_t position_source_rejected;
+	uint64_t position_nis_rejected;
+	uint64_t position_stale_rejected;
+	uint64_t position_pending_overflow;
+	uint64_t position_accepted;
+	uint64_t replay_count;
+	uint64_t replayed_imu_count;
+	uint64_t zero_velocity_update_count;
+} fusion_engine_diagnostics_t;
+
+typedef struct {
+	uint64_t start_ticks;
+	uint64_t last_ticks;
+	double accel_mean[3];
+	double gyro_mean[3];
+	double accel_m2[3];
+	double gyro_m2[3];
+	unsigned int sample_count;
+	bool have_start;
+} fusion_calibration_accumulator_t;
+
+typedef struct {
+	fusion_engine_config_t config;
+	fusion_engine_state_t state;
+	fusion_runtime_t current;
+	fusion_runtime_t checkpoint;
+	fusion_calibration_accumulator_t calibration;
+	fusion_imu_measurement_t imu_history[FUSION_IMU_HISTORY_CAPACITY];
+	fusion_position_measurement_t pending_positions[FUSION_POSITION_PENDING_CAPACITY];
+	fusion_position_measurement_t initial_position;
+	fusion_engine_diagnostics_t diagnostics;
+	unsigned int imu_history_head;
+	unsigned int imu_history_count;
+	unsigned int pending_position_count;
+	bool checkpoint_valid;
+	bool have_initial_position;
+} fusion_engine_t;
+
+void fusion_engine_default_config(fusion_engine_config_t* config,
+	                              uint64_t timebase_hz);
+bool fusion_engine_init(fusion_engine_t* engine,
+	                    const fusion_engine_config_t* config);
+
+bool fusion_engine_push_imu(fusion_engine_t* engine,
+	                        const fusion_imu_measurement_t* measurement);
+bool fusion_engine_push_position(
+	fusion_engine_t* engine,
+	const fusion_position_measurement_t* measurement,
+	eskf_update_result_t* update_result_out,
+	double* nis_out);
+
+bool fusion_engine_get_runtime(const fusion_engine_t* engine,
+	                           fusion_runtime_t* runtime_out);
+const fusion_engine_diagnostics_t* fusion_engine_diagnostics(
+	const fusion_engine_t* engine);
+
+#endif

soc/software/gps_system/header/fusion_gps_adapter.h

@@ -0,0 +1,37 @@
+#ifndef FUSION_GPS_ADAPTER_H
+#define FUSION_GPS_ADAPTER_H
+
+#include <stdbool.h>
+
+#include "fusion_measurement.h"
+
+typedef struct {
+	double minimum_horizontal_std_m;
+	double minimum_vertical_std_m;
+	double maximum_position_std_m;
+} fusion_gps_adapter_config_t;
+
+typedef struct {
+	double origin_latitude_deg;
+	double origin_longitude_deg;
+	double origin_height_m;
+	double origin_ecef_m[3];
+	double ecef_to_enu[3][3];
+	fusion_gps_adapter_config_t config;
+	bool origin_valid;
+} fusion_gps_adapter_t;
+
+void fusion_gps_adapter_default_config(fusion_gps_adapter_config_t* config);
+bool fusion_gps_adapter_init(fusion_gps_adapter_t* adapter,
+	                         const fusion_gps_adapter_config_t* config);
+bool fusion_gps_adapter_convert(fusion_gps_adapter_t* adapter,
+	                            const fusion_gps_measurement_t* gps,
+	                            fusion_position_measurement_t* position_out);
+bool fusion_gps_adapter_local_to_geodetic(
+	const fusion_gps_adapter_t* adapter,
+	const double position_enu_m[3],
+	double* latitude_deg_out,
+	double* longitude_deg_out,
+	double* height_m_out);
+
+#endif

soc/software/gps_system/header/fusion_measurement.h

@@ -0,0 +1,43 @@
+#ifndef FUSION_MEASUREMENT_H
+#define FUSION_MEASUREMENT_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+/*
+ * Hardware- and solver-neutral messages shared at the GPS/ESKF boundary.
+ * timestamp_ticks always belongs to the common local monotonic timebase; GPS
+ * week/TOW is metadata and must not drive filter dt.
+ */
+typedef struct {
+	uint64_t timestamp_ticks;
+	double latitude_deg;
+	double longitude_deg;
+	double height_m;
+	double covariance_enu_m2[3][3];
+	double hacc_m;
+	double vacc_m;
+	double residual_rms_m;
+	double chi_square_ratio;
+	unsigned int used_satellites;
+	bool solution_valid;
+} fusion_gps_measurement_t;
+
+typedef struct {
+	uint64_t timestamp_ticks;
+	double accel_mps2[3];
+	double gyro_rps[3];
+	double temperature_c;
+	uint32_t sequence;
+} fusion_imu_measurement_t;
+
+/* Generic local-navigation measurement consumed by the ESKF coordinator. */
+typedef struct {
+	uint64_t timestamp_ticks;
+	double position_enu_m[3];
+	double covariance_enu_m2[9];
+	double source_quality;
+	bool source_valid;
+} fusion_position_measurement_t;
+
+#endif

soc/software/gps_system/header/fusion_motion.h

@@ -0,0 +1,63 @@
+#ifndef FUSION_MOTION_H
+#define FUSION_MOTION_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+typedef enum {
+	FUSION_MOTION_BOOTSTRAP = 0,
+	FUSION_MOTION_STATIONARY,
+	FUSION_MOTION_MOVING,
+	FUSION_MOTION_STATIONARY_CANDIDATE
+} fusion_motion_state_t;
+
+typedef enum {
+	FUSION_GNSS_MOTION_UNKNOWN = 0,
+	FUSION_GNSS_MOTION_STILL,
+	FUSION_GNSS_MOTION_MOVING
+} fusion_gnss_motion_evidence_t;
+
+typedef struct {
+	double stationary_accel_limit_mps2;
+	double stationary_gyro_limit_rps;
+	double moving_accel_threshold_mps2;
+	double moving_gyro_threshold_rps;
+	double moving_jerk_threshold_mps3;
+	double jerk_filter_time_constant_s;
+	double stationary_speed_limit_mps;
+	double moving_speed_threshold_mps;
+	double stationary_dwell_s;
+	unsigned int weak_motion_samples;
+} fusion_motion_config_t;
+
+typedef struct {
+	fusion_motion_state_t state;
+	double quiet_time_s;
+	double filtered_linear_accel_mps2[3];
+	double previous_filtered_linear_accel_mps2[3];
+	double last_linear_accel_norm_mps2;
+	double last_gyro_norm_rps;
+	double last_jerk_norm_mps3;
+	double last_speed_mps;
+	unsigned int weak_motion_count;
+	uint64_t transition_count;
+	bool have_previous_accel;
+} fusion_motion_detector_t;
+
+void fusion_motion_default_config(fusion_motion_config_t* config);
+void fusion_motion_init(fusion_motion_detector_t* detector,
+	                    bool bootstrap_stationary);
+
+fusion_motion_state_t fusion_motion_update(
+	fusion_motion_detector_t* detector,
+	const fusion_motion_config_t* config,
+	const double linear_accel_nav_mps2[3],
+	const double unbiased_gyro_rps[3],
+	double speed_mps,
+	fusion_gnss_motion_evidence_t gnss_evidence,
+	double dt_s);
+
+bool fusion_motion_allows_zero_update(const fusion_motion_detector_t* detector);
+const char* fusion_motion_state_name(fusion_motion_state_t state);
+
+#endif

soc/software/gps_system/header/gps_fusion_publisher.h

@@ -0,0 +1,12 @@
+#ifndef GPS_FUSION_PUBLISHER_H
+#define GPS_FUSION_PUBLISHER_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "NavEquations.h"
+
+bool gps_fusion_publish_solution(const gps_nav_solution_t* solution,
+	                             uint64_t measurement_timestamp_ticks);
+
+#endif

soc/software/gps_system/header/gps_ip_defines.h

@@ -46,7 +46,9 @@
 			#define GPS_EVENT_QUEUE_SIZE        64u
 			#define GPS_SYSTICK_HZ              20u
 			#define GPS_NAV_SOLVE_TIMER_TICK_MS 20u
+			#ifndef GPS_NAV_SOLVE_TIMER_TICKS
 			#define GPS_NAV_SOLVE_TIMER_TICKS   50u
+			#endif
 			#define GPS_NAV_SOLVE_PERIOD_MS     (GPS_NAV_SOLVE_TIMER_TICKS * GPS_NAV_SOLVE_TIMER_TICK_MS)
 			/* Keep integrity statistics active, but do not let leave-one-out FDE
 			 * replace the full-WLS position until the real measurement-noise model
@@ -62,7 +64,7 @@
 			#define GPS_NAV_TOW_CLUSTER_TOLERANCE_SEC 0.001
 			#define GPS_HOW_ANCHOR_MAX_AGE_SEC         12.0
 
-			_Static_assert(GPS_NAV_SOLVE_PERIOD_MS == 1000u, "NAV solve period must be 1 second");
+			_Static_assert(GPS_NAV_SOLVE_TIMER_TICKS > 0u, "NAV solve timer must be non-zero");
 			_Static_assert((GPS_NAV_ENABLE_FDE == 0u) || (GPS_NAV_ENABLE_FDE == 1u),
 			               "GPS_NAV_ENABLE_FDE must be zero or one");
 

soc/software/gps_system/header/gps_macros.h

@@ -30,6 +30,7 @@
 		#define GPS_LOG_CAT_SCHED "SCHED"
 		#define GPS_LOG_CAT_MEM   "MEM"
 		#define GPS_LOG_CAT_HWREG "HWREG"
+		#define GPS_LOG_CAT_FUSION "FUSION"
 	
 		// Set the current verbosity level
 		#ifdef DEBUG

soc/software/gps_system/header/linked_list.h

@@ -17,6 +17,7 @@ typedef struct gps_ephem_ll {
 	volatile uint32_t gps_nav_solve_consumed_sequence;
 	volatile uint32_t gps_last_nav_solve_abs_sample;
 	volatile double gps_last_nav_solve_corrected_abs_sample;
+	volatile uint64_t gps_last_nav_solve_local_ticks;
 	volatile uint64_t gps_last_nav_solve_pps_counter;
 	volatile uint32_t gps_last_pps_capture_sequence;
 	volatile uint64_t gps_last_measurement_tick;

soc/software/gps_system/header/navigation_fusion.h

@@ -0,0 +1,33 @@
+#ifndef NAVIGATION_FUSION_H
+#define NAVIGATION_FUSION_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "fusion_engine.h"
+#include "fusion_measurement.h"
+#include "fusion_motion.h"
+
+typedef struct {
+	uint64_t timestamp_ticks;
+	double position_enu_m[3];
+	double velocity_enu_mps[3];
+	double latitude_deg;
+	double longitude_deg;
+	double height_m;
+	double position_std_m[3];
+	double speed_mps;
+	double last_position_nis;
+	fusion_motion_state_t motion_state;
+	uint64_t gps_accepted;
+	uint64_t gps_nis_rejected;
+	bool valid;
+} navigation_fusion_output_t;
+
+bool navigation_fusion_init(uint64_t local_timebase_hz);
+bool navigation_fusion_publish_gps(const fusion_gps_measurement_t* measurement);
+unsigned int navigation_fusion_service(void);
+bool navigation_fusion_get_output(navigation_fusion_output_t* output);
+const fusion_engine_t* navigation_fusion_engine(void);
+
+#endif

soc/software/gps_system/header/tdk_icm42688.h

@@ -0,0 +1,75 @@
+#ifndef TDK_ICM42688_H
+#define TDK_ICM42688_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "fusion_measurement.h"
+
+#ifndef TDK_IMU_RAW_QUEUE_CAPACITY
+#define TDK_IMU_RAW_QUEUE_CAPACITY 256u
+#endif
+
+#ifndef TDK_IMU_ODR_HZ
+#define TDK_IMU_ODR_HZ 500u
+#endif
+
+typedef enum {
+	TDK_ACCEL_FS_2G = 2,
+	TDK_ACCEL_FS_4G = 4,
+	TDK_ACCEL_FS_8G = 8,
+	TDK_ACCEL_FS_16G = 16
+} tdk_accel_full_scale_t;
+
+typedef enum {
+	TDK_GYRO_FS_15D625 = 15625,
+	TDK_GYRO_FS_31D25 = 31250,
+	TDK_GYRO_FS_62D5 = 62500,
+	TDK_GYRO_FS_125 = 125000,
+	TDK_GYRO_FS_250 = 250000,
+	TDK_GYRO_FS_500 = 500000,
+	TDK_GYRO_FS_1000 = 1000000,
+	TDK_GYRO_FS_2000 = 2000000
+} tdk_gyro_full_scale_mdps_t;
+
+typedef struct {
+	uint32_t odr_hz;
+	tdk_accel_full_scale_t accel_full_scale_g;
+	tdk_gyro_full_scale_mdps_t gyro_full_scale_mdps;
+	uint8_t ui_filter_config;
+} tdk_icm42688_config_t;
+
+typedef struct {
+	uint64_t timestamp_ticks;
+	int16_t temperature_raw;
+	int16_t accel_raw[3];
+	int16_t gyro_raw[3];
+	uint32_t sequence;
+} tdk_icm42688_raw_sample_t;
+
+typedef struct {
+	uint64_t irq_count;
+	uint64_t sample_count;
+	uint64_t invalid_sample_count;
+	uint64_t spurious_irq_count;
+	uint64_t queue_overflow_count;
+	uint64_t estimated_missed_sample_count;
+	uint64_t last_timestamp_ticks;
+	uint8_t who_am_i;
+	bool initialized;
+	bool running;
+} tdk_icm42688_diagnostics_t;
+
+void tdk_icm42688_default_config(tdk_icm42688_config_t* config);
+bool tdk_icm42688_init(const tdk_icm42688_config_t* config);
+bool tdk_icm42688_start(void);
+void tdk_icm42688_stop(void);
+
+/* Called only by the common PLIC ISR for BSP_PMOD_IRQ. */
+void tdk_icm42688_irq_handler(void);
+
+/* Foreground conversion; no floating-point work is performed in the ISR. */
+bool tdk_icm42688_take_measurement(fusion_imu_measurement_t* measurement_out);
+void tdk_icm42688_get_diagnostics(tdk_icm42688_diagnostics_t* diagnostics_out);
+
+#endif

soc/software/gps_system/src/eskf.c

@@ -0,0 +1,809 @@
+#include "eskf.h"
+
+#include <float.h>
+#include <math.h>
+#include <stddef.h>
+#include <string.h>
+
+#define ESKF_POSITION_INDEX 0u
+#define ESKF_VELOCITY_INDEX 3u
+#define ESKF_ATTITUDE_INDEX 6u
+#define ESKF_ACCEL_BIAS_INDEX 9u
+#define ESKF_GYRO_BIAS_INDEX 12u
+
+#define ESKF_PI 3.14159265358979323846264338327950288
+
+static double square(double value)
+{
+	return value * value;
+}
+
+static double vector_norm3(const double value[3])
+{
+	return sqrt(square(value[0]) + square(value[1]) + square(value[2]));
+}
+
+static bool vector_is_finite(const double* value, size_t count)
+{
+	if(value == NULL)
+	{
+		return false;
+	}
+	for(size_t i = 0 ; i < count ; i++)
+	{
+		if(!isfinite(value[i]))
+		{
+			return false;
+		}
+	}
+	return true;
+}
+
+static bool quaternion_normalize(double quaternion[4])
+{
+	double norm = sqrt(square(quaternion[0]) + square(quaternion[1]) +
+	                   square(quaternion[2]) + square(quaternion[3]));
+	if(!(norm > DBL_EPSILON) || !isfinite(norm))
+	{
+		return false;
+	}
+	for(unsigned int i = 0 ; i < 4u ; i++)
+	{
+		quaternion[i] /= norm;
+	}
+	return true;
+}
+
+static void quaternion_multiply(const double first[4], const double second[4],
+	                            double output[4])
+{
+	output[0] = first[0] * second[0] - first[1] * second[1] -
+	            first[2] * second[2] - first[3] * second[3];
+	output[1] = first[0] * second[1] + first[1] * second[0] +
+	            first[2] * second[3] - first[3] * second[2];
+	output[2] = first[0] * second[2] - first[1] * second[3] +
+	            first[2] * second[0] + first[3] * second[1];
+	output[3] = first[0] * second[3] + first[1] * second[2] -
+	            first[2] * second[1] + first[3] * second[0];
+}
+
+static void rotation_vector_quaternion(const double rotation_vector[3],
+	                                   double quaternion[4])
+{
+	double angle = vector_norm3(rotation_vector);
+	if(angle < 1.0e-10)
+	{
+		quaternion[0] = 1.0;
+		quaternion[1] = 0.5 * rotation_vector[0];
+		quaternion[2] = 0.5 * rotation_vector[1];
+		quaternion[3] = 0.5 * rotation_vector[2];
+		(void)quaternion_normalize(quaternion);
+		return;
+	}
+
+	double scale = sin(0.5 * angle) / angle;
+	quaternion[0] = cos(0.5 * angle);
+	quaternion[1] = scale * rotation_vector[0];
+	quaternion[2] = scale * rotation_vector[1];
+	quaternion[3] = scale * rotation_vector[2];
+}
+
+static void quaternion_rotation_matrix(const double quaternion[4],
+	                                   double rotation[3][3])
+{
+	const double w = quaternion[0];
+	const double x = quaternion[1];
+	const double y = quaternion[2];
+	const double z = quaternion[3];
+
+	rotation[0][0] = 1.0 - 2.0 * (y * y + z * z);
+	rotation[0][1] = 2.0 * (x * y - w * z);
+	rotation[0][2] = 2.0 * (x * z + w * y);
+	rotation[1][0] = 2.0 * (x * y + w * z);
+	rotation[1][1] = 1.0 - 2.0 * (x * x + z * z);
+	rotation[1][2] = 2.0 * (y * z - w * x);
+	rotation[2][0] = 2.0 * (x * z - w * y);
+	rotation[2][1] = 2.0 * (y * z + w * x);
+	rotation[2][2] = 1.0 - 2.0 * (x * x + y * y);
+}
+
+static void skew_matrix(const double vector[3], double skew[3][3])
+{
+	skew[0][0] = 0.0;
+	skew[0][1] = -vector[2];
+	skew[0][2] = vector[1];
+	skew[1][0] = vector[2];
+	skew[1][1] = 0.0;
+	skew[1][2] = -vector[0];
+	skew[2][0] = -vector[1];
+	skew[2][1] = vector[0];
+	skew[2][2] = 0.0;
+}
+
+static bool inverse_symmetric3(const double input[9], double inverse[9])
+{
+	double lower[3][3] = {{0.0}};
+	double scale = fmax(fabs(input[0]), fmax(fabs(input[4]), fabs(input[8])));
+	if(!(scale > 0.0) || !isfinite(scale))
+	{
+		return false;
+	}
+
+	for(unsigned int row = 0 ; row < 3u ; row++)
+	{
+		for(unsigned int col = 0 ; col <= row ; col++)
+		{
+			double value = 0.5 * (input[row * 3u + col] + input[col * 3u + row]);
+			for(unsigned int k = 0 ; k < col ; k++)
+			{
+				value -= lower[row][k] * lower[col][k];
+			}
+			if(row == col)
+			{
+				if(value <= scale * DBL_EPSILON * 128.0 || !isfinite(value))
+				{
+					return false;
+				}
+				lower[row][col] = sqrt(value);
+			}
+			else
+			{
+				lower[row][col] = value / lower[col][col];
+			}
+		}
+	}
+
+	for(unsigned int column = 0 ; column < 3u ; column++)
+	{
+		double forward[3] = {0.0, 0.0, 0.0};
+		double solution[3] = {0.0, 0.0, 0.0};
+		for(unsigned int row = 0 ; row < 3u ; row++)
+		{
+			double rhs = (row == column) ? 1.0 : 0.0;
+			for(unsigned int k = 0 ; k < row ; k++)
+			{
+				rhs -= lower[row][k] * forward[k];
+			}
+			forward[row] = rhs / lower[row][row];
+		}
+		for(int row = 2 ; row >= 0 ; row--)
+		{
+			double rhs = forward[row];
+			for(unsigned int k = (unsigned int)row + 1u ; k < 3u ; k++)
+			{
+				rhs -= lower[k][row] * solution[k];
+			}
+			solution[row] = rhs / lower[row][row];
+		}
+		for(unsigned int row = 0 ; row < 3u ; row++)
+		{
+			inverse[row * 3u + column] = solution[row];
+		}
+	}
+	return vector_is_finite(inverse, 9u);
+}
+
+static void covariance_symmetrize_and_floor(eskf_t* filter, double floor_value)
+{
+	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
+	{
+		for(unsigned int col = row + 1u ; col < ESKF_ERROR_STATE_DIM ; col++)
+		{
+			double symmetric = 0.5 *
+				(filter->covariance[row][col] + filter->covariance[col][row]);
+			filter->covariance[row][col] = symmetric;
+			filter->covariance[col][row] = symmetric;
+		}
+		if(filter->covariance[row][row] < floor_value)
+		{
+			filter->covariance[row][row] = floor_value;
+		}
+	}
+}
+
+void eskf_default_config(eskf_config_t* config)
+{
+	if(config == NULL)
+	{
+		return;
+	}
+	memset(config, 0, sizeof(*config));
+	for(unsigned int i = 0 ; i < 3u ; i++)
+	{
+		config->accel_noise_density[i] = 0.03;
+		config->gyro_noise_density[i] = 0.003;
+		config->accel_bias_random_walk[i] = 0.002;
+		config->gyro_bias_random_walk[i] = 0.002 * ESKF_PI / 180.0;
+		config->initial_position_std_m[i] = (i < 2u) ? 10.0 : 20.0;
+		config->initial_velocity_std_mps[i] = 1.0;
+		config->initial_attitude_std_rad[i] =
+			((i < 2u) ? 5.0 : 30.0) * ESKF_PI / 180.0;
+		config->initial_accel_bias_std_mps2[i] = 0.1;
+		config->initial_gyro_bias_std_rps[i] = 0.05 * ESKF_PI / 180.0;
+	}
+	config->gravity_mps2 = 9.80665;
+	config->max_propagation_step_s = 0.02;
+	config->position_nis_gate = 16.26623619623813; /* chi-square 3 DOF, 99.9%. */
+	config->covariance_diagonal_floor = 1.0e-12;
+}
+
+bool eskf_quaternion_align_vectors(const double source[3],
+	                               const double target[3],
+	                               double quaternion_out[4])
+{
+	if(!vector_is_finite(source, 3u) || !vector_is_finite(target, 3u) ||
+	   (quaternion_out == NULL))
+	{
+		return false;
+	}
+	double source_norm = vector_norm3(source);
+	double target_norm = vector_norm3(target);
+	if(!(source_norm > DBL_EPSILON) || !(target_norm > DBL_EPSILON))
+	{
+		return false;
+	}
+	double first[3];
+	double second[3];
+	for(unsigned int i = 0 ; i < 3u ; i++)
+	{
+		first[i] = source[i] / source_norm;
+		second[i] = target[i] / target_norm;
+	}
+	double dot = first[0] * second[0] + first[1] * second[1] + first[2] * second[2];
+	if(dot < -0.999999)
+	{
+		double axis[3] = {0.0, first[2], -first[1]};
+		if(vector_norm3(axis) < 1.0e-6)
+		{
+			axis[0] = -first[2];
+			axis[1] = 0.0;
+			axis[2] = first[0];
+		}
+		double axis_norm = vector_norm3(axis);
+		quaternion_out[0] = 0.0;
+		for(unsigned int i = 0 ; i < 3u ; i++)
+		{
+			quaternion_out[i + 1u] = axis[i] / axis_norm;
+		}
+		return true;
+	}
+
+	quaternion_out[0] = 1.0 + dot;
+	quaternion_out[1] = first[1] * second[2] - first[2] * second[1];
+	quaternion_out[2] = first[2] * second[0] - first[0] * second[2];
+	quaternion_out[3] = first[0] * second[1] - first[1] * second[0];
+	return quaternion_normalize(quaternion_out);
+}
+
+bool eskf_initialize(eskf_t* filter,
+	                 const eskf_config_t* config,
+	                 const double initial_position_m[3],
+	                 const double body_to_nav_quaternion[4],
+	                 const double accel_bias_mps2[3],
+	                 const double gyro_bias_rps[3])
+{
+	if((filter == NULL) || (config == NULL) ||
+	   !vector_is_finite(initial_position_m, 3u) ||
+	   !vector_is_finite(body_to_nav_quaternion, 4u) ||
+	   !vector_is_finite(accel_bias_mps2, 3u) ||
+	   !vector_is_finite(gyro_bias_rps, 3u) ||
+	   !(config->gravity_mps2 > 0.0))
+	{
+		return false;
+	}
+
+	memset(filter, 0, sizeof(*filter));
+	memcpy(filter->position_m, initial_position_m, sizeof(filter->position_m));
+	memcpy(filter->body_to_nav_quaternion, body_to_nav_quaternion,
+	       sizeof(filter->body_to_nav_quaternion));
+	memcpy(filter->accel_bias_mps2, accel_bias_mps2,
+	       sizeof(filter->accel_bias_mps2));
+	memcpy(filter->gyro_bias_rps, gyro_bias_rps, sizeof(filter->gyro_bias_rps));
+	if(!quaternion_normalize(filter->body_to_nav_quaternion))
+	{
+		return false;
+	}
+
+	for(unsigned int i = 0 ; i < 3u ; i++)
+	{
+		filter->covariance[ESKF_POSITION_INDEX + i][ESKF_POSITION_INDEX + i] =
+			square(config->initial_position_std_m[i]);
+		filter->covariance[ESKF_VELOCITY_INDEX + i][ESKF_VELOCITY_INDEX + i] =
+			square(config->initial_velocity_std_mps[i]);
+		filter->covariance[ESKF_ATTITUDE_INDEX + i][ESKF_ATTITUDE_INDEX + i] =
+			square(config->initial_attitude_std_rad[i]);
+		filter->covariance[ESKF_ACCEL_BIAS_INDEX + i][ESKF_ACCEL_BIAS_INDEX + i] =
+			square(config->initial_accel_bias_std_mps2[i]);
+		filter->covariance[ESKF_GYRO_BIAS_INDEX + i][ESKF_GYRO_BIAS_INDEX + i] =
+			square(config->initial_gyro_bias_std_rps[i]);
+	}
+	covariance_symmetrize_and_floor(filter, config->covariance_diagonal_floor);
+	filter->last_position_nis = NAN;
+	filter->initialized = true;
+	return true;
+}
+
+static bool eskf_propagate_step(eskf_t* filter,
+	                            const eskf_config_t* config,
+	                            const double accel_mps2[3],
+	                            const double gyro_rps[3],
+	                            double dt_s)
+{
+	double specific_force[3];
+	double angular_rate[3];
+	for(unsigned int i = 0 ; i < 3u ; i++)
+	{
+		specific_force[i] = accel_mps2[i] - filter->accel_bias_mps2[i];
+		angular_rate[i] = gyro_rps[i] - filter->gyro_bias_rps[i];
+		filter->last_unbiased_gyro_rps[i] = angular_rate[i];
+	}
+
+	double rotation[3][3];
+	quaternion_rotation_matrix(filter->body_to_nav_quaternion, rotation);
+	double linear_accel[3] = {0.0, 0.0, -config->gravity_mps2};
+	for(unsigned int row = 0 ; row < 3u ; row++)
+	{
+		for(unsigned int col = 0 ; col < 3u ; col++)
+		{
+			linear_accel[row] += rotation[row][col] * specific_force[col];
+		}
+		filter->last_linear_accel_nav_mps2[row] = linear_accel[row];
+		filter->position_m[row] += filter->velocity_mps[row] * dt_s +
+		                           0.5 * linear_accel[row] * dt_s * dt_s;
+		filter->velocity_mps[row] += linear_accel[row] * dt_s;
+	}
+
+	double rotation_vector[3] = {
+		angular_rate[0] * dt_s,
+		angular_rate[1] * dt_s,
+		angular_rate[2] * dt_s
+	};
+	double delta_quaternion[4];
+	double updated_quaternion[4];
+	rotation_vector_quaternion(rotation_vector, delta_quaternion);
+	quaternion_multiply(filter->body_to_nav_quaternion, delta_quaternion,
+	                    updated_quaternion);
+	memcpy(filter->body_to_nav_quaternion, updated_quaternion,
+	       sizeof(updated_quaternion));
+	if(!quaternion_normalize(filter->body_to_nav_quaternion))
+	{
+		return false;
+	}
+
+	double system[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
+	for(unsigned int i = 0 ; i < 3u ; i++)
+	{
+		system[ESKF_POSITION_INDEX + i][ESKF_VELOCITY_INDEX + i] = 1.0;
+		system[ESKF_ATTITUDE_INDEX + i][ESKF_GYRO_BIAS_INDEX + i] = -1.0;
+	}
+	double force_skew[3][3];
+	double rate_skew[3][3];
+	skew_matrix(specific_force, force_skew);
+	skew_matrix(angular_rate, rate_skew);
+	for(unsigned int row = 0 ; row < 3u ; row++)
+	{
+		for(unsigned int col = 0 ; col < 3u ; col++)
+		{
+			double rotated_skew = 0.0;
+			for(unsigned int k = 0 ; k < 3u ; k++)
+			{
+				rotated_skew += rotation[row][k] * force_skew[k][col];
+			}
+			system[ESKF_VELOCITY_INDEX + row][ESKF_ATTITUDE_INDEX + col] =
+				-rotated_skew;
+			system[ESKF_VELOCITY_INDEX + row][ESKF_ACCEL_BIAS_INDEX + col] =
+				-rotation[row][col];
+			system[ESKF_ATTITUDE_INDEX + row][ESKF_ATTITUDE_INDEX + col] =
+				-rate_skew[row][col];
+		}
+	}
+
+	double transition[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
+	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
+	{
+		for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
+		{
+			transition[row][col] = system[row][col] * dt_s;
+		}
+		transition[row][row] += 1.0;
+	}
+
+	double intermediate[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
+	double propagated[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
+	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
+	{
+		for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
+		{
+			for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
+			{
+				intermediate[row][col] +=
+					transition[row][k] * filter->covariance[k][col];
+			}
+		}
+	}
+	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
+	{
+		for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
+		{
+			for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
+			{
+				propagated[row][col] += intermediate[row][k] * transition[col][k];
+			}
+		}
+	}
+
+	for(unsigned int row = 0 ; row < 3u ; row++)
+	{
+		for(unsigned int col = 0 ; col < 3u ; col++)
+		{
+			double accel_noise = 0.0;
+			for(unsigned int axis = 0 ; axis < 3u ; axis++)
+			{
+				accel_noise += rotation[row][axis] *
+				               square(config->accel_noise_density[axis]) *
+				               rotation[col][axis];
+			}
+			propagated[ESKF_VELOCITY_INDEX + row][ESKF_VELOCITY_INDEX + col] +=
+				accel_noise * dt_s;
+		}
+		propagated[ESKF_ATTITUDE_INDEX + row][ESKF_ATTITUDE_INDEX + row] +=
+			square(config->gyro_noise_density[row]) * dt_s;
+		propagated[ESKF_ACCEL_BIAS_INDEX + row][ESKF_ACCEL_BIAS_INDEX + row] +=
+			square(config->accel_bias_random_walk[row]) * dt_s;
+		propagated[ESKF_GYRO_BIAS_INDEX + row][ESKF_GYRO_BIAS_INDEX + row] +=
+			square(config->gyro_bias_random_walk[row]) * dt_s;
+	}
+	memcpy(filter->covariance, propagated, sizeof(propagated));
+	covariance_symmetrize_and_floor(filter, config->covariance_diagonal_floor);
+	filter->propagation_count++;
+	return eskf_state_is_finite(filter);
+}
+
+bool eskf_propagate(eskf_t* filter,
+	                const eskf_config_t* config,
+	                const double accel_mps2[3],
+	                const double gyro_rps[3],
+	                double dt_s)
+{
+	if((filter == NULL) || !filter->initialized || (config == NULL) ||
+	   !vector_is_finite(accel_mps2, 3u) || !vector_is_finite(gyro_rps, 3u) ||
+	   !isfinite(dt_s) || (dt_s < 0.0) ||
+	   !(config->max_propagation_step_s > 0.0))
+	{
+		return false;
+	}
+	if(dt_s == 0.0)
+	{
+		return true;
+	}
+	unsigned int steps = (unsigned int)ceil(dt_s / config->max_propagation_step_s);
+	if((steps == 0u) || (steps > 10000u))
+	{
+		return false;
+	}
+	double step_s = dt_s / (double)steps;
+	for(unsigned int step = 0 ; step < steps ; step++)
+	{
+		if(!eskf_propagate_step(filter, config, accel_mps2, gyro_rps, step_s))
+		{
+			return false;
+		}
+	}
+	return true;
+}
+
+static bool eskf_inject_error(eskf_t* filter, const double error[ESKF_ERROR_STATE_DIM])
+{
+	for(unsigned int i = 0 ; i < 3u ; i++)
+	{
+		filter->position_m[i] += error[ESKF_POSITION_INDEX + i];
+		filter->velocity_mps[i] += error[ESKF_VELOCITY_INDEX + i];
+		filter->accel_bias_mps2[i] += error[ESKF_ACCEL_BIAS_INDEX + i];
+		filter->gyro_bias_rps[i] += error[ESKF_GYRO_BIAS_INDEX + i];
+	}
+	double attitude_error[3] = {
+		error[ESKF_ATTITUDE_INDEX],
+		error[ESKF_ATTITUDE_INDEX + 1u],
+		error[ESKF_ATTITUDE_INDEX + 2u]
+	};
+	double delta_quaternion[4];
+	double updated_quaternion[4];
+	rotation_vector_quaternion(attitude_error, delta_quaternion);
+	quaternion_multiply(filter->body_to_nav_quaternion, delta_quaternion,
+	                    updated_quaternion);
+	memcpy(filter->body_to_nav_quaternion, updated_quaternion,
+	       sizeof(updated_quaternion));
+	if(!quaternion_normalize(filter->body_to_nav_quaternion))
+	{
+		return false;
+	}
+
+	/* Right attitude-error reset after q <- q (x) Exp(delta_theta). */
+	double reset[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
+	for(unsigned int i = 0 ; i < ESKF_ERROR_STATE_DIM ; i++)
+	{
+		reset[i][i] = 1.0;
+	}
+	double attitude_skew[3][3];
+	skew_matrix(attitude_error, attitude_skew);
+	for(unsigned int row = 0 ; row < 3u ; row++)
+	{
+		for(unsigned int col = 0 ; col < 3u ; col++)
+		{
+			reset[ESKF_ATTITUDE_INDEX + row][ESKF_ATTITUDE_INDEX + col] -=
+				0.5 * attitude_skew[row][col];
+		}
+	}
+	double intermediate[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
+	double reset_covariance[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
+	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
+	{
+		for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
+		{
+			for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
+			{
+				intermediate[row][col] += reset[row][k] * filter->covariance[k][col];
+			}
+		}
+	}
+	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
+	{
+		for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
+		{
+			for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
+			{
+				reset_covariance[row][col] += intermediate[row][k] * reset[col][k];
+			}
+		}
+	}
+	memcpy(filter->covariance, reset_covariance, sizeof(reset_covariance));
+	return true;
+}
+
+static bool eskf_selected_update(eskf_t* filter,
+	                             unsigned int selected_state_index,
+	                             const double residual[3],
+	                             const double measurement_covariance[9],
+	                             bool gate_enabled,
+	                             double gate,
+	                             double* nis_out)
+{
+	if((selected_state_index + 3u) > ESKF_ERROR_STATE_DIM ||
+	   !vector_is_finite(residual, 3u) ||
+	   !vector_is_finite(measurement_covariance, 9u))
+	{
+		return false;
+	}
+	double covariance_inverse[9];
+	if(!inverse_symmetric3(measurement_covariance, covariance_inverse))
+	{
+		return false;
+	}
+	double innovation[9];
+	for(unsigned int row = 0 ; row < 3u ; row++)
+	{
+		for(unsigned int col = 0 ; col < 3u ; col++)
+		{
+			innovation[row * 3u + col] =
+				filter->covariance[selected_state_index + row][selected_state_index + col] +
+				measurement_covariance[row * 3u + col];
+		}
+	}
+	double innovation_inverse[9];
+	if(!inverse_symmetric3(innovation, innovation_inverse))
+	{
+		return false;
+	}
+	double nis = 0.0;
+	for(unsigned int row = 0 ; row < 3u ; row++)
+	{
+		for(unsigned int col = 0 ; col < 3u ; col++)
+		{
+			nis += residual[row] * innovation_inverse[row * 3u + col] * residual[col];
+		}
+	}
+	if(nis_out != NULL)
+	{
+		*nis_out = nis;
+	}
+	if(!isfinite(nis) || (gate_enabled && (nis > gate)))
+	{
+		return false;
+	}
+
+	double gain[ESKF_ERROR_STATE_DIM][3] = {{0.0}};
+	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
+	{
+		for(unsigned int col = 0 ; col < 3u ; col++)
+		{
+			for(unsigned int k = 0 ; k < 3u ; k++)
+			{
+				gain[row][col] +=
+					filter->covariance[row][selected_state_index + k] *
+					innovation_inverse[k * 3u + col];
+			}
+		}
+	}
+	double error[ESKF_ERROR_STATE_DIM] = {0.0};
+	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
+	{
+		for(unsigned int col = 0 ; col < 3u ; col++)
+		{
+			error[row] += gain[row][col] * residual[col];
+		}
+	}
+
+	double joseph_left[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
+	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
+	{
+		joseph_left[row][row] = 1.0;
+		for(unsigned int col = 0 ; col < 3u ; col++)
+		{
+			joseph_left[row][selected_state_index + col] -= gain[row][col];
+		}
+	}
+	double intermediate[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
+	double updated_covariance[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
+	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
+	{
+		for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
+		{
+			for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
+			{
+				intermediate[row][col] +=
+					joseph_left[row][k] * filter->covariance[k][col];
+			}
+		}
+	}
+	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
+	{
+		for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
+		{
+			for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
+			{
+				updated_covariance[row][col] +=
+					intermediate[row][k] * joseph_left[col][k];
+			}
+			for(unsigned int first = 0 ; first < 3u ; first++)
+			{
+				for(unsigned int second = 0 ; second < 3u ; second++)
+				{
+					updated_covariance[row][col] += gain[row][first] *
+						measurement_covariance[first * 3u + second] * gain[col][second];
+				}
+			}
+		}
+	}
+	memcpy(filter->covariance, updated_covariance, sizeof(updated_covariance));
+	if(!eskf_inject_error(filter, error))
+	{
+		return false;
+	}
+	covariance_symmetrize_and_floor(filter, 1.0e-12);
+	return eskf_state_is_finite(filter);
+}
+
+eskf_update_result_t eskf_position_update(
+	eskf_t* filter,
+	const eskf_config_t* config,
+	const double position_m[3],
+	const double covariance_m2[9],
+	double* nis_out)
+{
+	if(nis_out != NULL)
+	{
+		*nis_out = NAN;
+	}
+	if((filter == NULL) || !filter->initialized || (config == NULL) ||
+	   !vector_is_finite(position_m, 3u) || !vector_is_finite(covariance_m2, 9u))
+	{
+		return ESKF_UPDATE_INVALID;
+	}
+	double residual[3];
+	for(unsigned int i = 0 ; i < 3u ; i++)
+	{
+		residual[i] = position_m[i] - filter->position_m[i];
+	}
+	double nis = NAN;
+	if(!eskf_selected_update(filter, ESKF_POSITION_INDEX, residual, covariance_m2,
+	                         true, config->position_nis_gate, &nis))
+	{
+		filter->last_position_nis = nis;
+		if(isfinite(nis) && (nis > config->position_nis_gate))
+		{
+			filter->position_update_rejected_count++;
+			if(nis_out != NULL)
+			{
+				*nis_out = nis;
+			}
+			return ESKF_UPDATE_REJECTED_NIS;
+		}
+		return ESKF_UPDATE_INVALID;
+	}
+	filter->last_position_nis = nis;
+	filter->position_update_accepted_count++;
+	if(nis_out != NULL)
+	{
+		*nis_out = nis;
+	}
+	return ESKF_UPDATE_ACCEPTED;
+}
+
+bool eskf_zero_velocity_update(eskf_t* filter, double velocity_std_mps)
+{
+	if((filter == NULL) || !filter->initialized ||
+	   !(velocity_std_mps > 0.0) || !isfinite(velocity_std_mps))
+	{
+		return false;
+	}
+	double residual[3] = {
+		-filter->velocity_mps[0],
+		-filter->velocity_mps[1],
+		-filter->velocity_mps[2]
+	};
+	double covariance[9] = {0.0};
+	for(unsigned int i = 0 ; i < 3u ; i++)
+	{
+		covariance[i * 3u + i] = square(velocity_std_mps);
+	}
+	return eskf_selected_update(filter, ESKF_VELOCITY_INDEX, residual, covariance,
+	                            false, 0.0, NULL);
+}
+
+bool eskf_zero_angular_rate_update(
+	eskf_t* filter,
+	const double measured_gyro_rps[3],
+	const double measurement_std_rps[3])
+{
+	if((filter == NULL) || !filter->initialized ||
+	   !vector_is_finite(measured_gyro_rps, 3u) ||
+	   !vector_is_finite(measurement_std_rps, 3u))
+	{
+		return false;
+	}
+	double residual[3];
+	double covariance[9] = {0.0};
+	for(unsigned int i = 0 ; i < 3u ; i++)
+	{
+		if(!(measurement_std_rps[i] > 0.0))
+		{
+			return false;
+		}
+		residual[i] = measured_gyro_rps[i] - filter->gyro_bias_rps[i];
+		covariance[i * 3u + i] = square(measurement_std_rps[i]);
+	}
+	return eskf_selected_update(filter, ESKF_GYRO_BIAS_INDEX, residual, covariance,
+	                            false, 0.0, NULL);
+}
+
+double eskf_speed_mps(const eskf_t* filter)
+{
+	if((filter == NULL) || !filter->initialized)
+	{
+		return NAN;
+	}
+	return vector_norm3(filter->velocity_mps);
+}
+
+bool eskf_state_is_finite(const eskf_t* filter)
+{
+	if((filter == NULL) || !filter->initialized ||
+	   !vector_is_finite(filter->position_m, 3u) ||
+	   !vector_is_finite(filter->velocity_mps, 3u) ||
+	   !vector_is_finite(filter->body_to_nav_quaternion, 4u) ||
+	   !vector_is_finite(filter->accel_bias_mps2, 3u) ||
+	   !vector_is_finite(filter->gyro_bias_rps, 3u) ||
+	   !vector_is_finite(&filter->covariance[0][0],
+	                     ESKF_ERROR_STATE_DIM * ESKF_ERROR_STATE_DIM))
+	{
+		return false;
+	}
+	for(unsigned int i = 0 ; i < ESKF_ERROR_STATE_DIM ; i++)
+	{
+		if(filter->covariance[i][i] < 0.0)
+		{
+			return false;
+		}
+	}
+	return true;
+}

soc/software/gps_system/src/fusion_bus.c

@@ -0,0 +1,102 @@
+#include "fusion_bus.h"
+
+#include <stddef.h>
+#include <string.h>
+
+#if (FUSION_IMU_QUEUE_CAPACITY < 2u)
+#error "FUSION_IMU_QUEUE_CAPACITY must be at least 2"
+#endif
+
+#if (FUSION_GPS_QUEUE_CAPACITY < 2u)
+#error "FUSION_GPS_QUEUE_CAPACITY must be at least 2"
+#endif
+
+static unsigned int next_index(unsigned int index, unsigned int capacity)
+{
+	index++;
+	return (index == capacity) ? 0u : index;
+}
+
+void fusion_bus_init(fusion_bus_t* bus)
+{
+	if(bus != NULL)
+	{
+		memset(bus, 0, sizeof(*bus));
+	}
+}
+
+bool fusion_bus_publish_imu(fusion_bus_t* bus,
+	                        const fusion_imu_measurement_t* measurement)
+{
+	if((bus == NULL) || (measurement == NULL))
+	{
+		return false;
+	}
+	unsigned int head = __atomic_load_n(&bus->imu_head, __ATOMIC_RELAXED);
+	unsigned int next = next_index(head, FUSION_IMU_QUEUE_CAPACITY);
+	if(next == __atomic_load_n(&bus->imu_tail, __ATOMIC_ACQUIRE))
+	{
+		__atomic_fetch_add(&bus->imu_overflow_count, 1u, __ATOMIC_RELAXED);
+		return false;
+	}
+	bus->imu_queue[head] = *measurement;
+	__atomic_store_n(&bus->imu_head, next, __ATOMIC_RELEASE);
+	return true;
+}
+
+bool fusion_bus_take_imu(fusion_bus_t* bus,
+	                     fusion_imu_measurement_t* measurement_out)
+{
+	if((bus == NULL) || (measurement_out == NULL))
+	{
+		return false;
+	}
+	unsigned int tail = __atomic_load_n(&bus->imu_tail, __ATOMIC_RELAXED);
+	if(tail == __atomic_load_n(&bus->imu_head, __ATOMIC_ACQUIRE))
+	{
+		return false;
+	}
+	*measurement_out = bus->imu_queue[tail];
+	__atomic_store_n(&bus->imu_tail,
+	                 next_index(tail, FUSION_IMU_QUEUE_CAPACITY),
+	                 __ATOMIC_RELEASE);
+	return true;
+}
+
+bool fusion_bus_publish_gps(fusion_bus_t* bus,
+	                        const fusion_gps_measurement_t* measurement)
+{
+	if((bus == NULL) || (measurement == NULL))
+	{
+		return false;
+	}
+	unsigned int head = __atomic_load_n(&bus->gps_head, __ATOMIC_RELAXED);
+	unsigned int next = next_index(head, FUSION_GPS_QUEUE_CAPACITY);
+	if(next == __atomic_load_n(&bus->gps_tail, __ATOMIC_ACQUIRE))
+	{
+		__atomic_fetch_add(&bus->gps_overflow_count, 1u, __ATOMIC_RELAXED);
+		return false;
+	}
+	bus->gps_queue[head] = *measurement;
+	__atomic_store_n(&bus->gps_head, next, __ATOMIC_RELEASE);
+	return true;
+}
+
+bool fusion_bus_take_gps(fusion_bus_t* bus,
+	                     fusion_gps_measurement_t* measurement_out)
+{
+	if((bus == NULL) || (measurement_out == NULL))
+	{
+		return false;
+	}
+	unsigned int tail = __atomic_load_n(&bus->gps_tail, __ATOMIC_RELAXED);
+	if(tail == __atomic_load_n(&bus->gps_head, __ATOMIC_ACQUIRE))
+	{
+		return false;
+	}
+	*measurement_out = bus->gps_queue[tail];
+	__atomic_store_n(&bus->gps_tail,
+	                 next_index(tail, FUSION_GPS_QUEUE_CAPACITY),
+	                 __ATOMIC_RELEASE);
+	return true;
+}

soc/software/gps_system/src/fusion_engine.c

@@ -0,0 +1,730 @@
+#include "fusion_engine.h"
+
+#include <float.h>
+#include <math.h>
+#include <stddef.h>
+#include <string.h>
+
+#define FUSION_PI 3.14159265358979323846264338327950288
+
+static double square(double value)
+{
+	return value * value;
+}
+
+static double norm3(const double value[3])
+{
+	return sqrt(square(value[0]) + square(value[1]) + square(value[2]));
+}
+
+static bool finite_values(const double* values, unsigned int count)
+{
+	if(values == NULL)
+	{
+		return false;
+	}
+	for(unsigned int i = 0 ; i < count ; i++)
+	{
+		if(!isfinite(values[i]))
+		{
+			return false;
+		}
+	}
+	return true;
+}
+
+static double ticks_to_seconds(const fusion_engine_t* engine, uint64_t ticks)
+{
+	return (double)ticks / (double)engine->config.timebase_hz;
+}
+
+static uint64_t seconds_to_ticks(const fusion_engine_t* engine, double seconds)
+{
+	double ticks = seconds * (double)engine->config.timebase_hz;
+	if(ticks <= 1.0)
+	{
+		return 1u;
+	}
+	return (uint64_t)(ticks + 0.5);
+}
+
+void fusion_engine_default_config(fusion_engine_config_t* config,
+	                              uint64_t timebase_hz)
+{
+	if(config == NULL)
+	{
+		return;
+	}
+	memset(config, 0, sizeof(*config));
+	eskf_default_config(&config->eskf);
+	fusion_motion_default_config(&config->motion);
+	config->timebase_hz = timebase_hz;
+	config->calibration_duration_s = 2.0;
+	config->calibration_min_samples = 50u;
+	config->calibration_accel_norm_tolerance_mps2 = 0.30;
+	config->calibration_gyro_limit_rps = 2.0 * FUSION_PI / 180.0;
+	config->calibration_accel_std_limit_mps2 = 0.12;
+	config->calibration_gyro_std_limit_rps = 0.15 * FUSION_PI / 180.0;
+	config->zero_update_period_s = 0.05;
+	config->zero_velocity_std_mps = 0.03;
+	for(unsigned int i = 0 ; i < 3u ; i++)
+	{
+		config->zero_gyro_std_rps[i] = 0.01 * FUSION_PI / 180.0;
+	}
+	config->gnss_motion_speed_threshold_mps = 0.50;
+	config->gnss_displacement_nis_gate = 9.21034037197618; /* 2 DOF, 99%. */
+	config->gnss_moving_confirmations = 2u;
+	config->gnss_still_confirmations = 2u;
+}
+
+static bool config_valid(const fusion_engine_config_t* config)
+{
+	return (config != NULL) && (config->timebase_hz != 0u) &&
+	       isfinite(config->calibration_duration_s) &&
+	       (config->calibration_duration_s > 0.0) &&
+	       (config->calibration_min_samples >= 2u) &&
+	       isfinite(config->calibration_accel_norm_tolerance_mps2) &&
+	       (config->calibration_accel_norm_tolerance_mps2 > 0.0) &&
+	       isfinite(config->calibration_gyro_limit_rps) &&
+	       (config->calibration_gyro_limit_rps > 0.0) &&
+	       isfinite(config->zero_update_period_s) &&
+	       (config->zero_update_period_s > 0.0) &&
+	       isfinite(config->zero_velocity_std_mps) &&
+	       (config->zero_velocity_std_mps > 0.0) &&
+	       finite_values(config->zero_gyro_std_rps, 3u) &&
+	       (config->gnss_moving_confirmations != 0u) &&
+	       (config->gnss_still_confirmations != 0u);
+}
+
+bool fusion_engine_init(fusion_engine_t* engine,
+	                    const fusion_engine_config_t* config)
+{
+	if((engine == NULL) || !config_valid(config))
+	{
+		return false;
+	}
+	memset(engine, 0, sizeof(*engine));
+	engine->config = *config;
+	engine->state = FUSION_ENGINE_CALIBRATING;
+	return true;
+}
+
+static bool imu_valid(const fusion_imu_measurement_t* measurement)
+{
+	return (measurement != NULL) &&
+	       finite_values(measurement->accel_mps2, 3u) &&
+	       finite_values(measurement->gyro_rps, 3u) &&
+	       isfinite(measurement->temperature_c);
+}
+
+static bool position_valid(const fusion_position_measurement_t* measurement)
+{
+	return (measurement != NULL) &&
+	       finite_values(measurement->position_enu_m, 3u) &&
+	       finite_values(measurement->covariance_enu_m2, 9u) &&
+	       isfinite(measurement->source_quality);
+}
+
+static void calibration_reset(fusion_engine_t* engine)
+{
+	memset(&engine->calibration, 0, sizeof(engine->calibration));
+	engine->diagnostics.calibration_reset_count++;
+}
+
+static void calibration_add(fusion_calibration_accumulator_t* calibration,
+	                        const fusion_imu_measurement_t* measurement)
+{
+	calibration->sample_count++;
+	double count = (double)calibration->sample_count;
+	for(unsigned int axis = 0 ; axis < 3u ; axis++)
+	{
+		double accel_delta = measurement->accel_mps2[axis] - calibration->accel_mean[axis];
+		calibration->accel_mean[axis] += accel_delta / count;
+		calibration->accel_m2[axis] +=
+			accel_delta * (measurement->accel_mps2[axis] - calibration->accel_mean[axis]);
+		double gyro_delta = measurement->gyro_rps[axis] - calibration->gyro_mean[axis];
+		calibration->gyro_mean[axis] += gyro_delta / count;
+		calibration->gyro_m2[axis] +=
+			gyro_delta * (measurement->gyro_rps[axis] - calibration->gyro_mean[axis]);
+	}
+}
+
+static bool calibration_sample_is_quiet(const fusion_engine_t* engine,
+	                                    const fusion_imu_measurement_t* measurement)
+{
+	double accel_error = fabs(norm3(measurement->accel_mps2) - engine->config.eskf.gravity_mps2);
+	return (accel_error <= engine->config.calibration_accel_norm_tolerance_mps2) &&
+	       (norm3(measurement->gyro_rps) <= engine->config.calibration_gyro_limit_rps);
+}
+
+static bool calibration_statistics_are_quiet(const fusion_engine_t* engine)
+{
+	const fusion_calibration_accumulator_t* calibration = &engine->calibration;
+	if(calibration->sample_count < 2u)
+	{
+		return false;
+	}
+	double denominator = (double)(calibration->sample_count - 1u);
+	for(unsigned int axis = 0 ; axis < 3u ; axis++)
+	{
+		double accel_std = sqrt(fmax(0.0, calibration->accel_m2[axis] / denominator));
+		double gyro_std = sqrt(fmax(0.0, calibration->gyro_m2[axis] / denominator));
+		if((accel_std > engine->config.calibration_accel_std_limit_mps2) ||
+		   (gyro_std > engine->config.calibration_gyro_std_limit_rps))
+		{
+			return false;
+		}
+	}
+	return true;
+}
+
+static void history_clear(fusion_engine_t* engine)
+{
+	engine->imu_history_head = 0u;
+	engine->imu_history_count = 0u;
+}
+
+static const fusion_imu_measurement_t* history_at(const fusion_engine_t* engine,
+	                                              unsigned int offset)
+{
+	unsigned int index = (engine->imu_history_head + offset) % FUSION_IMU_HISTORY_CAPACITY;
+	return &engine->imu_history[index];
+}
+
+static bool runtime_apply_imu(fusion_engine_t* engine,
+	                          fusion_runtime_t* runtime,
+	                          const fusion_imu_measurement_t* measurement,
+	                          bool count_zero_updates);
+
+static void history_push(fusion_engine_t* engine,
+	                     const fusion_imu_measurement_t* measurement)
+{
+	if(engine->imu_history_count == FUSION_IMU_HISTORY_CAPACITY)
+	{
+		const fusion_imu_measurement_t oldest = *history_at(engine, 0u);
+		if(engine->checkpoint_valid &&
+		   (oldest.timestamp_ticks > engine->checkpoint.filter_timestamp_ticks))
+		{
+			if(!runtime_apply_imu(engine, &engine->checkpoint, &oldest, false))
+			{
+				engine->state = FUSION_ENGINE_FAULT;
+				return;
+			}
+			engine->diagnostics.checkpoint_advance_count++;
+		}
+		engine->imu_history_head =
+			(engine->imu_history_head + 1u) % FUSION_IMU_HISTORY_CAPACITY;
+		engine->imu_history_count--;
+		engine->diagnostics.imu_history_overflow++;
+	}
+	unsigned int index = (engine->imu_history_head + engine->imu_history_count) %
+	                     FUSION_IMU_HISTORY_CAPACITY;
+	engine->imu_history[index] = *measurement;
+	engine->imu_history_count++;
+}
+
+static void history_drop_through(fusion_engine_t* engine, uint64_t timestamp_ticks)
+{
+	while((engine->imu_history_count != 0u) &&
+	      (history_at(engine, 0u)->timestamp_ticks <= timestamp_ticks))
+	{
+		engine->imu_history_head =
+			(engine->imu_history_head + 1u) % FUSION_IMU_HISTORY_CAPACITY;
+		engine->imu_history_count--;
+	}
+}
+
+static bool runtime_propagate_to(fusion_engine_t* engine,
+	                             fusion_runtime_t* runtime,
+	                             uint64_t timestamp_ticks,
+	                             const fusion_imu_measurement_t* input)
+{
+	if((runtime == NULL) || (input == NULL) || !runtime->filter.initialized ||
+	   (timestamp_ticks < runtime->filter_timestamp_ticks))
+	{
+		return false;
+	}
+	if(timestamp_ticks == runtime->filter_timestamp_ticks)
+	{
+		return true;
+	}
+	double dt_s = ticks_to_seconds(engine,
+	                              timestamp_ticks - runtime->filter_timestamp_ticks);
+	if(!eskf_propagate(&runtime->filter, &engine->config.eskf,
+	                   input->accel_mps2, input->gyro_rps, dt_s))
+	{
+		return false;
+	}
+	runtime->filter_timestamp_ticks = timestamp_ticks;
+	return true;
+}
+
+static bool runtime_apply_imu(fusion_engine_t* engine,
+	                          fusion_runtime_t* runtime,
+	                          const fusion_imu_measurement_t* measurement,
+	                          bool count_zero_updates)
+{
+	if(!runtime->have_held_imu)
+	{
+		runtime->held_imu = *measurement;
+		runtime->have_held_imu = true;
+		runtime->filter_timestamp_ticks = measurement->timestamp_ticks;
+		return true;
+	}
+	if((measurement->timestamp_ticks <= runtime->held_imu.timestamp_ticks) ||
+	   (measurement->timestamp_ticks < runtime->filter_timestamp_ticks))
+	{
+		return false;
+	}
+	double motion_dt_s = ticks_to_seconds(
+		engine, measurement->timestamp_ticks - runtime->held_imu.timestamp_ticks);
+	if(!runtime_propagate_to(engine, runtime, measurement->timestamp_ticks, measurement))
+	{
+		return false;
+	}
+	(void)fusion_motion_update(
+		&runtime->motion, &engine->config.motion,
+		runtime->filter.last_linear_accel_nav_mps2,
+		runtime->filter.last_unbiased_gyro_rps,
+		eskf_speed_mps(&runtime->filter), runtime->gnss_evidence, motion_dt_s);
+	bool inertial_motion =
+		(runtime->motion.last_linear_accel_norm_mps2 >=
+		 engine->config.motion.moving_accel_threshold_mps2) ||
+		(runtime->motion.last_gyro_norm_rps >=
+		 engine->config.motion.moving_gyro_threshold_rps) ||
+		(runtime->motion.last_jerk_norm_mps3 >=
+		 engine->config.motion.moving_jerk_threshold_mps3);
+	if(inertial_motion &&
+	   (runtime->gnss_evidence == FUSION_GNSS_MOTION_STILL))
+	{
+		/* A new IMU motion onset makes an older GNSS-still decision stale. */
+		runtime->gnss_evidence = FUSION_GNSS_MOTION_UNKNOWN;
+		runtime->gnss_moving_count = 0u;
+		runtime->gnss_still_count = 0u;
+	}
+	runtime->held_imu = *measurement;
+
+	uint64_t zero_period_ticks =
+		seconds_to_ticks(engine, engine->config.zero_update_period_s);
+	if(fusion_motion_allows_zero_update(&runtime->motion) &&
+	   ((measurement->timestamp_ticks - runtime->last_zero_update_ticks) >=
+	    zero_period_ticks))
+	{
+		if(!eskf_zero_velocity_update(&runtime->filter,
+		                              engine->config.zero_velocity_std_mps) ||
+		   !eskf_zero_angular_rate_update(&runtime->filter,
+		                                  measurement->gyro_rps,
+		                                  engine->config.zero_gyro_std_rps))
+		{
+			return false;
+		}
+		runtime->last_zero_update_ticks = measurement->timestamp_ticks;
+		if(count_zero_updates)
+		{
+			engine->diagnostics.zero_velocity_update_count++;
+		}
+	}
+	return true;
+}
+
+static bool finish_calibration(fusion_engine_t* engine,
+	                           const fusion_imu_measurement_t* last_measurement)
+{
+	if(!calibration_statistics_are_quiet(engine))
+	{
+		calibration_reset(engine);
+		return false;
+	}
+	const fusion_calibration_accumulator_t* calibration = &engine->calibration;
+	double gravity_norm = norm3(calibration->accel_mean);
+	if(!(gravity_norm > DBL_EPSILON))
+	{
+		calibration_reset(engine);
+		return false;
+	}
+	double nav_up[3] = {0.0, 0.0, 1.0};
+	double quaternion[4];
+	if(!eskf_quaternion_align_vectors(calibration->accel_mean, nav_up, quaternion))
+	{
+		engine->state = FUSION_ENGINE_FAULT;
+		return false;
+	}
+	double accel_bias[3];
+	double gyro_bias[3];
+	for(unsigned int axis = 0 ; axis < 3u ; axis++)
+	{
+		double expected_gravity = calibration->accel_mean[axis] *
+		                          engine->config.eskf.gravity_mps2 / gravity_norm;
+		accel_bias[axis] = calibration->accel_mean[axis] - expected_gravity;
+		gyro_bias[axis] = calibration->gyro_mean[axis];
+	}
+	double initial_position[3] = {0.0, 0.0, 0.0};
+	if(engine->have_initial_position)
+	{
+		memcpy(initial_position, engine->initial_position.position_enu_m,
+		       sizeof(initial_position));
+	}
+	if(!eskf_initialize(&engine->current.filter, &engine->config.eskf,
+	                    initial_position, quaternion, accel_bias, gyro_bias))
+	{
+		engine->state = FUSION_ENGINE_FAULT;
+		return false;
+	}
+	fusion_motion_init(&engine->current.motion, true);
+	engine->current.held_imu = *last_measurement;
+	engine->current.have_held_imu = true;
+	engine->current.filter_timestamp_ticks = last_measurement->timestamp_ticks;
+	engine->current.last_zero_update_ticks = last_measurement->timestamp_ticks;
+	engine->current.gnss_evidence = FUSION_GNSS_MOTION_UNKNOWN;
+	if(engine->have_initial_position)
+	{
+		engine->current.have_accepted_position = true;
+		engine->current.last_accepted_position_ticks =
+			engine->initial_position.timestamp_ticks;
+		memcpy(engine->current.last_accepted_position_m,
+		       engine->initial_position.position_enu_m,
+		       sizeof(engine->current.last_accepted_position_m));
+		memcpy(engine->current.last_accepted_position_covariance_m2,
+		       engine->initial_position.covariance_enu_m2,
+		       sizeof(engine->current.last_accepted_position_covariance_m2));
+	}
+	engine->checkpoint = engine->current;
+	engine->checkpoint_valid = true;
+	history_clear(engine);
+	engine->state = FUSION_ENGINE_RUNNING;
+	return true;
+}
+
+static bool process_calibration_imu(fusion_engine_t* engine,
+	                                const fusion_imu_measurement_t* measurement)
+{
+	if(engine->calibration.have_start &&
+	   (measurement->timestamp_ticks <= engine->calibration.last_ticks))
+	{
+		return false;
+	}
+	if(!calibration_sample_is_quiet(engine, measurement))
+	{
+		calibration_reset(engine);
+		return true;
+	}
+	if(!engine->calibration.have_start)
+	{
+		engine->calibration.start_ticks = measurement->timestamp_ticks;
+		engine->calibration.have_start = true;
+	}
+	engine->calibration.last_ticks = measurement->timestamp_ticks;
+	calibration_add(&engine->calibration, measurement);
+	double duration_s = ticks_to_seconds(
+		engine, engine->calibration.last_ticks - engine->calibration.start_ticks);
+	if((engine->calibration.sample_count >= engine->config.calibration_min_samples) &&
+	   (duration_s >= engine->config.calibration_duration_s))
+	{
+		(void)finish_calibration(engine, measurement);
+	}
+	return true;
+}
+
+static fusion_gnss_motion_evidence_t position_motion_evidence(
+	const fusion_engine_t* engine,
+	const fusion_runtime_t* runtime,
+	const fusion_position_measurement_t* measurement)
+{
+	if(!runtime->have_accepted_position ||
+	   (measurement->timestamp_ticks <= runtime->last_accepted_position_ticks))
+	{
+		return FUSION_GNSS_MOTION_UNKNOWN;
+	}
+	double dt_s = ticks_to_seconds(
+		engine, measurement->timestamp_ticks - runtime->last_accepted_position_ticks);
+	if(!(dt_s > 0.0))
+	{
+		return FUSION_GNSS_MOTION_UNKNOWN;
+	}
+	double delta_e = measurement->position_enu_m[0] - runtime->last_accepted_position_m[0];
+	double delta_n = measurement->position_enu_m[1] - runtime->last_accepted_position_m[1];
+	double s00 = measurement->covariance_enu_m2[0] +
+	             runtime->last_accepted_position_covariance_m2[0];
+	double s01 = measurement->covariance_enu_m2[1] +
+	             runtime->last_accepted_position_covariance_m2[1];
+	double s10 = measurement->covariance_enu_m2[3] +
+	             runtime->last_accepted_position_covariance_m2[3];
+	double s11 = measurement->covariance_enu_m2[4] +
+	             runtime->last_accepted_position_covariance_m2[4];
+	double off_diagonal = 0.5 * (s01 + s10);
+	double determinant = s00 * s11 - off_diagonal * off_diagonal;
+	if(!(s00 > 0.0) || !(s11 > 0.0) || !(determinant > DBL_EPSILON) ||
+	   !isfinite(determinant))
+	{
+		return FUSION_GNSS_MOTION_UNKNOWN;
+	}
+	double displacement_nis =
+		(delta_e * delta_e * s11 - 2.0 * delta_e * delta_n * off_diagonal +
+		 delta_n * delta_n * s00) / determinant;
+	double speed_mps = sqrt(delta_e * delta_e + delta_n * delta_n) / dt_s;
+	if((speed_mps >= engine->config.gnss_motion_speed_threshold_mps) &&
+	   (displacement_nis >= engine->config.gnss_displacement_nis_gate))
+	{
+		return FUSION_GNSS_MOTION_MOVING;
+	}
+	return FUSION_GNSS_MOTION_STILL;
+}
+
+static eskf_update_result_t runtime_apply_position(
+	fusion_engine_t* engine,
+	fusion_runtime_t* runtime,
+	const fusion_position_measurement_t* measurement,
+	double* nis_out)
+{
+	eskf_update_result_t result = eskf_position_update(
+		&runtime->filter, &engine->config.eskf,
+		measurement->position_enu_m, measurement->covariance_enu_m2, nis_out);
+	if(result == ESKF_UPDATE_ACCEPTED)
+	{
+		fusion_gnss_motion_evidence_t raw_evidence =
+			position_motion_evidence(engine, runtime, measurement);
+		if(raw_evidence == FUSION_GNSS_MOTION_MOVING)
+		{
+			runtime->gnss_still_count = 0u;
+			if(runtime->gnss_moving_count < UINT32_MAX)
+			{
+				runtime->gnss_moving_count++;
+			}
+			if(runtime->gnss_moving_count >= engine->config.gnss_moving_confirmations)
+			{
+				runtime->gnss_evidence = FUSION_GNSS_MOTION_MOVING;
+			}
+		}
+		else if(raw_evidence == FUSION_GNSS_MOTION_STILL)
+		{
+			runtime->gnss_moving_count = 0u;
+			if(runtime->gnss_still_count < UINT32_MAX)
+			{
+				runtime->gnss_still_count++;
+			}
+			if(runtime->gnss_still_count >= engine->config.gnss_still_confirmations)
+			{
+				runtime->gnss_evidence = FUSION_GNSS_MOTION_STILL;
+			}
+		}
+		runtime->have_accepted_position = true;
+		runtime->last_accepted_position_ticks = measurement->timestamp_ticks;
+		memcpy(runtime->last_accepted_position_m, measurement->position_enu_m,
+		       sizeof(runtime->last_accepted_position_m));
+		memcpy(runtime->last_accepted_position_covariance_m2,
+		       measurement->covariance_enu_m2,
+		       sizeof(runtime->last_accepted_position_covariance_m2));
+	}
+	return result;
+}
+
+static bool process_position_now(fusion_engine_t* engine,
+	                             const fusion_position_measurement_t* measurement,
+	                             eskf_update_result_t* result_out,
+	                             double* nis_out)
+{
+	if(!engine->checkpoint_valid ||
+	   (measurement->timestamp_ticks < engine->checkpoint.filter_timestamp_ticks))
+	{
+		engine->diagnostics.position_stale_rejected++;
+		return false;
+	}
+	fusion_runtime_t revised = engine->checkpoint;
+	uint64_t replayed = 0u;
+	for(unsigned int i = 0 ; i < engine->imu_history_count ; i++)
+	{
+		const fusion_imu_measurement_t* imu = history_at(engine, i);
+		if(imu->timestamp_ticks > measurement->timestamp_ticks)
+		{
+			break;
+		}
+		if(!runtime_apply_imu(engine, &revised, imu, false))
+		{
+			engine->state = FUSION_ENGINE_FAULT;
+			return false;
+		}
+		replayed++;
+	}
+	if(revised.filter_timestamp_ticks < measurement->timestamp_ticks)
+	{
+		if(!revised.have_held_imu ||
+		   !runtime_propagate_to(engine, &revised,
+		                         measurement->timestamp_ticks, &revised.held_imu))
+		{
+			engine->state = FUSION_ENGINE_FAULT;
+			return false;
+		}
+	}
+	double nis = NAN;
+	eskf_update_result_t result = runtime_apply_position(engine, &revised,
+	                                                    measurement, &nis);
+	if(result == ESKF_UPDATE_INVALID)
+	{
+		return false;
+	}
+	if(result == ESKF_UPDATE_ACCEPTED)
+	{
+		engine->diagnostics.position_accepted++;
+	}
+	else
+	{
+		engine->diagnostics.position_nis_rejected++;
+	}
+	engine->checkpoint = revised;
+	engine->checkpoint_valid = true;
+	history_drop_through(engine, measurement->timestamp_ticks);
+	for(unsigned int i = 0 ; i < engine->imu_history_count ; i++)
+	{
+		if(!runtime_apply_imu(engine, &revised, history_at(engine, i), false))
+		{
+			engine->state = FUSION_ENGINE_FAULT;
+			return false;
+		}
+		replayed++;
+	}
+	engine->current = revised;
+	engine->diagnostics.replay_count++;
+	engine->diagnostics.replayed_imu_count += replayed;
+	if(result_out != NULL)
+	{
+		*result_out = result;
+	}
+	if(nis_out != NULL)
+	{
+		*nis_out = nis;
+	}
+	return true;
+}
+
+static bool pending_insert(fusion_engine_t* engine,
+	                       const fusion_position_measurement_t* measurement)
+{
+	if(engine->pending_position_count >= FUSION_POSITION_PENDING_CAPACITY)
+	{
+		engine->diagnostics.position_pending_overflow++;
+		return false;
+	}
+	unsigned int index = engine->pending_position_count;
+	while((index != 0u) &&
+	      (engine->pending_positions[index - 1u].timestamp_ticks >
+	       measurement->timestamp_ticks))
+	{
+		engine->pending_positions[index] = engine->pending_positions[index - 1u];
+		index--;
+	}
+	engine->pending_positions[index] = *measurement;
+	engine->pending_position_count++;
+	return true;
+}
+
+static void process_ready_pending_positions(fusion_engine_t* engine)
+{
+	while((engine->pending_position_count != 0u) &&
+	      (engine->pending_positions[0].timestamp_ticks <=
+	       engine->current.filter_timestamp_ticks))
+	{
+		fusion_position_measurement_t measurement = engine->pending_positions[0];
+		for(unsigned int i = 1u ; i < engine->pending_position_count ; i++)
+		{
+			engine->pending_positions[i - 1u] = engine->pending_positions[i];
+		}
+		engine->pending_position_count--;
+		(void)process_position_now(engine, &measurement, NULL, NULL);
+		if(engine->state == FUSION_ENGINE_FAULT)
+		{
+			return;
+		}
+	}
+}
+
+bool fusion_engine_push_imu(fusion_engine_t* engine,
+	                        const fusion_imu_measurement_t* measurement)
+{
+	if((engine == NULL) || (engine->state == FUSION_ENGINE_FAULT) ||
+	   !imu_valid(measurement))
+	{
+		if(engine != NULL)
+		{
+			engine->diagnostics.imu_rejected++;
+		}
+		return false;
+	}
+	engine->diagnostics.imu_received++;
+	if(engine->state == FUSION_ENGINE_CALIBRATING)
+	{
+		bool ok = process_calibration_imu(engine, measurement);
+		if(!ok)
+		{
+			engine->diagnostics.imu_rejected++;
+		}
+		return ok;
+	}
+	if(!runtime_apply_imu(engine, &engine->current, measurement, true))
+	{
+		engine->diagnostics.imu_rejected++;
+		return false;
+	}
+	history_push(engine, measurement);
+	if(engine->state == FUSION_ENGINE_FAULT)
+	{
+		return false;
+	}
+	process_ready_pending_positions(engine);
+	return engine->state == FUSION_ENGINE_RUNNING;
+}
+
+bool fusion_engine_push_position(
+	fusion_engine_t* engine,
+	const fusion_position_measurement_t* measurement,
+	eskf_update_result_t* update_result_out,
+	double* nis_out)
+{
+	if(update_result_out != NULL)
+	{
+		*update_result_out = ESKF_UPDATE_INVALID;
+	}
+	if(nis_out != NULL)
+	{
+		*nis_out = NAN;
+	}
+	if((engine == NULL) || (engine->state == FUSION_ENGINE_FAULT) ||
+	   !position_valid(measurement))
+	{
+		return false;
+	}
+	engine->diagnostics.position_received++;
+	if(!measurement->source_valid)
+	{
+		engine->diagnostics.position_source_rejected++;
+		return false;
+	}
+	if(engine->state == FUSION_ENGINE_CALIBRATING)
+	{
+		engine->initial_position = *measurement;
+		engine->have_initial_position = true;
+		return true;
+	}
+	if(measurement->timestamp_ticks > engine->current.filter_timestamp_ticks)
+	{
+		return pending_insert(engine, measurement);
+	}
+	return process_position_now(engine, measurement, update_result_out, nis_out);
+}
+
+bool fusion_engine_get_runtime(const fusion_engine_t* engine,
+	                           fusion_runtime_t* runtime_out)
+{
+	if((engine == NULL) || (runtime_out == NULL) ||
+	   (engine->state != FUSION_ENGINE_RUNNING))
+	{
+		return false;
+	}
+	*runtime_out = engine->current;
+	return true;
+}
+
+const fusion_engine_diagnostics_t* fusion_engine_diagnostics(
+	const fusion_engine_t* engine)
+{
+	return (engine != NULL) ? &engine->diagnostics : NULL;
+}

soc/software/gps_system/src/fusion_gps_adapter.c

@@ -0,0 +1,254 @@
+#include "fusion_gps_adapter.h"
+
+#include <float.h>
+#include <math.h>
+#include <stddef.h>
+#include <string.h>
+
+#define WGS84_SEMI_MAJOR_M 6378137.0
+#define WGS84_FLATTENING (1.0 / 298.257223563)
+#define FUSION_GPS_PI 3.14159265358979323846264338327950288
+
+static double radians(double degrees)
+{
+	return degrees * (FUSION_GPS_PI / 180.0);
+}
+
+static double degrees(double radians_value)
+{
+	return radians_value * (180.0 / FUSION_GPS_PI);
+}
+
+static bool llh_valid(double latitude_deg, double longitude_deg, double height_m)
+{
+	return isfinite(latitude_deg) && isfinite(longitude_deg) && isfinite(height_m) &&
+	       (latitude_deg >= -90.0) && (latitude_deg <= 90.0) &&
+	       (longitude_deg >= -180.0) && (longitude_deg <= 180.0) &&
+	       (height_m >= -10000.0) && (height_m <= 100000.0);
+}
+
+static void geodetic_to_ecef(double latitude_deg, double longitude_deg,
+	                         double height_m, double ecef_m[3])
+{
+	double latitude = radians(latitude_deg);
+	double longitude = radians(longitude_deg);
+	double flattening = WGS84_FLATTENING;
+	double eccentricity_squared = flattening * (2.0 - flattening);
+	double sin_latitude = sin(latitude);
+	double cos_latitude = cos(latitude);
+	double prime_vertical = WGS84_SEMI_MAJOR_M /
+		                    sqrt(1.0 - eccentricity_squared *
+		                         sin_latitude * sin_latitude);
+	ecef_m[0] = (prime_vertical + height_m) * cos_latitude * cos(longitude);
+	ecef_m[1] = (prime_vertical + height_m) * cos_latitude * sin(longitude);
+	ecef_m[2] = (prime_vertical * (1.0 - eccentricity_squared) + height_m) *
+	            sin_latitude;
+}
+
+static void configure_origin(fusion_gps_adapter_t* adapter,
+	                         const fusion_gps_measurement_t* gps)
+{
+	adapter->origin_latitude_deg = gps->latitude_deg;
+	adapter->origin_longitude_deg = gps->longitude_deg;
+	adapter->origin_height_m = gps->height_m;
+	geodetic_to_ecef(gps->latitude_deg, gps->longitude_deg, gps->height_m,
+	                 adapter->origin_ecef_m);
+	double latitude = radians(gps->latitude_deg);
+	double longitude = radians(gps->longitude_deg);
+	double sin_latitude = sin(latitude);
+	double cos_latitude = cos(latitude);
+	double sin_longitude = sin(longitude);
+	double cos_longitude = cos(longitude);
+	adapter->ecef_to_enu[0][0] = -sin_longitude;
+	adapter->ecef_to_enu[0][1] = cos_longitude;
+	adapter->ecef_to_enu[0][2] = 0.0;
+	adapter->ecef_to_enu[1][0] = -sin_latitude * cos_longitude;
+	adapter->ecef_to_enu[1][1] = -sin_latitude * sin_longitude;
+	adapter->ecef_to_enu[1][2] = cos_latitude;
+	adapter->ecef_to_enu[2][0] = cos_latitude * cos_longitude;
+	adapter->ecef_to_enu[2][1] = cos_latitude * sin_longitude;
+	adapter->ecef_to_enu[2][2] = sin_latitude;
+	adapter->origin_valid = true;
+}
+
+void fusion_gps_adapter_default_config(fusion_gps_adapter_config_t* config)
+{
+	if(config == NULL)
+	{
+		return;
+	}
+	config->minimum_horizontal_std_m = 0.50;
+	config->minimum_vertical_std_m = 1.00;
+	config->maximum_position_std_m = 1000.0;
+}
+
+bool fusion_gps_adapter_init(fusion_gps_adapter_t* adapter,
+	                         const fusion_gps_adapter_config_t* config)
+{
+	if((adapter == NULL) || (config == NULL) ||
+	   !(config->minimum_horizontal_std_m > 0.0) ||
+	   !(config->minimum_vertical_std_m > 0.0) ||
+	   !(config->maximum_position_std_m >= config->minimum_vertical_std_m))
+	{
+		return false;
+	}
+	memset(adapter, 0, sizeof(*adapter));
+	adapter->config = *config;
+	return true;
+}
+
+static bool sanitize_covariance(const fusion_gps_adapter_t* adapter,
+	                            const double input[3][3], double output[9])
+{
+	double variance_floor[3] = {
+		adapter->config.minimum_horizontal_std_m *
+			adapter->config.minimum_horizontal_std_m,
+		adapter->config.minimum_horizontal_std_m *
+			adapter->config.minimum_horizontal_std_m,
+		adapter->config.minimum_vertical_std_m *
+			adapter->config.minimum_vertical_std_m
+	};
+	double variance_ceiling = adapter->config.maximum_position_std_m *
+	                          adapter->config.maximum_position_std_m;
+	for(unsigned int axis = 0 ; axis < 3u ; axis++)
+	{
+		double variance = input[axis][axis];
+		if(!isfinite(variance) || (variance <= 0.0))
+		{
+			return false;
+		}
+		output[axis * 3u + axis] = fmin(fmax(variance, variance_floor[axis]),
+		                                      variance_ceiling);
+	}
+	for(unsigned int row = 0 ; row < 3u ; row++)
+	{
+		for(unsigned int col = row + 1u ; col < 3u ; col++)
+		{
+			double covariance = 0.5 * (input[row][col] + input[col][row]);
+			if(!isfinite(covariance))
+			{
+				return false;
+			}
+			double limit = 0.95 * sqrt(output[row * 3u + row] *
+			                           output[col * 3u + col]);
+			covariance = fmin(fmax(covariance, -limit), limit);
+			output[row * 3u + col] = covariance;
+			output[col * 3u + row] = covariance;
+		}
+	}
+	/* Sylvester criterion: the sanitized symmetric covariance must be PD. */
+	double minor2 = output[0] * output[4] - output[1] * output[3];
+	double determinant =
+		output[0] * (output[4] * output[8] - output[5] * output[7]) -
+		output[1] * (output[3] * output[8] - output[5] * output[6]) +
+		output[2] * (output[3] * output[7] - output[4] * output[6]);
+	return (output[0] > 0.0) && (minor2 > DBL_EPSILON) &&
+	       (determinant > DBL_EPSILON) && isfinite(determinant);
+}
+
+bool fusion_gps_adapter_convert(fusion_gps_adapter_t* adapter,
+	                            const fusion_gps_measurement_t* gps,
+	                            fusion_position_measurement_t* position_out)
+{
+	if((adapter == NULL) || (gps == NULL) || (position_out == NULL) ||
+	   !llh_valid(gps->latitude_deg, gps->longitude_deg, gps->height_m))
+	{
+		return false;
+	}
+	memset(position_out, 0, sizeof(*position_out));
+	position_out->timestamp_ticks = gps->timestamp_ticks;
+	position_out->source_valid = gps->solution_valid;
+	position_out->source_quality =
+		(isfinite(gps->chi_square_ratio) && (gps->chi_square_ratio >= 0.0))
+		? 100.0 / (1.0 + gps->chi_square_ratio) : 0.0;
+	if(!sanitize_covariance(adapter, gps->covariance_enu_m2,
+	                        position_out->covariance_enu_m2))
+	{
+		return false;
+	}
+	if(!adapter->origin_valid)
+	{
+		if(!gps->solution_valid)
+		{
+			return false;
+		}
+		configure_origin(adapter, gps);
+	}
+	double ecef_m[3];
+	double delta_ecef_m[3];
+	geodetic_to_ecef(gps->latitude_deg, gps->longitude_deg, gps->height_m, ecef_m);
+	for(unsigned int axis = 0 ; axis < 3u ; axis++)
+	{
+		delta_ecef_m[axis] = ecef_m[axis] - adapter->origin_ecef_m[axis];
+	}
+	for(unsigned int row = 0 ; row < 3u ; row++)
+	{
+		for(unsigned int col = 0 ; col < 3u ; col++)
+		{
+			position_out->position_enu_m[row] +=
+				adapter->ecef_to_enu[row][col] * delta_ecef_m[col];
+		}
+	}
+	return isfinite(position_out->position_enu_m[0]) &&
+	       isfinite(position_out->position_enu_m[1]) &&
+	       isfinite(position_out->position_enu_m[2]);
+}
+
+bool fusion_gps_adapter_local_to_geodetic(
+	const fusion_gps_adapter_t* adapter,
+	const double position_enu_m[3],
+	double* latitude_deg_out,
+	double* longitude_deg_out,
+	double* height_m_out)
+{
+	if((adapter == NULL) || !adapter->origin_valid || (position_enu_m == NULL) ||
+	   (latitude_deg_out == NULL) || (longitude_deg_out == NULL) ||
+	   (height_m_out == NULL))
+	{
+		return false;
+	}
+	double ecef_m[3];
+	for(unsigned int row = 0 ; row < 3u ; row++)
+	{
+		ecef_m[row] = adapter->origin_ecef_m[row];
+		for(unsigned int axis = 0 ; axis < 3u ; axis++)
+		{
+			/* R^-1 = R^T for the orthonormal ECEF-to-ENU transform. */
+			ecef_m[row] += adapter->ecef_to_enu[axis][row] * position_enu_m[axis];
+		}
+	}
+	double flattening = WGS84_FLATTENING;
+	double eccentricity_squared = flattening * (2.0 - flattening);
+	double longitude = atan2(ecef_m[1], ecef_m[0]);
+	double horizontal = hypot(ecef_m[0], ecef_m[1]);
+	double latitude = atan2(ecef_m[2], horizontal * (1.0 - eccentricity_squared));
+	double height = 0.0;
+	for(unsigned int iteration = 0 ; iteration < 12u ; iteration++)
+	{
+		double sin_latitude = sin(latitude);
+		double prime_vertical = WGS84_SEMI_MAJOR_M /
+			sqrt(1.0 - eccentricity_squared * sin_latitude * sin_latitude);
+		double cos_latitude = cos(latitude);
+		if(fabs(cos_latitude) < 1.0e-12)
+		{
+			height = fabs(ecef_m[2]) - prime_vertical * (1.0 - eccentricity_squared);
+		}
+		else
+		{
+			height = horizontal / cos_latitude - prime_vertical;
+		}
+		double next = atan2(ecef_m[2],
+			horizontal * (1.0 - eccentricity_squared *
+			prime_vertical / (prime_vertical + height)));
+		if(fabs(next - latitude) < 1.0e-13)
+		{
+			latitude = next;
+			break;
+		}
+		latitude = next;
+	}
+	*latitude_deg_out = degrees(latitude);
+	*longitude_deg_out = degrees(longitude);
+	*height_m_out = height;
+	return llh_valid(*latitude_deg_out, *longitude_deg_out, *height_m_out);
+}

soc/software/gps_system/src/fusion_motion.c

@@ -0,0 +1,217 @@
+#include "fusion_motion.h"
+
+#include <math.h>
+#include <stddef.h>
+#include <string.h>
+
+#define FUSION_MOTION_PI 3.14159265358979323846264338327950288
+
+static double norm3(const double vector[3])
+{
+	return sqrt(vector[0] * vector[0] + vector[1] * vector[1] +
+	            vector[2] * vector[2]);
+}
+
+static bool finite3(const double vector[3])
+{
+	return (vector != NULL) && isfinite(vector[0]) &&
+	       isfinite(vector[1]) && isfinite(vector[2]);
+}
+
+static void transition_to(fusion_motion_detector_t* detector,
+	                      fusion_motion_state_t state)
+{
+	if(detector->state != state)
+	{
+		detector->state = state;
+		detector->transition_count++;
+	}
+	if(state != FUSION_MOTION_STATIONARY_CANDIDATE)
+	{
+		detector->quiet_time_s = 0.0;
+	}
+	if(state != FUSION_MOTION_STATIONARY)
+	{
+		detector->weak_motion_count = 0u;
+	}
+}
+
+void fusion_motion_default_config(fusion_motion_config_t* config)
+{
+	if(config == NULL)
+	{
+		return;
+	}
+	config->stationary_accel_limit_mps2 = 0.15;
+	config->stationary_gyro_limit_rps = 1.0 * FUSION_MOTION_PI / 180.0;
+	config->moving_accel_threshold_mps2 = 0.40;
+	config->moving_gyro_threshold_rps = 3.0 * FUSION_MOTION_PI / 180.0;
+	config->moving_jerk_threshold_mps3 = 4.0;
+	config->jerk_filter_time_constant_s = 0.10;
+	config->stationary_speed_limit_mps = 0.40;
+	config->moving_speed_threshold_mps = 0.50;
+	config->stationary_dwell_s = 1.0;
+	config->weak_motion_samples = 3u;
+}
+
+void fusion_motion_init(fusion_motion_detector_t* detector,
+	                    bool bootstrap_stationary)
+{
+	if(detector == NULL)
+	{
+		return;
+	}
+	memset(detector, 0, sizeof(*detector));
+	detector->state = bootstrap_stationary
+	                ? FUSION_MOTION_STATIONARY
+	                : FUSION_MOTION_BOOTSTRAP;
+}
+
+fusion_motion_state_t fusion_motion_update(
+	fusion_motion_detector_t* detector,
+	const fusion_motion_config_t* config,
+	const double linear_accel_nav_mps2[3],
+	const double unbiased_gyro_rps[3],
+	double speed_mps,
+	fusion_gnss_motion_evidence_t gnss_evidence,
+	double dt_s)
+{
+	if((detector == NULL) || (config == NULL) ||
+	   !finite3(linear_accel_nav_mps2) || !finite3(unbiased_gyro_rps) ||
+	   !isfinite(speed_mps) || (speed_mps < 0.0) ||
+	   !isfinite(dt_s) || (dt_s <= 0.0))
+	{
+		return (detector != NULL) ? detector->state : FUSION_MOTION_BOOTSTRAP;
+	}
+
+	double accel_norm = norm3(linear_accel_nav_mps2);
+	double gyro_norm = norm3(unbiased_gyro_rps);
+	double jerk_norm = 0.0;
+	if(!detector->have_previous_accel)
+	{
+		memcpy(detector->filtered_linear_accel_mps2, linear_accel_nav_mps2,
+		       sizeof(detector->filtered_linear_accel_mps2));
+		memcpy(detector->previous_filtered_linear_accel_mps2,
+		       detector->filtered_linear_accel_mps2,
+		       sizeof(detector->previous_filtered_linear_accel_mps2));
+		detector->have_previous_accel = true;
+	}
+	else
+	{
+		double time_constant_s = fmax(0.0, config->jerk_filter_time_constant_s);
+		double alpha = (time_constant_s > 0.0)
+		             ? dt_s / (time_constant_s + dt_s)
+		             : 1.0;
+		double delta[3];
+		for(unsigned int axis = 0u ; axis < 3u ; axis++)
+		{
+			detector->previous_filtered_linear_accel_mps2[axis] =
+				detector->filtered_linear_accel_mps2[axis];
+			detector->filtered_linear_accel_mps2[axis] += alpha *
+				(linear_accel_nav_mps2[axis] -
+				 detector->filtered_linear_accel_mps2[axis]);
+			delta[axis] = detector->filtered_linear_accel_mps2[axis] -
+			              detector->previous_filtered_linear_accel_mps2[axis];
+		}
+		jerk_norm = norm3(delta) / dt_s;
+	}
+	detector->last_linear_accel_norm_mps2 = accel_norm;
+	detector->last_gyro_norm_rps = gyro_norm;
+	detector->last_jerk_norm_mps3 = jerk_norm;
+	detector->last_speed_mps = speed_mps;
+
+	bool gnss_moving = gnss_evidence == FUSION_GNSS_MOTION_MOVING;
+	bool strong_motion = gnss_moving ||
+	                     (accel_norm >= config->moving_accel_threshold_mps2) ||
+	                     (gyro_norm >= config->moving_gyro_threshold_rps) ||
+	                     (detector->have_previous_accel &&
+	                      (jerk_norm >= config->moving_jerk_threshold_mps3)) ||
+	                     (speed_mps >= config->moving_speed_threshold_mps);
+	bool quiet = (accel_norm <= config->stationary_accel_limit_mps2) &&
+	             (gyro_norm <= config->stationary_gyro_limit_rps);
+	bool stationary_candidate = quiet && !gnss_moving &&
+	                            (speed_mps <= config->stationary_speed_limit_mps);
+	bool weak_motion = !quiet;
+
+	switch(detector->state)
+	{
+		case FUSION_MOTION_STATIONARY:
+			if(strong_motion)
+			{
+				transition_to(detector, FUSION_MOTION_MOVING);
+			}
+			else if(weak_motion)
+			{
+				detector->weak_motion_count++;
+				if(detector->weak_motion_count >= config->weak_motion_samples)
+				{
+					transition_to(detector, FUSION_MOTION_MOVING);
+				}
+			}
+			else
+			{
+				detector->weak_motion_count = 0u;
+			}
+			break;
+
+		case FUSION_MOTION_BOOTSTRAP:
+		case FUSION_MOTION_MOVING:
+			if(strong_motion)
+			{
+				transition_to(detector, FUSION_MOTION_MOVING);
+			}
+			else if(stationary_candidate)
+			{
+				transition_to(detector, FUSION_MOTION_STATIONARY_CANDIDATE);
+				detector->quiet_time_s = dt_s;
+			}
+			else
+			{
+				transition_to(detector, FUSION_MOTION_MOVING);
+			}
+			break;
+
+		case FUSION_MOTION_STATIONARY_CANDIDATE:
+			if(strong_motion || !stationary_candidate)
+			{
+				transition_to(detector, FUSION_MOTION_MOVING);
+			}
+			else
+			{
+				detector->quiet_time_s += dt_s;
+				if(detector->quiet_time_s >= config->stationary_dwell_s)
+				{
+					transition_to(detector, FUSION_MOTION_STATIONARY);
+				}
+			}
+			break;
+
+		default:
+			transition_to(detector, FUSION_MOTION_BOOTSTRAP);
+			break;
+	}
+	return detector->state;
+}
+
+bool fusion_motion_allows_zero_update(const fusion_motion_detector_t* detector)
+{
+	return (detector != NULL) &&
+	       (detector->state == FUSION_MOTION_STATIONARY);
+}
+
+const char* fusion_motion_state_name(fusion_motion_state_t state)
+{
+	switch(state)
+	{
+		case FUSION_MOTION_BOOTSTRAP:
+			return "bootstrap";
+		case FUSION_MOTION_STATIONARY:
+			return "stationary";
+		case FUSION_MOTION_MOVING:
+			return "moving";
+		case FUSION_MOTION_STATIONARY_CANDIDATE:
+			return "stationary_candidate";
+		default:
+			return "unknown";
+	}
+}

soc/software/gps_system/src/gps_fusion_publisher.c

@@ -0,0 +1,30 @@
+#include "gps_fusion_publisher.h"
+
+#include "navigation_fusion.h"
+
+#include <stddef.h>
+#include <string.h>
+
+bool gps_fusion_publish_solution(const gps_nav_solution_t* solution,
+	                             uint64_t measurement_timestamp_ticks)
+{
+	if((solution == NULL) || (measurement_timestamp_ticks == 0u))
+	{
+		return false;
+	}
+	fusion_gps_measurement_t measurement;
+	memset(&measurement, 0, sizeof(measurement));
+	measurement.timestamp_ticks = measurement_timestamp_ticks;
+	measurement.latitude_deg = solution->latitude_deg;
+	measurement.longitude_deg = solution->longitude_deg;
+	measurement.height_m = solution->height_m;
+	memcpy(measurement.covariance_enu_m2, solution->covariance_enu_m2,
+	       sizeof(measurement.covariance_enu_m2));
+	measurement.hacc_m = solution->hacc_m;
+	measurement.vacc_m = solution->vacc_m;
+	measurement.residual_rms_m = solution->residual_rms_m;
+	measurement.chi_square_ratio = solution->chi_square_ratio;
+	measurement.used_satellites = solution->used_satellite_count;
+	measurement.solution_valid = solution->solution_valid;
+	return navigation_fusion_publish_gps(&measurement);
+}

soc/software/gps_system/src/gps_interrupt_handlers.c

@@ -3,6 +3,7 @@
 #include "Navigation.h"
 #include "gps_pps.h"
 #include "gps_time_anchor.h"
+#include "tdk_icm42688.h"
 #include <bsp/irq.h>
 #include <bsp/mmio.h>
 #include <bsp/plic.h>
@@ -25,6 +26,7 @@ typedef struct {
 	double corrected_abs_sample;
 	uint32_t nav_solve_timer;
 	uint32_t nav_solve_event_count;
+	uint64_t local_timestamp_ticks;
 	uint64_t pps_counter;
 	uint32_t pps_capture_sequence;
 	bool pps_capture_valid;
@@ -393,6 +395,12 @@ void gps_interrupt_init(void)
 
 void gps_dispatch_irq(unsigned int irq_id)
 {
+	if(irq_id == BSP_PMOD_IRQ)
+	{
+		tdk_icm42688_irq_handler();
+		return;
+	}
+
 	unsigned char acq_engine_id;
 	if(gps_irq_to_acq_engine(irq_id, &acq_engine_id))
 	{
@@ -633,6 +641,7 @@ void trck_intr_handler(volatile gps_trck_t* const trck_ip,
 		return;
 	}
 
+	uint64_t local_timestamp_ticks = bsp_read64(BSP_CLINT_MTIME);
 	uint32_t status_reg = trck_ip->STATUS_REG;
 	if((status_reg & (1u << GPS_TRCK_INTR_PEND_POS)) == 0u)
 	{
@@ -653,6 +662,7 @@ void trck_intr_handler(volatile gps_trck_t* const trck_ip,
 	event.corrected_abs_sample = gps_correct_abs_sample(event.abs_sample_raw, event.rem_code_phase);
 	event.nav_solve_timer = trck_ip->NAV_SOLVE_TIMER;
 	event.nav_solve_event_count = (status_reg & (1u << GPS_TRCK_NAV_SOLVE_FLAG_POS)) ? 1u : 0u;
+	event.local_timestamp_ticks = local_timestamp_ticks;
 	if(event.nav_solve_event_count != 0u)
 	{
 		event.pps_capture_valid = gps_pps_read_capture(trck_ip_no,
@@ -1032,6 +1042,7 @@ static void gps_process_tracking_nav_solve(const gps_isr_event_t* event)
 		gps_nav_solve_period_pending = true;
 		node->gps_last_nav_solve_abs_sample = event->abs_sample_raw;
 		node->gps_last_nav_solve_corrected_abs_sample = event->corrected_abs_sample;
+		node->gps_last_nav_solve_local_ticks = event->local_timestamp_ticks;
 		if(event->pps_capture_valid)
 		{
 			node->gps_last_nav_solve_pps_counter = event->pps_counter;

soc/software/gps_system/src/gps_ip.c

@@ -9,6 +9,7 @@
 #include "linked_list.h"
 #include "gps_interrupt_handlers.h"
 #include "gps_pps.h"
+#include "gps_fusion_publisher.h"
 #include "defines.h"
 #include <bsp/irq.h>
 
@@ -766,6 +767,7 @@ static void gps_capture_nav_metadata_locked(const unsigned char snapshot_ips[GPS
 	                                         uint32_t snapshot_sequences[GPS_TRCK_IP_COUNT],
 	                                         uint64_t snapshot_pps_counters[GPS_TRCK_IP_COUNT],
 	                                         uint32_t snapshot_pps_sequences[GPS_TRCK_IP_COUNT],
+	                                         uint64_t snapshot_local_timestamps[GPS_TRCK_IP_COUNT],
 	                                         unsigned int satellite_count)
 {
 	for(unsigned int i = 0 ; i < satellite_count ; i++)
@@ -774,9 +776,34 @@ static void gps_capture_nav_metadata_locked(const unsigned char snapshot_ips[GPS
 		snapshot_sequences[i] = (node != NULL) ? node->gps_nav_solve_sequence : 0u;
 		snapshot_pps_counters[i] = (node != NULL) ? node->gps_last_nav_solve_pps_counter : 0u;
 		snapshot_pps_sequences[i] = (node != NULL) ? node->gps_last_pps_capture_sequence : 0u;
+		snapshot_local_timestamps[i] =
+			(node != NULL) ? node->gps_last_nav_solve_local_ticks : 0u;
 	}
 }
 
+static uint64_t gps_nav_median_local_timestamp(
+	const uint64_t timestamps[GPS_TRCK_IP_COUNT], unsigned int count)
+{
+	uint64_t sorted[GPS_TRCK_IP_COUNT];
+	unsigned int valid_count = 0u;
+	for(unsigned int i = 0 ; (i < count) && (i < GPS_TRCK_IP_COUNT) ; i++)
+	{
+		if(timestamps[i] == 0u)
+		{
+			continue;
+		}
+		unsigned int insert = valid_count;
+		while((insert != 0u) && (sorted[insert - 1u] > timestamps[i]))
+		{
+			sorted[insert] = sorted[insert - 1u];
+			insert--;
+		}
+		sorted[insert] = timestamps[i];
+		valid_count++;
+	}
+	return (valid_count != 0u) ? sorted[valid_count / 2u] : 0u;
+}
+
 static void gps_consume_nav_sequences_locked(const unsigned char snapshot_ips[GPS_TRCK_IP_COUNT],
 	                                         const uint32_t snapshot_sequences[GPS_TRCK_IP_COUNT],
 	                                         unsigned int satellite_count)
@@ -1069,6 +1096,7 @@ void calculate_position()
 	uint32_t snapshot_sequences[GPS_TRCK_IP_COUNT];
 	uint64_t snapshot_pps_counters[GPS_TRCK_IP_COUNT];
 	uint32_t snapshot_pps_sequences[GPS_TRCK_IP_COUNT];
+	uint64_t snapshot_local_timestamps[GPS_TRCK_IP_COUNT];
 	unsigned int satellite_count = 0;
 	bool run_nav = false;
 	bool initial_run = false;
@@ -1115,6 +1143,7 @@ void calculate_position()
 			{
 				gps_capture_nav_metadata_locked(snapshot_ips, snapshot_sequences,
 				                               snapshot_pps_counters, snapshot_pps_sequences,
+				                               snapshot_local_timestamps,
 				                               satellite_count);
 			}
 		}
@@ -1157,6 +1186,14 @@ void calculate_position()
 		uint64_t nav_software_cycles = gps_read_cycle_counter() - nav_software_start_cycles;
 		if(nav_ok)
 		{
+			uint64_t measurement_timestamp = gps_nav_median_local_timestamp(
+				snapshot_local_timestamps, satellite_count);
+			if(!gps_fusion_publish_solution(&nav_solution, measurement_timestamp))
+			{
+				GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
+				        "fusion GPS publish skipped timestamp=%lu\r\n",
+				        (unsigned long)measurement_timestamp);
+			}
 			uint64_t consume_irq_state = bsp_irq_save();
 			gps_consume_nav_sequences_locked(snapshot_ips, snapshot_sequences, satellite_count);
 			bsp_irq_restore(consume_irq_state);

soc/software/gps_system/src/linked_list.c

@@ -70,6 +70,7 @@ bool insert_element(volatile gps_ephem_ll_t** node,
 	new_node->gps_nav_solve_consumed_sequence = 0u;
 	new_node->gps_last_nav_solve_abs_sample = 0u;
 	new_node->gps_last_nav_solve_corrected_abs_sample = 0.0;
+	new_node->gps_last_nav_solve_local_ticks = 0u;
 	new_node->gps_last_nav_solve_pps_counter = 0u;
 	new_node->gps_last_pps_capture_sequence = 0u;
 	new_node->gps_last_measurement_tick = 0u;

soc/software/gps_system/src/navigation_fusion.c

@@ -0,0 +1,108 @@
+#include "navigation_fusion.h"
+
+#include "fusion_bus.h"
+#include "fusion_gps_adapter.h"
+#include "tdk_icm42688.h"
+
+#include <math.h>
+#include <stddef.h>
+#include <string.h>
+
+static fusion_bus_t measurement_bus;
+static fusion_gps_adapter_t gps_adapter;
+static fusion_engine_t fusion_engine;
+static bool fusion_initialized;
+
+bool navigation_fusion_init(uint64_t local_timebase_hz)
+{
+	fusion_engine_config_t engine_config;
+	fusion_gps_adapter_config_t adapter_config;
+	if(local_timebase_hz == 0u)
+	{
+		return false;
+	}
+	fusion_bus_init(&measurement_bus);
+	fusion_engine_default_config(&engine_config, local_timebase_hz);
+	fusion_gps_adapter_default_config(&adapter_config);
+	if(!fusion_engine_init(&fusion_engine, &engine_config) ||
+	   !fusion_gps_adapter_init(&gps_adapter, &adapter_config))
+	{
+		return false;
+	}
+	fusion_initialized = true;
+	return true;
+}
+
+bool navigation_fusion_publish_gps(const fusion_gps_measurement_t* measurement)
+{
+	return fusion_initialized &&
+	       fusion_bus_publish_gps(&measurement_bus, measurement);
+}
+
+unsigned int navigation_fusion_service(void)
+{
+	if(!fusion_initialized)
+	{
+		return 0u;
+	}
+	unsigned int processed = 0u;
+	fusion_imu_measurement_t imu;
+	while(tdk_icm42688_take_measurement(&imu))
+	{
+		(void)fusion_engine_push_imu(&fusion_engine, &imu);
+		processed++;
+	}
+
+	/* GPS is intentionally consumed after all currently queued IMU samples.
+	 * A delayed WLS result is applied at its own timestamp by fixed-lag replay. */
+	fusion_gps_measurement_t gps;
+	while(fusion_bus_take_gps(&measurement_bus, &gps))
+	{
+		fusion_position_measurement_t position;
+		if(fusion_gps_adapter_convert(&gps_adapter, &gps, &position))
+		{
+			(void)fusion_engine_push_position(&fusion_engine, &position, NULL, NULL);
+		}
+		processed++;
+	}
+	return processed;
+}
+
+bool navigation_fusion_get_output(navigation_fusion_output_t* output)
+{
+	if((output == NULL) || !fusion_initialized ||
+	   (fusion_engine.state != FUSION_ENGINE_RUNNING))
+	{
+		return false;
+	}
+	memset(output, 0, sizeof(*output));
+	const fusion_runtime_t* runtime = &fusion_engine.current;
+	output->timestamp_ticks = runtime->filter_timestamp_ticks;
+	memcpy(output->position_enu_m, runtime->filter.position_m,
+	       sizeof(output->position_enu_m));
+	memcpy(output->velocity_enu_mps, runtime->filter.velocity_mps,
+	       sizeof(output->velocity_enu_mps));
+	for(unsigned int axis = 0 ; axis < 3u ; axis++)
+	{
+		output->position_std_m[axis] =
+			sqrt(fmax(0.0, runtime->filter.covariance[axis][axis]));
+	}
+	output->speed_mps = eskf_speed_mps(&runtime->filter);
+	output->last_position_nis = runtime->filter.last_position_nis;
+	output->motion_state = runtime->motion.state;
+	output->gps_accepted = fusion_engine.diagnostics.position_accepted;
+	output->gps_nis_rejected = fusion_engine.diagnostics.position_nis_rejected;
+	if(!fusion_gps_adapter_local_to_geodetic(
+		&gps_adapter, runtime->filter.position_m,
+		&output->latitude_deg, &output->longitude_deg, &output->height_m))
+	{
+		return false;
+	}
+	output->valid = true;
+	return true;
+}
+
+const fusion_engine_t* navigation_fusion_engine(void)
+{
+	return fusion_initialized ? &fusion_engine : NULL;
+}

soc/software/gps_system/src/tdk_icm42688.c

@@ -0,0 +1,393 @@
+#include "tdk_icm42688.h"
+
+#include <bsp/mmio.h>
+#include <bsp/plic.h>
+#include <bsp/pmod_spi.h>
+#include <bsp/soc.h>
+#include <limits.h>
+#include <math.h>
+#include <stddef.h>
+#include <string.h>
+
+#define ICM_DEVICE_CONFIG       0x11u
+#define ICM_INT_CONFIG          0x14u
+#define ICM_TEMP_DATA1          0x1du
+#define ICM_INT_STATUS          0x2du
+#define ICM_PWR_MGMT0           0x4eu
+#define ICM_GYRO_CONFIG0        0x4fu
+#define ICM_ACCEL_CONFIG0       0x50u
+#define ICM_GYRO_CONFIG1        0x51u
+#define ICM_GYRO_ACCEL_CONFIG0  0x52u
+#define ICM_ACCEL_CONFIG1       0x53u
+#define ICM_INT_CONFIG0         0x63u
+#define ICM_INT_CONFIG1         0x64u
+#define ICM_INT_SOURCE0         0x65u
+#define ICM_WHO_AM_I            0x75u
+
+#define ICM_SPI_READ            0x80u
+#define ICM_SPI_DUMMY           0xffu
+#define ICM_DATA_READY          0x08u
+#define ICM42688_WHO_AM_I       0x47u
+#define ICM_PWR_LOW_NOISE       0x0fu
+#define ICM_INT1_LATCHED_HIGH_PUSH_PULL 0x07u
+#define ICM_UI_DRDY_INT1_EN     0x08u
+
+#define TDK_GRAVITY_MPS2 9.80665
+#define TDK_DEGREES_TO_RADIANS 0.017453292519943295769236907684886
+
+#if (TDK_IMU_RAW_QUEUE_CAPACITY < 2u)
+#error "TDK_IMU_RAW_QUEUE_CAPACITY must be at least 2"
+#endif
+
+static tdk_icm42688_config_t active_config;
+static tdk_icm42688_raw_sample_t sample_queue[TDK_IMU_RAW_QUEUE_CAPACITY];
+static volatile unsigned int sample_head;
+static volatile unsigned int sample_tail;
+static volatile tdk_icm42688_diagnostics_t diagnostics;
+
+static uint64_t time_now(void)
+{
+	return bsp_read64(BSP_CLINT_MTIME);
+}
+
+static void delay_us(uint32_t microseconds)
+{
+	uint64_t ticks = ((uint64_t)BSP_CLINT_TIMEBASE_HZ * microseconds + 999999u) /
+	                 1000000u;
+	uint64_t start = time_now();
+	while((time_now() - start) < ticks)
+	{
+	}
+}
+
+static void delay_ms(uint32_t milliseconds)
+{
+	uint64_t ticks = ((uint64_t)BSP_CLINT_TIMEBASE_HZ * milliseconds + 999u) /
+	                 1000u;
+	uint64_t start = time_now();
+	while((time_now() - start) < ticks)
+	{
+	}
+}
+
+static void write_register(uint8_t reg, uint8_t value)
+{
+	bsp_pmod_spi_write_reg8(reg & (uint8_t)~ICM_SPI_READ, value);
+}
+
+static uint8_t read_register(uint8_t reg)
+{
+	return bsp_pmod_spi_read_reg8(reg | ICM_SPI_READ, ICM_SPI_DUMMY);
+}
+
+static int16_t big_endian_i16(const uint8_t* bytes)
+{
+	return (int16_t)(((uint16_t)bytes[0] << 8) | bytes[1]);
+}
+
+static bool odr_register_value(uint32_t odr_hz, uint8_t* register_value)
+{
+	if(register_value == NULL)
+	{
+		return false;
+	}
+	switch(odr_hz)
+	{
+		case 1000u: *register_value = 0x06u; return true;
+		case 500u:  *register_value = 0x0fu; return true;
+		case 200u:  *register_value = 0x07u; return true;
+		case 100u:  *register_value = 0x08u; return true;
+		case 50u:   *register_value = 0x09u; return true;
+		case 25u:   *register_value = 0x0au; return true;
+		default: return false;
+	}
+}
+
+static bool accel_full_scale_bits(tdk_accel_full_scale_t full_scale,
+	                              uint8_t* bits)
+{
+	if(bits == NULL)
+	{
+		return false;
+	}
+	switch(full_scale)
+	{
+		case TDK_ACCEL_FS_16G: *bits = 0u; return true;
+		case TDK_ACCEL_FS_8G:  *bits = 1u; return true;
+		case TDK_ACCEL_FS_4G:  *bits = 2u; return true;
+		case TDK_ACCEL_FS_2G:  *bits = 3u; return true;
+		default: return false;
+	}
+}
+
+static bool gyro_full_scale_bits(tdk_gyro_full_scale_mdps_t full_scale,
+	                             uint8_t* bits)
+{
+	if(bits == NULL)
+	{
+		return false;
+	}
+	switch(full_scale)
+	{
+		case TDK_GYRO_FS_2000:   *bits = 0u; return true;
+		case TDK_GYRO_FS_1000:   *bits = 1u; return true;
+		case TDK_GYRO_FS_500:    *bits = 2u; return true;
+		case TDK_GYRO_FS_250:    *bits = 3u; return true;
+		case TDK_GYRO_FS_125:    *bits = 4u; return true;
+		case TDK_GYRO_FS_62D5:   *bits = 5u; return true;
+		case TDK_GYRO_FS_31D25:  *bits = 6u; return true;
+		case TDK_GYRO_FS_15D625: *bits = 7u; return true;
+		default: return false;
+	}
+}
+
+static unsigned int next_queue_index(unsigned int index)
+{
+	index++;
+	return (index == TDK_IMU_RAW_QUEUE_CAPACITY) ? 0u : index;
+}
+
+static bool raw_sample_valid(const tdk_icm42688_raw_sample_t* sample)
+{
+	if((sample == NULL) || (sample->temperature_raw == INT16_MIN))
+	{
+		return false;
+	}
+	for(unsigned int axis = 0 ; axis < 3u ; axis++)
+	{
+		if((sample->accel_raw[axis] == INT16_MIN) ||
+		   (sample->gyro_raw[axis] == INT16_MIN))
+		{
+			return false;
+		}
+	}
+	return true;
+}
+
+static bool queue_push_isr(const tdk_icm42688_raw_sample_t* sample)
+{
+	unsigned int head = __atomic_load_n(&sample_head, __ATOMIC_RELAXED);
+	unsigned int next = next_queue_index(head);
+	if(next == __atomic_load_n(&sample_tail, __ATOMIC_ACQUIRE))
+	{
+		__atomic_fetch_add(&diagnostics.queue_overflow_count, 1u, __ATOMIC_RELAXED);
+		return false;
+	}
+	sample_queue[head] = *sample;
+	__atomic_store_n(&sample_head, next, __ATOMIC_RELEASE);
+	return true;
+}
+
+static bool queue_pop(tdk_icm42688_raw_sample_t* sample)
+{
+	unsigned int tail = __atomic_load_n(&sample_tail, __ATOMIC_RELAXED);
+	if(tail == __atomic_load_n(&sample_head, __ATOMIC_ACQUIRE))
+	{
+		return false;
+	}
+	*sample = sample_queue[tail];
+	__atomic_store_n(&sample_tail, next_queue_index(tail), __ATOMIC_RELEASE);
+	return true;
+}
+
+void tdk_icm42688_default_config(tdk_icm42688_config_t* config)
+{
+	if(config == NULL)
+	{
+		return;
+	}
+	config->odr_hz = TDK_IMU_ODR_HZ;
+	/* Motion-capable defaults; stationary characterization may select narrower ranges. */
+	config->accel_full_scale_g = TDK_ACCEL_FS_4G;
+	config->gyro_full_scale_mdps = TDK_GYRO_FS_250;
+	config->ui_filter_config = 0x77u;
+}
+
+bool tdk_icm42688_init(const tdk_icm42688_config_t* config)
+{
+	uint8_t odr_bits;
+	uint8_t accel_bits;
+	uint8_t gyro_bits;
+	if((config == NULL) || !odr_register_value(config->odr_hz, &odr_bits) ||
+	   !accel_full_scale_bits(config->accel_full_scale_g, &accel_bits) ||
+	   !gyro_full_scale_bits(config->gyro_full_scale_mdps, &gyro_bits))
+	{
+		return false;
+	}
+	memset((void*)&diagnostics, 0, sizeof(diagnostics));
+	sample_head = 0u;
+	sample_tail = 0u;
+	active_config = *config;
+
+	bsp_pmod_spi_init();
+	bsp_pmod_spi_set_mode(BSP_PMOD_SPI_MODE_SINGLE);
+	bsp_pmod_spi_set_timing(10u, 10u, 10u, 10u);
+	delay_us(1000u);
+	diagnostics.who_am_i = read_register(ICM_WHO_AM_I);
+	if((diagnostics.who_am_i != ICM42688_WHO_AM_I) ||
+	   (read_register(ICM_DEVICE_CONFIG) != 0x00u))
+	{
+		return false;
+	}
+
+	write_register(ICM_INT_SOURCE0, 0x00u);
+	write_register(ICM_PWR_MGMT0, 0x00u);
+	delay_ms(1u);
+	write_register(ICM_GYRO_CONFIG0, (uint8_t)((gyro_bits << 5) | odr_bits));
+	write_register(ICM_ACCEL_CONFIG0, (uint8_t)((accel_bits << 5) | odr_bits));
+	write_register(ICM_GYRO_CONFIG1,
+	               (uint8_t)((read_register(ICM_GYRO_CONFIG1) & 0x10u) | 0xcau));
+	write_register(ICM_ACCEL_CONFIG1,
+	               (uint8_t)((read_register(ICM_ACCEL_CONFIG1) & 0xe1u) | 0x14u));
+	write_register(ICM_GYRO_ACCEL_CONFIG0, config->ui_filter_config);
+
+	/* Active-high, push-pull, latched INT1. Status read clears data-ready. */
+	write_register(ICM_INT_CONFIG, ICM_INT1_LATCHED_HIGH_PUSH_PULL);
+	write_register(ICM_INT_CONFIG0, 0x00u);
+	/* Datasheet requires clearing the reset default bit 4 for pin operation. */
+	write_register(ICM_INT_CONFIG1, 0x00u);
+	write_register(ICM_PWR_MGMT0, ICM_PWR_LOW_NOISE);
+	delay_ms(50u);
+
+	if((read_register(ICM_PWR_MGMT0) != ICM_PWR_LOW_NOISE) ||
+	   (read_register(ICM_GYRO_CONFIG0) != (uint8_t)((gyro_bits << 5) | odr_bits)) ||
+	   (read_register(ICM_ACCEL_CONFIG0) != (uint8_t)((accel_bits << 5) | odr_bits)) ||
+	   (read_register(ICM_GYRO_ACCEL_CONFIG0) != config->ui_filter_config) ||
+	   (read_register(ICM_INT_CONFIG) != ICM_INT1_LATCHED_HIGH_PUSH_PULL) ||
+	   (read_register(ICM_INT_CONFIG1) != 0x00u))
+	{
+		write_register(ICM_PWR_MGMT0, 0x00u);
+		return false;
+	}
+	(void)read_register(ICM_INT_STATUS);
+	diagnostics.initialized = true;
+	return true;
+}
+
+bool tdk_icm42688_start(void)
+{
+	if(!diagnostics.initialized || diagnostics.running)
+	{
+		return diagnostics.running;
+	}
+	(void)read_register(ICM_INT_STATUS);
+	bsp_plic_set_priority(BSP_PMOD_IRQ, 1u);
+	write_register(ICM_INT_SOURCE0, ICM_UI_DRDY_INT1_EN);
+	if(read_register(ICM_INT_SOURCE0) != ICM_UI_DRDY_INT1_EN)
+	{
+		bsp_plic_disable(BSP_PMOD_IRQ);
+		return false;
+	}
+	(void)read_register(ICM_INT_STATUS);
+	diagnostics.running = true;
+	bsp_plic_enable(BSP_PMOD_IRQ);
+	return true;
+}
+
+void tdk_icm42688_stop(void)
+{
+	bsp_plic_disable(BSP_PMOD_IRQ);
+	write_register(ICM_INT_SOURCE0, 0x00u);
+	(void)read_register(ICM_INT_STATUS);
+	diagnostics.running = false;
+}
+
+void tdk_icm42688_irq_handler(void)
+{
+	uint64_t timestamp = time_now();
+	__atomic_fetch_add(&diagnostics.irq_count, 1u, __ATOMIC_RELAXED);
+	uint8_t status = read_register(ICM_INT_STATUS);
+	if(!diagnostics.running || ((status & ICM_DATA_READY) == 0u))
+	{
+		__atomic_fetch_add(&diagnostics.spurious_irq_count, 1u, __ATOMIC_RELAXED);
+		return;
+	}
+	uint8_t bytes[14];
+	bsp_pmod_spi_read_regs(ICM_TEMP_DATA1 | ICM_SPI_READ, ICM_SPI_DUMMY,
+	                       bytes, sizeof(bytes));
+	tdk_icm42688_raw_sample_t sample;
+	sample.timestamp_ticks = timestamp;
+	sample.temperature_raw = big_endian_i16(&bytes[0]);
+	for(unsigned int axis = 0 ; axis < 3u ; axis++)
+	{
+		sample.accel_raw[axis] = big_endian_i16(&bytes[2u + 2u * axis]);
+		sample.gyro_raw[axis] = big_endian_i16(&bytes[8u + 2u * axis]);
+	}
+	sample.sequence = (uint32_t)(__atomic_load_n(&diagnostics.sample_count,
+	                                            __ATOMIC_RELAXED) + 1u);
+	if(!raw_sample_valid(&sample))
+	{
+		__atomic_fetch_add(&diagnostics.invalid_sample_count, 1u, __ATOMIC_RELAXED);
+		return;
+	}
+	uint64_t previous_timestamp =
+		__atomic_exchange_n(&diagnostics.last_timestamp_ticks, timestamp,
+		                    __ATOMIC_RELAXED);
+	if(previous_timestamp != 0u)
+	{
+		uint64_t expected_ticks = BSP_CLINT_TIMEBASE_HZ / active_config.odr_hz;
+		uint64_t elapsed = timestamp - previous_timestamp;
+		if((expected_ticks != 0u) && (elapsed > expected_ticks + expected_ticks / 2u))
+		{
+			uint64_t periods = (elapsed + expected_ticks / 2u) / expected_ticks;
+			if(periods > 1u)
+			{
+				__atomic_fetch_add(&diagnostics.estimated_missed_sample_count,
+				                   periods - 1u, __ATOMIC_RELAXED);
+			}
+		}
+	}
+	(void)queue_push_isr(&sample);
+	__atomic_fetch_add(&diagnostics.sample_count, 1u, __ATOMIC_RELAXED);
+}
+
+bool tdk_icm42688_take_measurement(fusion_imu_measurement_t* measurement_out)
+{
+	if(measurement_out == NULL)
+	{
+		return false;
+	}
+	tdk_icm42688_raw_sample_t raw;
+	if(!queue_pop(&raw))
+	{
+		return false;
+	}
+	double accel_scale = (double)active_config.accel_full_scale_g *
+	                     TDK_GRAVITY_MPS2 / 32768.0;
+	double gyro_scale = ((double)active_config.gyro_full_scale_mdps / 1000.0) *
+	                    TDK_DEGREES_TO_RADIANS / 32768.0;
+	measurement_out->timestamp_ticks = raw.timestamp_ticks;
+	for(unsigned int axis = 0 ; axis < 3u ; axis++)
+	{
+		measurement_out->accel_mps2[axis] = (double)raw.accel_raw[axis] * accel_scale;
+		measurement_out->gyro_rps[axis] = (double)raw.gyro_raw[axis] * gyro_scale;
+	}
+	measurement_out->temperature_c = 25.0 + (double)raw.temperature_raw / 132.48;
+	measurement_out->sequence = raw.sequence;
+	return true;
+}
+
+void tdk_icm42688_get_diagnostics(tdk_icm42688_diagnostics_t* diagnostics_out)
+{
+	if(diagnostics_out == NULL)
+	{
+		return;
+	}
+	diagnostics_out->irq_count =
+		__atomic_load_n(&diagnostics.irq_count, __ATOMIC_RELAXED);
+	diagnostics_out->sample_count =
+		__atomic_load_n(&diagnostics.sample_count, __ATOMIC_RELAXED);
+	diagnostics_out->invalid_sample_count =
+		__atomic_load_n(&diagnostics.invalid_sample_count, __ATOMIC_RELAXED);
+	diagnostics_out->spurious_irq_count =
+		__atomic_load_n(&diagnostics.spurious_irq_count, __ATOMIC_RELAXED);
+	diagnostics_out->queue_overflow_count =
+		__atomic_load_n(&diagnostics.queue_overflow_count, __ATOMIC_RELAXED);
+	diagnostics_out->estimated_missed_sample_count =
+		__atomic_load_n(&diagnostics.estimated_missed_sample_count, __ATOMIC_RELAXED);
+	diagnostics_out->last_timestamp_ticks =
+		__atomic_load_n(&diagnostics.last_timestamp_ticks, __ATOMIC_RELAXED);
+	diagnostics_out->who_am_i = diagnostics.who_am_i;
+	diagnostics_out->initialized = diagnostics.initialized;
+	diagnostics_out->running = diagnostics.running;
+}