Add generic IMU fusion and PMOD IRQ verification

soc/software/gps_system/Makefile

@@ -5,9 +5,41 @@ TARGET := gps_system
 # with the common 100 MHz fallback divider.
 BSP_UART_CLOCK_HZ ?= 50000000
 
+# Select one sensor backend, or keep the default read-only WHO_AM_I probe.
+# Accepted values: auto, tdk, st (stm is an alias for st).
+IMU_SENSOR ?= auto
+ifeq ($(IMU_SENSOR),auto)
+IMU_DRIVER_SRCS := src/tdk_icm42688.c src/st_lsm6dsox.c
+IMU_BACKEND_CFLAG := -DIMU_SENSOR_BUILD_AUTO=1
+else ifeq ($(IMU_SENSOR),tdk)
+IMU_DRIVER_SRCS := src/tdk_icm42688.c
+IMU_BACKEND_CFLAG := -DIMU_SENSOR_BUILD_TDK=1
+else ifeq ($(IMU_SENSOR),st)
+IMU_DRIVER_SRCS := src/st_lsm6dsox.c
+IMU_BACKEND_CFLAG := -DIMU_SENSOR_BUILD_ST=1
+else ifeq ($(IMU_SENSOR),stm)
+IMU_DRIVER_SRCS := src/st_lsm6dsox.c
+IMU_BACKEND_CFLAG := -DIMU_SENSOR_BUILD_ST=1
+else
+$(error IMU_SENSOR must be auto, tdk, st or stm)
+endif
+
+# Zero means use the selected backend's default. The legacy TDK variable is
+# accepted so existing build commands remain valid.
+ifdef TDK_IMU_ODR_HZ
+IMU_ODR_HZ ?= $(TDK_IMU_ODR_HZ)
+else
+IMU_ODR_HZ ?= 0
+endif
+IMU_ACCEL_FS_G ?= 0
+IMU_GYRO_FS_DPS ?= 0
+
 APP_SRCS := \
 	gps_test.c \
-	src/tdk_icm42688.c \
+	src/imu_calibration.c \
+	src/imu_sensor.c \
+	src/imu_sensor_profile.c \
+	$(IMU_DRIVER_SRCS) \
 	src/eskf.c \
 	src/fusion_motion.c \
 	src/fusion_engine.c \
@@ -25,13 +57,15 @@ APP_SRCS := \
 
 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) \
+CFLAGS += $(IMU_BACKEND_CFLAG) \
+	-DIMU_ODR_HZ=$(IMU_ODR_HZ) \
+	-DIMU_ACCEL_FS_G=$(IMU_ACCEL_FS_G) \
+	-DIMU_GYRO_FS_DPS=$(IMU_GYRO_FS_DPS) \
 	-DGPS_NAV_SOLVE_TIMER_TICKS=$(GPS_NAV_SOLVE_TIMER_TICKS)
 LDLIBS += -lm
 

soc/software/gps_system/README.md

@@ -2,6 +2,124 @@
 
 Bu notlar GPS bare-metal yaziliminin VCU118 Rocket SoC portu ve interrupt debug sureci icin yazildi.
 
+## Generic IMU Mimarisi (TDK ve ST)
+
+GPS/ESKF artik dogrudan bir sensor surucusune bagli degildir. Veri yolu:
+
+```text
+TDK ICM42688 -----\
+                   -> imu_sensor -> imu_calibration -> fusion_imu_measurement -> motion detector + ESKF
+ST LSM6DSOX ------/
+```
+
+- `src/tdk_icm42688.c`: TDK ICM42688'e ozel SPI, register, interrupt ve
+  ham-veri donusumu.
+- `src/st_lsm6dsox.c`: STEVAL-MKI217V1 uzerindeki ST LSM6DSOX'e ozel SPI,
+  register, foreground polling ve ham-veri donusumu.
+- `src/imu_sensor.c`: ortak init/start/stop/IRQ/service/measurement arayuzu.
+- `src/imu_calibration.c`: markadan bagimsiz bias, 3x3 scale/cross-axis/mounting
+  donusumu ve opsiyonel sicaklik katsayisi.
+- `fusion_measurement.h`, motion detector ve ESKF yalniz SI birimindeki ortak
+  mesaji gorur; aktif sensorun markasini bilmez.
+
+Firmware isletim sistemi kullanmaz. Yeni IMU katmani `malloc` cagirmadan sensor
+kuyruklarini ve butun durumunu statik ayirir. Newlib ve formatli UART destegi
+nedeniyle linker'da heap alani yine bulunur; yeni sensor kodu buna guvenmez.
+Karttaki 1 MiB DTIM test icin genis tutulmustur; firmware tasarim butcesi
+ilerideki 512 KiB DTIM hedefidir. `auto` build iki sensor surucusunu da icerir;
+kesin sensor secimi kullanilmayan surucuyu linkten cikarir.
+
+2026-07-13 tarihli `-Os` bare-metal link boyutlari:
+
+| Secim | text | data | bss | Toplam DTIM image |
+| --- | ---: | ---: | ---: | ---: |
+| `tdk` | 67,256 | 2,496 | 205,160 | 274,912 bayt |
+| `st` | 66,936 | 2,496 | 199,280 | 268,712 bayt |
+| `auto` | 68,888 | 2,496 | 207,560 | 278,944 bayt |
+
+En buyuk `auto` image 512 KiB DTIM'in yaklasik yuzde 53.2'sini kullanir; stack
+ve ilerideki buffer'lar icin 245,344 bayt (yaklasik 239.6 KiB) adres alani
+kalir. Linker bunun 16 KiB'ini varsayilan stack icin ayirir. 1 MiB test
+konfigurasyonunda gorulen ek bosluk yeni statik buffer eklemek icin gerekce
+sayilmamalidir. Host unit-test executable'i bu rakamlara dahil degildir.
+`auto` konfigurasyonu ayrica `BSP_DTIM_SIZE=0x00080000` ile gercekten link
+edilerek 512 KiB sinirinda dogrulanmistir.
+
+### Bare-metal sensor secimi
+
+Otomatik WHO_AM_I algilama varsayilandir:
+
+```sh
+make -C soc/software/gps_system IMU_SENSOR=auto
+```
+
+Otomatik secimde eslesen cihaz bulunana kadar yalniz read-only WHO_AM_I probe
+yapilir. TDK ve ST build'leri acikca su sekilde secilebilir:
+
+```sh
+make -C soc/software/gps_system IMU_SENSOR=tdk \
+  IMU_ODR_HZ=500 IMU_ACCEL_FS_G=4 IMU_GYRO_FS_DPS=250
+
+make -C soc/software/gps_system IMU_SENSOR=st \
+  IMU_ODR_HZ=104 IMU_ACCEL_FS_G=2 IMU_GYRO_FS_DPS=250
+```
+
+`IMU_SENSOR=stm`, `st` seciminin alias'idir. `IMU_ODR_HZ=0`,
+`IMU_ACCEL_FS_G=0` ve `IMU_GYRO_FS_DPS=0` aktif surucunun guvenli varsayilanini
+secer. Bu ozellikle `auto` icin onemlidir; TDK ve ST'nin ortak bir ODR degeri
+yoktur. Desteklenen ODR'ler TDK icin 25/50/100/200/500/1000 Hz, ST icin
+52/104/208 Hz'dir.
+
+TDK veri-hazir interrupt'i ile calisir ve ana dongu `wfi` kullanir. Mevcut ST
+baglantisinda interrupt pini bagli olmadigi icin ST surucusu foreground'da
+ODR'nin iki kati hizda status poll eder; yeni ornekleri sensor ODR'sinde
+uretir. Bu yontem fonksiyoneldir fakat CPU'nun `wfi` ile uyumasini engeller.
+Uzun WLS islemi sirasinda foreground durdugu icin sensorun yeni ornekleri
+overwrite olabilir. Bu nedenle interruptsiz ST yolu stationary bring-up ve log
+testi icindir; hareketli fusion icin kayipsiz bir yol kabul edilmez.
+Uretim baglantisinda ST INT pini eklendiginde yalniz ST backend acquisition
+yontemi interrupt'a cevrilecek (alternatif olarak sensor FIFO ve sensor
+timestamp'i kullanilacak); ortak arayuz ve ESKF degismeyecektir.
+
+Sensor ODR'si ile GPS nav-solve periyodu birbirinden bagimsizdir. Her IMU ve GPS
+olcumu ayni local monotonic CPU tick domaininde kendi olcum zamaniyla tasinir;
+ESKF olaylari timestamp sirasinda isler. TDK interrupt timestamp'i data-ready
+kenarini yakindan temsil eder. Poll edilen ST timestamp'i ise data-ready'nin
+foreground'da goruldugu andir ve bir polling gecikmesi tasir. Hareketli nihai
+sistemde ST INT veya FIFO timestamp kullanma gereksiniminin nedeni budur.
+
+### Kalibrasyon ve sicaklik politikasi
+
+Uc farkli duzeltme birbirinden ayridir:
+
+1. Sensorun fabrika trim'i ve datasheet sensitivity degerleri sensor
+   surucusunde kullanilir.
+2. Kalici, karta ozel bias/scale/eksen/sicaklik katsayilari ortak
+   `imu_calibration_t` icinde uygulanabilir. TDK ve ST ayarlari
+   `calibration_by_backend[]` icinde ayri profillerdir; `auto` secimi bir
+   sensorun kalibrasyonunu digerine uygulayamaz. Varsayilan deger identity +
+   sifir bias'tir; yani varsayilanda olcumu degistirmez.
+3. Fusion engine acilista kart sessizken iki saniyelik ornek toplar. Anlik gyro
+   bias'ini, ilk roll/pitch hizalamasini ve gravity yonundeki accel kalintisini
+   tahmin eder. ESKF bundan sonra residual accel/gyro bias state'lerini izler.
+
+Sicaklik GPS/IMU fusionunun calismasi icin zorunlu degildir. Kisa sureli ve
+dar sicaklik araligindaki ilk testte sicaklik yalniz olculur ve mesajda
+tasinir; ESKF denklemlerini etkilemez. Uzun sure, dis ortam veya sensorun
+isinmasiyla bias degisimi gorulurse sicaklik katsayilari offline loglardan
+cikarilip `temperature_compensation_enabled=true` ile etkinlestirilir. Katsayi
+olmadan otomatik sicaklik duzeltmesi yapmak yanlis bias uretme riski tasir.
+
+Portable kalibrasyon matematigi Linux'ta heap kullanmadan su testle dogrulanir:
+
+```sh
+make -C soc/software/gps_system/tests clean run
+```
+
+Bu test identity davranisini, bias + 3x3 donusumu, opsiyonel sicaklik
+duzeltmesini ve singular matris reddini kontrol eder. Test executable'i host
+icindir ve bare-metal firmware'e linklenmez.
+
 ## Donanim Adresleri ve IRQ Map
 
 Generated DTS/RTL'e gore GPS IP adresleri:

soc/software/gps_system/gps_test.c

@@ -5,29 +5,42 @@
 #include <float.h>
 #include "matrixLib.h"
 #include "gps_macros.h"
+#include "imu_sensor.h"
 #include "navigation_fusion.h"
-#include "tdk_icm42688.h"
 
 int main()
 {
-	tdk_icm42688_config_t imu_config;
-	tdk_icm42688_default_config(&imu_config);
-	bool imu_initialized = tdk_icm42688_init(&imu_config);
+	imu_sensor_config_t imu_config;
+	imu_sensor_default_config(&imu_config);
+	imu_config.odr_hz = IMU_ODR_HZ;
+	imu_config.accel_full_scale_g = IMU_ACCEL_FS_G;
+	imu_config.gyro_full_scale_dps = IMU_GYRO_FS_DPS;
+	bool imu_initialized = imu_sensor_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();
+	bool imu_running = imu_initialized && fusion_initialized && imu_sensor_start();
+	(void)imu_running;
+	imu_sensor_diagnostics_t imu_diagnostics;
+	imu_sensor_get_diagnostics(&imu_diagnostics);
 	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);
+	        "init imu=%u sensor=%s who=0x%02x running=%u odr_hz=%lu "
+	        "poll=%u timebase_hz=%lu\r\n",
+	        imu_initialized ? 1u : 0u,
+	        imu_sensor_backend_name(imu_diagnostics.backend),
+	        (unsigned int)imu_diagnostics.who_am_i,
+	        imu_running ? 1u : 0u,
+	        (unsigned long)imu_diagnostics.configured_odr_hz,
+	        imu_diagnostics.foreground_polling ? 1u : 0u,
+	        (unsigned long)BSP_CLINT_TIMEBASE_HZ);
 
 	acq_start_loop();
 	uint64_t last_fusion_status_tick = 0u;
 
 	while(1)
 	{
-		asm volatile("wfi"); 
+		/* GPS and either IMU backend wake the shared machine interrupt path. */
+		asm volatile("wfi");
 		gps_background_task();
 		calculate_position();
 		if(fusion_initialized)

soc/software/gps_system/header/fusion_engine.h

@@ -5,6 +5,7 @@
 #include <stdint.h>
 
 #include "eskf.h"
+#include "fusion_imu_quality.h"
 #include "fusion_measurement.h"
 #include "fusion_motion.h"
 
@@ -104,6 +105,9 @@ typedef struct {
 
 void fusion_engine_default_config(fusion_engine_config_t* config,
 	                              uint64_t timebase_hz);
+bool fusion_engine_apply_imu_quality(
+	fusion_engine_config_t* config,
+	const fusion_imu_quality_profile_t* profile);
 bool fusion_engine_init(fusion_engine_t* engine,
 	                    const fusion_engine_config_t* config);
 

soc/software/gps_system/header/fusion_imu_quality.h

@@ -0,0 +1,31 @@
+#ifndef FUSION_IMU_QUALITY_H
+#define FUSION_IMU_QUALITY_H
+
+#include <stdbool.h>
+
+#define FUSION_IMU_QUALITY_AXIS_COUNT 3u
+
+/*
+ * Hardware-neutral IMU characteristics.  The nominal noise fields describe
+ * the sensor/board measurement stream.  The process-noise fields are the
+ * deliberately conservative values used by the ESKF and may include timing,
+ * mounting, calibration and temperature uncertainty in addition to white
+ * sensor noise.
+ */
+typedef struct {
+	const char* name;
+	double nominal_accel_noise_density_mps2_sqrt_hz[FUSION_IMU_QUALITY_AXIS_COUNT];
+	double nominal_gyro_noise_density_rps_sqrt_hz[FUSION_IMU_QUALITY_AXIS_COUNT];
+	double process_accel_noise_density_mps2_sqrt_hz[FUSION_IMU_QUALITY_AXIS_COUNT];
+	double process_gyro_noise_density_rps_sqrt_hz[FUSION_IMU_QUALITY_AXIS_COUNT];
+	double accel_bias_random_walk_mps2_sqrt_s[FUSION_IMU_QUALITY_AXIS_COUNT];
+	double gyro_bias_random_walk_rps_sqrt_s[FUSION_IMU_QUALITY_AXIS_COUNT];
+	double calibration_accel_std_limit_mps2;
+	double calibration_gyro_std_limit_rps;
+	double zero_gyro_measurement_std_rps[FUSION_IMU_QUALITY_AXIS_COUNT];
+} fusion_imu_quality_profile_t;
+
+void fusion_imu_quality_default(fusion_imu_quality_profile_t* profile);
+bool fusion_imu_quality_is_valid(const fusion_imu_quality_profile_t* profile);
+
+#endif

soc/software/gps_system/header/imu_calibration.h

@@ -0,0 +1,33 @@
+#ifndef IMU_CALIBRATION_H
+#define IMU_CALIBRATION_H
+
+#include <stdbool.h>
+
+#include "fusion_measurement.h"
+
+/*
+ * Sensor-independent calibration in the IMU body frame.
+ *
+ * corrected = transform * (measured - bias - temperature_coefficient * dT)
+ *
+ * The transform can contain scale, cross-axis and mounting-axis corrections.
+ * Temperature correction is deliberately opt-in; collecting temperature does
+ * not change measurements unless temperature_compensation_enabled is true.
+ */
+typedef struct {
+	double accel_bias_mps2[3];
+	double gyro_bias_rps[3];
+	double accel_transform[3][3];
+	double gyro_transform[3][3];
+	double reference_temperature_c;
+	double accel_temperature_coefficient_mps2_per_c[3];
+	double gyro_temperature_coefficient_rps_per_c[3];
+	bool temperature_compensation_enabled;
+} imu_calibration_t;
+
+void imu_calibration_default(imu_calibration_t* calibration);
+bool imu_calibration_is_valid(const imu_calibration_t* calibration);
+bool imu_calibration_apply(const imu_calibration_t* calibration,
+	                       fusion_imu_measurement_t* measurement);
+
+#endif

soc/software/gps_system/header/imu_sensor.h

@@ -0,0 +1,65 @@
+#ifndef IMU_SENSOR_H
+#define IMU_SENSOR_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "fusion_measurement.h"
+#include "fusion_imu_quality.h"
+#include "imu_calibration.h"
+
+typedef enum {
+	IMU_SENSOR_NONE = 0,
+	IMU_SENSOR_TDK_ICM42688,
+	IMU_SENSOR_ST_LSM6DSOX,
+	IMU_SENSOR_BACKEND_COUNT
+} imu_sensor_backend_t;
+
+typedef struct {
+	/* Zero selects the active backend's safe default. */
+	uint32_t odr_hz;
+	unsigned int accel_full_scale_g;
+	unsigned int gyro_full_scale_dps;
+	/* Auto-detected devices must never share board-specific calibration. */
+	imu_calibration_t calibration_by_backend[IMU_SENSOR_BACKEND_COUNT];
+} imu_sensor_config_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;
+	uint32_t configured_odr_hz;
+	uint8_t who_am_i;
+	imu_sensor_backend_t backend;
+	bool initialized;
+	bool running;
+	bool foreground_polling;
+} imu_sensor_diagnostics_t;
+
+void imu_sensor_default_config(imu_sensor_config_t* config);
+bool imu_sensor_init(const imu_sensor_config_t* config);
+bool imu_sensor_start(void);
+void imu_sensor_stop(void);
+
+/* Interrupt backend hook. Safe no-op for a foreground-polled backend. */
+void imu_sensor_irq_handler(void);
+
+/* Foreground acquisition hook. Safe no-op for an interrupt backend. */
+void imu_sensor_service(void);
+bool imu_sensor_requires_foreground_service(void);
+
+bool imu_sensor_take_measurement(fusion_imu_measurement_t* measurement_out);
+void imu_sensor_get_diagnostics(imu_sensor_diagnostics_t* diagnostics_out);
+imu_sensor_backend_t imu_sensor_active_backend(void);
+const char* imu_sensor_backend_name(imu_sensor_backend_t backend);
+bool imu_sensor_quality_profile_for_backend(
+	imu_sensor_backend_t backend,
+	fusion_imu_quality_profile_t* profile_out);
+bool imu_sensor_active_quality_profile(
+	fusion_imu_quality_profile_t* profile_out);
+
+#endif

soc/software/gps_system/header/st_lsm6dsox.h

@@ -0,0 +1,53 @@
+#ifndef ST_LSM6DSOX_H
+#define ST_LSM6DSOX_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "fusion_measurement.h"
+
+#ifndef ST_IMU_RAW_QUEUE_CAPACITY
+#define ST_IMU_RAW_QUEUE_CAPACITY 256u
+#endif
+
+typedef struct {
+	uint32_t odr_hz;
+	unsigned int accel_full_scale_g;
+	unsigned int gyro_full_scale_dps;
+} st_lsm6dsox_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;
+} st_lsm6dsox_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;
+} st_lsm6dsox_diagnostics_t;
+
+void st_lsm6dsox_default_config(st_lsm6dsox_config_t* config);
+bool st_lsm6dsox_init(const st_lsm6dsox_config_t* config);
+bool st_lsm6dsox_start(void);
+void st_lsm6dsox_stop(void);
+/* Called only by the common PLIC ISR for BSP_PMOD_IRQ. */
+void st_lsm6dsox_irq_handler(void);
+/* Retained as a no-op so every backend has the same service interface. */
+void st_lsm6dsox_service(void);
+bool st_lsm6dsox_take_measurement(
+	fusion_imu_measurement_t* measurement_out);
+void st_lsm6dsox_get_diagnostics(
+	st_lsm6dsox_diagnostics_t* diagnostics_out);
+
+#endif

soc/software/gps_system/src/fusion_engine.c

@@ -48,6 +48,64 @@ static uint64_t seconds_to_ticks(const fusion_engine_t* engine, double seconds)
 	return (uint64_t)(ticks + 0.5);
 }
 
+void fusion_imu_quality_default(fusion_imu_quality_profile_t* profile)
+{
+	if(profile == NULL)
+	{
+		return;
+	}
+	memset(profile, 0, sizeof(*profile));
+	profile->name = "generic-conservative";
+	for(unsigned int axis = 0u ; axis < 3u ; axis++)
+	{
+		/* Nominal fields are informational for the generic fallback. */
+		profile->nominal_accel_noise_density_mps2_sqrt_hz[axis] = 0.001;
+		profile->nominal_gyro_noise_density_rps_sqrt_hz[axis] = 0.0001;
+		profile->process_accel_noise_density_mps2_sqrt_hz[axis] = 0.03;
+		profile->process_gyro_noise_density_rps_sqrt_hz[axis] = 0.003;
+		profile->accel_bias_random_walk_mps2_sqrt_s[axis] = 0.002;
+		profile->gyro_bias_random_walk_rps_sqrt_s[axis] =
+			0.002 * FUSION_PI / 180.0;
+		profile->zero_gyro_measurement_std_rps[axis] =
+			0.01 * FUSION_PI / 180.0;
+	}
+	profile->calibration_accel_std_limit_mps2 = 0.12;
+	profile->calibration_gyro_std_limit_rps = 0.15 * FUSION_PI / 180.0;
+}
+
+bool fusion_imu_quality_is_valid(const fusion_imu_quality_profile_t* profile)
+{
+	if((profile == NULL) || (profile->name == NULL) ||
+	   !isfinite(profile->calibration_accel_std_limit_mps2) ||
+	   !(profile->calibration_accel_std_limit_mps2 > 0.0) ||
+	   !isfinite(profile->calibration_gyro_std_limit_rps) ||
+	   !(profile->calibration_gyro_std_limit_rps > 0.0))
+	{
+		return false;
+	}
+	for(unsigned int axis = 0u ; axis < 3u ; axis++)
+	{
+		const double values[] = {
+			profile->nominal_accel_noise_density_mps2_sqrt_hz[axis],
+			profile->nominal_gyro_noise_density_rps_sqrt_hz[axis],
+			profile->process_accel_noise_density_mps2_sqrt_hz[axis],
+			profile->process_gyro_noise_density_rps_sqrt_hz[axis],
+			profile->accel_bias_random_walk_mps2_sqrt_s[axis],
+			profile->gyro_bias_random_walk_rps_sqrt_s[axis],
+			profile->zero_gyro_measurement_std_rps[axis]
+		};
+		for(unsigned int value = 0u ; value < sizeof(values) / sizeof(values[0]);
+		    value++)
+		{
+			if(!isfinite(values[value]) || !(values[value] > 0.0))
+			{
+				return false;
+			}
+		}
+	}
+	return true;
+}
+
 void fusion_engine_default_config(fusion_engine_config_t* config,
 	                              uint64_t timebase_hz)
 {
@@ -77,6 +135,34 @@ void fusion_engine_default_config(fusion_engine_config_t* config,
 	config->gnss_still_confirmations = 2u;
 }
 
+bool fusion_engine_apply_imu_quality(
+	fusion_engine_config_t* config,
+	const fusion_imu_quality_profile_t* profile)
+{
+	if((config == NULL) || !fusion_imu_quality_is_valid(profile))
+	{
+		return false;
+	}
+	for(unsigned int axis = 0u ; axis < 3u ; axis++)
+	{
+		config->eskf.accel_noise_density[axis] =
+			profile->process_accel_noise_density_mps2_sqrt_hz[axis];
+		config->eskf.gyro_noise_density[axis] =
+			profile->process_gyro_noise_density_rps_sqrt_hz[axis];
+		config->eskf.accel_bias_random_walk[axis] =
+			profile->accel_bias_random_walk_mps2_sqrt_s[axis];
+		config->eskf.gyro_bias_random_walk[axis] =
+			profile->gyro_bias_random_walk_rps_sqrt_s[axis];
+		config->zero_gyro_std_rps[axis] =
+			profile->zero_gyro_measurement_std_rps[axis];
+	}
+	config->calibration_accel_std_limit_mps2 =
+		profile->calibration_accel_std_limit_mps2;
+	config->calibration_gyro_std_limit_rps =
+		profile->calibration_gyro_std_limit_rps;
+	return true;
+}
+
 static bool config_valid(const fusion_engine_config_t* config)
 {
 	return (config != NULL) && (config->timebase_hz != 0u) &&

soc/software/gps_system/src/gps_interrupt_handlers.c

@@ -3,7 +3,7 @@
 #include "Navigation.h"
 #include "gps_pps.h"
 #include "gps_time_anchor.h"
-#include "tdk_icm42688.h"
+#include "imu_sensor.h"
 #include <bsp/irq.h>
 #include <bsp/mmio.h>
 #include <bsp/plic.h>
@@ -397,7 +397,7 @@ void gps_dispatch_irq(unsigned int irq_id)
 {
 	if(irq_id == BSP_PMOD_IRQ)
 	{
-		tdk_icm42688_irq_handler();
+		imu_sensor_irq_handler();
 		return;
 	}
 

soc/software/gps_system/src/imu_calibration.c

@@ -0,0 +1,113 @@
+#include "imu_calibration.h"
+
+#include <math.h>
+#include <stddef.h>
+#include <string.h>
+
+static bool vector_is_finite(const double vector[3])
+{
+	return isfinite(vector[0]) && isfinite(vector[1]) && isfinite(vector[2]);
+}
+
+static bool matrix_is_finite_and_invertible(const double matrix[3][3])
+{
+	for(unsigned int row = 0 ; row < 3u ; row++)
+	{
+		if(!vector_is_finite(matrix[row]))
+		{
+			return false;
+		}
+	}
+	double determinant =
+		matrix[0][0] * (matrix[1][1] * matrix[2][2] -
+		                matrix[1][2] * matrix[2][1]) -
+		matrix[0][1] * (matrix[1][0] * matrix[2][2] -
+		                matrix[1][2] * matrix[2][0]) +
+		matrix[0][2] * (matrix[1][0] * matrix[2][1] -
+		                matrix[1][1] * matrix[2][0]);
+	return isfinite(determinant) && (fabs(determinant) > 1.0e-12);
+}
+
+void imu_calibration_default(imu_calibration_t* calibration)
+{
+	if(calibration == NULL)
+	{
+		return;
+	}
+	memset(calibration, 0, sizeof(*calibration));
+	for(unsigned int axis = 0 ; axis < 3u ; axis++)
+	{
+		calibration->accel_transform[axis][axis] = 1.0;
+		calibration->gyro_transform[axis][axis] = 1.0;
+	}
+	calibration->reference_temperature_c = 25.0;
+}
+
+bool imu_calibration_is_valid(const imu_calibration_t* calibration)
+{
+	if((calibration == NULL) ||
+	   !vector_is_finite(calibration->accel_bias_mps2) ||
+	   !vector_is_finite(calibration->gyro_bias_rps) ||
+	   !matrix_is_finite_and_invertible(calibration->accel_transform) ||
+	   !matrix_is_finite_and_invertible(calibration->gyro_transform) ||
+	   !isfinite(calibration->reference_temperature_c) ||
+	   !vector_is_finite(calibration->accel_temperature_coefficient_mps2_per_c) ||
+	   !vector_is_finite(calibration->gyro_temperature_coefficient_rps_per_c))
+	{
+		return false;
+	}
+	return true;
+}
+
+bool imu_calibration_apply(const imu_calibration_t* calibration,
+	                       fusion_imu_measurement_t* measurement)
+{
+	if(!imu_calibration_is_valid(calibration) || (measurement == NULL) ||
+	   !vector_is_finite(measurement->accel_mps2) ||
+	   !vector_is_finite(measurement->gyro_rps))
+	{
+		return false;
+	}
+	double temperature_delta_c = 0.0;
+	if(calibration->temperature_compensation_enabled)
+	{
+		if(!isfinite(measurement->temperature_c))
+		{
+			return false;
+		}
+		temperature_delta_c =
+			measurement->temperature_c - calibration->reference_temperature_c;
+	}
+
+	double accel_unbiased[3];
+	double gyro_unbiased[3];
+	for(unsigned int axis = 0 ; axis < 3u ; axis++)
+	{
+		accel_unbiased[axis] =
+			measurement->accel_mps2[axis] - calibration->accel_bias_mps2[axis] -
+			calibration->accel_temperature_coefficient_mps2_per_c[axis] *
+				temperature_delta_c;
+		gyro_unbiased[axis] =
+			measurement->gyro_rps[axis] - calibration->gyro_bias_rps[axis] -
+			calibration->gyro_temperature_coefficient_rps_per_c[axis] *
+				temperature_delta_c;
+	}
+
+	double accel_corrected[3] = {0.0};
+	double gyro_corrected[3] = {0.0};
+	for(unsigned int row = 0 ; row < 3u ; row++)
+	{
+		for(unsigned int col = 0 ; col < 3u ; col++)
+		{
+			accel_corrected[row] +=
+				calibration->accel_transform[row][col] * accel_unbiased[col];
+			gyro_corrected[row] +=
+				calibration->gyro_transform[row][col] * gyro_unbiased[col];
+		}
+	}
+	memcpy(measurement->accel_mps2, accel_corrected,
+	       sizeof(measurement->accel_mps2));
+	memcpy(measurement->gyro_rps, gyro_corrected,
+	       sizeof(measurement->gyro_rps));
+	return true;
+}

soc/software/gps_system/src/imu_sensor.c

@@ -0,0 +1,350 @@
+#include "imu_sensor.h"
+
+#include <stddef.h>
+#include <string.h>
+
+#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_TDK)
+#include "tdk_icm42688.h"
+#endif
+
+#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_ST)
+#include "st_lsm6dsox.h"
+#endif
+
+#if !defined(IMU_SENSOR_BUILD_AUTO) && !defined(IMU_SENSOR_BUILD_TDK) && \
+    !defined(IMU_SENSOR_BUILD_ST)
+#error "Select IMU_SENSOR_BUILD_AUTO, IMU_SENSOR_BUILD_TDK or IMU_SENSOR_BUILD_ST"
+#endif
+
+static imu_sensor_backend_t active_backend;
+static imu_calibration_t active_calibration;
+static uint32_t active_odr_hz;
+static uint64_t calibration_rejected_count;
+
+void imu_sensor_default_config(imu_sensor_config_t* config)
+{
+	if(config == NULL)
+	{
+		return;
+	}
+	memset(config, 0, sizeof(*config));
+	for(unsigned int backend = 0u ; backend < IMU_SENSOR_BACKEND_COUNT ; backend++)
+	{
+		imu_calibration_default(&config->calibration_by_backend[backend]);
+	}
+}
+
+#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_TDK)
+static bool tdk_accel_range(unsigned int full_scale_g,
+	                        tdk_accel_full_scale_t* range)
+{
+	if(range == NULL)
+	{
+		return false;
+	}
+	switch(full_scale_g)
+	{
+		case 2u: *range = TDK_ACCEL_FS_2G; return true;
+		case 4u: *range = TDK_ACCEL_FS_4G; return true;
+		case 8u: *range = TDK_ACCEL_FS_8G; return true;
+		case 16u: *range = TDK_ACCEL_FS_16G; return true;
+		default: return false;
+	}
+}
+
+static bool tdk_gyro_range(unsigned int full_scale_dps,
+	                       tdk_gyro_full_scale_mdps_t* range)
+{
+	if(range == NULL)
+	{
+		return false;
+	}
+	switch(full_scale_dps)
+	{
+		case 125u: *range = TDK_GYRO_FS_125; return true;
+		case 250u: *range = TDK_GYRO_FS_250; return true;
+		case 500u: *range = TDK_GYRO_FS_500; return true;
+		case 1000u: *range = TDK_GYRO_FS_1000; return true;
+		case 2000u: *range = TDK_GYRO_FS_2000; return true;
+		default: return false;
+	}
+}
+
+static bool try_tdk(const imu_sensor_config_t* config)
+{
+	if(!imu_calibration_is_valid(
+		&config->calibration_by_backend[IMU_SENSOR_TDK_ICM42688]))
+	{
+		return false;
+	}
+	tdk_icm42688_config_t backend_config;
+	tdk_icm42688_default_config(&backend_config);
+	if(config->odr_hz != 0u)
+	{
+		backend_config.odr_hz = config->odr_hz;
+	}
+	if((config->accel_full_scale_g != 0u) &&
+	   !tdk_accel_range(config->accel_full_scale_g,
+	                    &backend_config.accel_full_scale_g))
+	{
+		return false;
+	}
+	if((config->gyro_full_scale_dps != 0u) &&
+	   !tdk_gyro_range(config->gyro_full_scale_dps,
+	                   &backend_config.gyro_full_scale_mdps))
+	{
+		return false;
+	}
+	if(!tdk_icm42688_init(&backend_config))
+	{
+		return false;
+	}
+	active_backend = IMU_SENSOR_TDK_ICM42688;
+	active_calibration =
+		config->calibration_by_backend[IMU_SENSOR_TDK_ICM42688];
+	active_odr_hz = backend_config.odr_hz;
+	return true;
+}
+#endif
+
+#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_ST)
+static bool try_st(const imu_sensor_config_t* config)
+{
+	if(!imu_calibration_is_valid(
+		&config->calibration_by_backend[IMU_SENSOR_ST_LSM6DSOX]))
+	{
+		return false;
+	}
+	st_lsm6dsox_config_t backend_config;
+	st_lsm6dsox_default_config(&backend_config);
+	if(config->odr_hz != 0u)
+	{
+		backend_config.odr_hz = config->odr_hz;
+	}
+	if(config->accel_full_scale_g != 0u)
+	{
+		backend_config.accel_full_scale_g = config->accel_full_scale_g;
+	}
+	if(config->gyro_full_scale_dps != 0u)
+	{
+		backend_config.gyro_full_scale_dps = config->gyro_full_scale_dps;
+	}
+	if(!st_lsm6dsox_init(&backend_config))
+	{
+		return false;
+	}
+	active_backend = IMU_SENSOR_ST_LSM6DSOX;
+	active_calibration =
+		config->calibration_by_backend[IMU_SENSOR_ST_LSM6DSOX];
+	active_odr_hz = backend_config.odr_hz;
+	return true;
+}
+#endif
+
+bool imu_sensor_init(const imu_sensor_config_t* config)
+{
+	active_backend = IMU_SENSOR_NONE;
+	active_odr_hz = 0u;
+	calibration_rejected_count = 0u;
+	if(config == NULL)
+	{
+		return false;
+	}
+
+#if defined(IMU_SENSOR_BUILD_TDK)
+	return try_tdk(config);
+#elif defined(IMU_SENSOR_BUILD_ST)
+	return try_st(config);
+#else
+	/* WHO_AM_I probing is read-only until a matching backend is found. */
+	if(try_tdk(config))
+	{
+		return true;
+	}
+	return try_st(config);
+#endif
+}
+
+bool imu_sensor_start(void)
+{
+	switch(active_backend)
+	{
+#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_TDK)
+		case IMU_SENSOR_TDK_ICM42688:
+			return tdk_icm42688_start();
+#endif
+#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_ST)
+		case IMU_SENSOR_ST_LSM6DSOX:
+			return st_lsm6dsox_start();
+#endif
+		default:
+			return false;
+	}
+}
+
+void imu_sensor_stop(void)
+{
+	switch(active_backend)
+	{
+#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_TDK)
+		case IMU_SENSOR_TDK_ICM42688:
+			tdk_icm42688_stop();
+			break;
+#endif
+#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_ST)
+		case IMU_SENSOR_ST_LSM6DSOX:
+			st_lsm6dsox_stop();
+			break;
+#endif
+		default:
+			break;
+	}
+}
+
+void imu_sensor_irq_handler(void)
+{
+#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_TDK)
+	if(active_backend == IMU_SENSOR_TDK_ICM42688)
+	{
+		tdk_icm42688_irq_handler();
+	}
+#endif
+#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_ST)
+	if(active_backend == IMU_SENSOR_ST_LSM6DSOX)
+	{
+		st_lsm6dsox_irq_handler();
+	}
+#endif
+}
+
+void imu_sensor_service(void)
+{
+#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_ST)
+	if(active_backend == IMU_SENSOR_ST_LSM6DSOX)
+	{
+		st_lsm6dsox_service();
+	}
+#endif
+}
+
+bool imu_sensor_requires_foreground_service(void)
+{
+	return false;
+}
+
+bool imu_sensor_take_measurement(fusion_imu_measurement_t* measurement_out)
+{
+	if(measurement_out == NULL)
+	{
+		return false;
+	}
+	bool have_measurement = false;
+	switch(active_backend)
+	{
+#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_TDK)
+		case IMU_SENSOR_TDK_ICM42688:
+			have_measurement =
+				tdk_icm42688_take_measurement(measurement_out);
+			break;
+#endif
+#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_ST)
+		case IMU_SENSOR_ST_LSM6DSOX:
+			have_measurement =
+				st_lsm6dsox_take_measurement(measurement_out);
+			break;
+#endif
+		default:
+			break;
+	}
+	if(!have_measurement)
+	{
+		return false;
+	}
+	if(!imu_calibration_apply(&active_calibration, measurement_out))
+	{
+		calibration_rejected_count++;
+		return false;
+	}
+	return true;
+}
+
+void imu_sensor_get_diagnostics(imu_sensor_diagnostics_t* diagnostics_out)
+{
+	if(diagnostics_out == NULL)
+	{
+		return;
+	}
+	memset(diagnostics_out, 0, sizeof(*diagnostics_out));
+	diagnostics_out->backend = active_backend;
+	diagnostics_out->configured_odr_hz = active_odr_hz;
+	diagnostics_out->foreground_polling =
+		imu_sensor_requires_foreground_service();
+
+	switch(active_backend)
+	{
+#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_TDK)
+		case IMU_SENSOR_TDK_ICM42688:
+		{
+			tdk_icm42688_diagnostics_t source;
+			tdk_icm42688_get_diagnostics(&source);
+			diagnostics_out->irq_count = source.irq_count;
+			diagnostics_out->sample_count = source.sample_count;
+			diagnostics_out->invalid_sample_count =
+				source.invalid_sample_count + calibration_rejected_count;
+			diagnostics_out->spurious_irq_count = source.spurious_irq_count;
+			diagnostics_out->queue_overflow_count = source.queue_overflow_count;
+			diagnostics_out->estimated_missed_sample_count =
+				source.estimated_missed_sample_count;
+			diagnostics_out->last_timestamp_ticks = source.last_timestamp_ticks;
+			diagnostics_out->who_am_i = source.who_am_i;
+			diagnostics_out->initialized = source.initialized;
+			diagnostics_out->running = source.running;
+			break;
+		}
+#endif
+#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_ST)
+		case IMU_SENSOR_ST_LSM6DSOX:
+		{
+			st_lsm6dsox_diagnostics_t source;
+			st_lsm6dsox_get_diagnostics(&source);
+			diagnostics_out->irq_count = source.irq_count;
+			diagnostics_out->sample_count = source.sample_count;
+			diagnostics_out->invalid_sample_count =
+				source.invalid_sample_count + calibration_rejected_count;
+			diagnostics_out->spurious_irq_count = source.spurious_irq_count;
+			diagnostics_out->queue_overflow_count = source.queue_overflow_count;
+			diagnostics_out->estimated_missed_sample_count =
+				source.estimated_missed_sample_count;
+			diagnostics_out->last_timestamp_ticks = source.last_timestamp_ticks;
+			diagnostics_out->who_am_i = source.who_am_i;
+			diagnostics_out->initialized = source.initialized;
+			diagnostics_out->running = source.running;
+			break;
+		}
+#endif
+		default:
+			diagnostics_out->invalid_sample_count = calibration_rejected_count;
+			break;
+	}
+}
+
+imu_sensor_backend_t imu_sensor_active_backend(void)
+{
+	return active_backend;
+}
+
+bool imu_sensor_active_quality_profile(
+	fusion_imu_quality_profile_t* profile_out)
+{
+	return imu_sensor_quality_profile_for_backend(active_backend, profile_out);
+}
+
+const char* imu_sensor_backend_name(imu_sensor_backend_t backend)
+{
+	switch(backend)
+	{
+		case IMU_SENSOR_TDK_ICM42688: return "tdk-icm42688";
+		case IMU_SENSOR_ST_LSM6DSOX: return "st-lsm6dsox";
+		default: return "none";
+	}
+}

soc/software/gps_system/src/imu_sensor_profile.c

@@ -0,0 +1,74 @@
+#include "imu_sensor.h"
+
+#include <stddef.h>
+
+#define PROFILE_GRAVITY_MPS2 9.80665
+#define PROFILE_DEGREES_TO_RADIANS 0.017453292519943295769236907684886
+
+static void set_all(double values[3], double value)
+{
+	for(unsigned int axis = 0u ; axis < 3u ; axis++)
+	{
+		values[axis] = value;
+	}
+}
+
+bool imu_sensor_quality_profile_for_backend(
+	imu_sensor_backend_t backend,
+	fusion_imu_quality_profile_t* profile_out)
+{
+	if(profile_out == NULL)
+	{
+		return false;
+	}
+	fusion_imu_quality_default(profile_out);
+	switch(backend)
+	{
+		case IMU_SENSOR_TDK_ICM42688:
+			profile_out->name = "tdk-icm42688";
+			/* ICM-42688-P: 65/65/70 ug/sqrt(Hz), 2.8 mdps/sqrt(Hz). */
+			profile_out->nominal_accel_noise_density_mps2_sqrt_hz[0] =
+				65.0e-6 * PROFILE_GRAVITY_MPS2;
+			profile_out->nominal_accel_noise_density_mps2_sqrt_hz[1] =
+				65.0e-6 * PROFILE_GRAVITY_MPS2;
+			profile_out->nominal_accel_noise_density_mps2_sqrt_hz[2] =
+				70.0e-6 * PROFILE_GRAVITY_MPS2;
+			set_all(profile_out->nominal_gyro_noise_density_rps_sqrt_hz,
+			        2.8e-3 * PROFILE_DEGREES_TO_RADIANS);
+			/* Conservative fusion values; tune bias RW with long Allan logs. */
+			set_all(profile_out->process_accel_noise_density_mps2_sqrt_hz, 0.025);
+			set_all(profile_out->process_gyro_noise_density_rps_sqrt_hz, 0.0022);
+			set_all(profile_out->accel_bias_random_walk_mps2_sqrt_s, 0.0015);
+			set_all(profile_out->gyro_bias_random_walk_rps_sqrt_s,
+			        0.002 * PROFILE_DEGREES_TO_RADIANS);
+			profile_out->calibration_accel_std_limit_mps2 = 0.09;
+			profile_out->calibration_gyro_std_limit_rps =
+				0.10 * PROFILE_DEGREES_TO_RADIANS;
+			set_all(profile_out->zero_gyro_measurement_std_rps,
+			        0.02 * PROFILE_DEGREES_TO_RADIANS);
+			break;
+
+		case IMU_SENSOR_ST_LSM6DSOX:
+			profile_out->name = "st-lsm6dsox";
+			/* LSM6DSOX at +-2 g: 70 ug/sqrt(Hz), 3.8 mdps/sqrt(Hz). */
+			set_all(profile_out->nominal_accel_noise_density_mps2_sqrt_hz,
+			        70.0e-6 * PROFILE_GRAVITY_MPS2);
+			set_all(profile_out->nominal_gyro_noise_density_rps_sqrt_hz,
+			        3.8e-3 * PROFILE_DEGREES_TO_RADIANS);
+			set_all(profile_out->process_accel_noise_density_mps2_sqrt_hz, 0.03);
+			set_all(profile_out->process_gyro_noise_density_rps_sqrt_hz, 0.0030);
+			set_all(profile_out->accel_bias_random_walk_mps2_sqrt_s, 0.0025);
+			set_all(profile_out->gyro_bias_random_walk_rps_sqrt_s,
+			        0.004 * PROFILE_DEGREES_TO_RADIANS);
+			profile_out->calibration_accel_std_limit_mps2 = 0.12;
+			profile_out->calibration_gyro_std_limit_rps =
+				0.20 * PROFILE_DEGREES_TO_RADIANS;
+			set_all(profile_out->zero_gyro_measurement_std_rps,
+			        0.04 * PROFILE_DEGREES_TO_RADIANS);
+			break;
+
+		default:
+			return false;
+	}
+	return fusion_imu_quality_is_valid(profile_out);
+}

soc/software/gps_system/src/navigation_fusion.c

@@ -2,7 +2,7 @@
 
 #include "fusion_bus.h"
 #include "fusion_gps_adapter.h"
-#include "tdk_icm42688.h"
+#include "imu_sensor.h"
 
 #include <math.h>
 #include <stddef.h>
@@ -23,6 +23,12 @@ bool navigation_fusion_init(uint64_t local_timebase_hz)
 	}
 	fusion_bus_init(&measurement_bus);
 	fusion_engine_default_config(&engine_config, local_timebase_hz);
+	fusion_imu_quality_profile_t imu_profile;
+	if(imu_sensor_active_quality_profile(&imu_profile) &&
+	   !fusion_engine_apply_imu_quality(&engine_config, &imu_profile))
+	{
+		return false;
+	}
 	fusion_gps_adapter_default_config(&adapter_config);
 	if(!fusion_engine_init(&fusion_engine, &engine_config) ||
 	   !fusion_gps_adapter_init(&gps_adapter, &adapter_config))
@@ -46,8 +52,9 @@ unsigned int navigation_fusion_service(void)
 		return 0u;
 	}
 	unsigned int processed = 0u;
+	imu_sensor_service();
 	fusion_imu_measurement_t imu;
-	while(tdk_icm42688_take_measurement(&imu))
+	while(imu_sensor_take_measurement(&imu))
 	{
 		(void)fusion_engine_push_imu(&fusion_engine, &imu);
 		processed++;

soc/software/gps_system/src/st_lsm6dsox.c

@@ -0,0 +1,503 @@
+#include "st_lsm6dsox.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 LSM6DSOX_WHO_AM_I             0x0fu
+#define LSM6DSOX_COUNTER_BDR_REG1      0x0bu
+#define LSM6DSOX_INT1_CTRL             0x0du
+#define LSM6DSOX_INT2_CTRL             0x0eu
+#define LSM6DSOX_CTRL1_XL             0x10u
+#define LSM6DSOX_CTRL2_G              0x11u
+#define LSM6DSOX_CTRL3_C              0x12u
+#define LSM6DSOX_STATUS_REG           0x1eu
+#define LSM6DSOX_TAP_CFG2              0x58u
+#define LSM6DSOX_OUT_TEMP_L           0x20u
+#define LSM6DSOX_OUTX_L_G             0x22u
+#define LSM6DSOX_OUTX_L_A             0x28u
+
+#define LSM6DSOX_SPI_READ             0x80u
+#define LSM6DSOX_SPI_DUMMY            0xffu
+#define LSM6DSOX_WHO_AM_I_VALUE       0x6cu
+#define LSM6DSOX_CTRL3_SW_RESET       0x01u
+#define LSM6DSOX_CTRL3_IF_INC         0x04u
+#define LSM6DSOX_CTRL3_BDU            0x40u
+#define LSM6DSOX_STATUS_XLDA          0x01u
+#define LSM6DSOX_STATUS_GDA           0x02u
+#define LSM6DSOX_INT1_DRDY_XL         0x01u
+#define LSM6DSOX_DATAREADY_PULSED     0x80u
+#define LSM6DSOX_INTERRUPTS_ENABLE    0x80u
+#define LSM6DSOX_GYRO_READY_TIMEOUT_US 100u
+
+#define ST_GRAVITY_MPS2 9.80665
+#define ST_DEGREES_TO_RADIANS 0.017453292519943295769236907684886
+
+#if (ST_IMU_RAW_QUEUE_CAPACITY < 2u)
+#error "ST_IMU_RAW_QUEUE_CAPACITY must be at least 2"
+#endif
+
+static st_lsm6dsox_config_t active_config;
+static st_lsm6dsox_raw_sample_t sample_queue[ST_IMU_RAW_QUEUE_CAPACITY];
+static volatile unsigned int sample_head;
+static volatile unsigned int sample_tail;
+static volatile st_lsm6dsox_diagnostics_t diagnostics;
+
+static uint64_t time_now(void)
+{
+	return bsp_read64(BSP_CLINT_MTIME);
+}
+
+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)~LSM6DSOX_SPI_READ, value);
+}
+
+static uint8_t read_register(uint8_t reg)
+{
+	return bsp_pmod_spi_read_reg8(reg | LSM6DSOX_SPI_READ,
+	                              LSM6DSOX_SPI_DUMMY);
+}
+
+static int16_t little_endian_i16(const uint8_t* bytes)
+{
+	return (int16_t)((uint16_t)bytes[0] | ((uint16_t)bytes[1] << 8));
+}
+
+static bool odr_register_value(uint32_t odr_hz, uint8_t* value)
+{
+	if(value == NULL)
+	{
+		return false;
+	}
+	switch(odr_hz)
+	{
+		case 52u:  *value = 0x30u; return true;
+		case 104u: *value = 0x40u; return true;
+		case 208u: *value = 0x50u; return true;
+		default: return false;
+	}
+}
+
+static bool accel_full_scale_bits(unsigned int full_scale_g, uint8_t* bits,
+	                              double* mg_per_lsb)
+{
+	if((bits == NULL) || (mg_per_lsb == NULL))
+	{
+		return false;
+	}
+	switch(full_scale_g)
+	{
+		case 2u:  *bits = 0x00u; *mg_per_lsb = 0.061; return true;
+		case 4u:  *bits = 0x08u; *mg_per_lsb = 0.122; return true;
+		case 8u:  *bits = 0x0cu; *mg_per_lsb = 0.244; return true;
+		case 16u: *bits = 0x04u; *mg_per_lsb = 0.488; return true;
+		default: return false;
+	}
+}
+
+static bool gyro_full_scale_bits(unsigned int full_scale_dps, uint8_t* bits,
+	                             double* mdps_per_lsb)
+{
+	if((bits == NULL) || (mdps_per_lsb == NULL))
+	{
+		return false;
+	}
+	switch(full_scale_dps)
+	{
+		case 125u:  *bits = 0x02u; *mdps_per_lsb = 4.375; return true;
+		case 250u:  *bits = 0x00u; *mdps_per_lsb = 8.750; return true;
+		case 500u:  *bits = 0x04u; *mdps_per_lsb = 17.50; return true;
+		case 1000u: *bits = 0x08u; *mdps_per_lsb = 35.00; return true;
+		case 2000u: *bits = 0x0cu; *mdps_per_lsb = 70.00; return true;
+		default: return false;
+	}
+}
+
+static unsigned int next_queue_index(unsigned int index)
+{
+	index++;
+	return (index == ST_IMU_RAW_QUEUE_CAPACITY) ? 0u : index;
+}
+
+static bool raw_sample_valid(const st_lsm6dsox_raw_sample_t* sample)
+{
+	if((sample == NULL) || (sample->temperature_raw == INT16_MIN))
+	{
+		return false;
+	}
+	for(unsigned int axis = 0u ; 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 st_lsm6dsox_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(st_lsm6dsox_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;
+}
+
+static void read_raw_outputs(st_lsm6dsox_raw_sample_t* sample,
+	                         uint64_t timestamp_ticks)
+{
+	uint8_t bytes[14];
+	bsp_pmod_spi_read_regs(LSM6DSOX_OUT_TEMP_L | LSM6DSOX_SPI_READ,
+	                       LSM6DSOX_SPI_DUMMY, bytes, 2u);
+	/* Temperature and gyro/accel are separate internal output blocks. */
+	bsp_pmod_spi_read_regs(LSM6DSOX_OUTX_L_G | LSM6DSOX_SPI_READ,
+	                       LSM6DSOX_SPI_DUMMY, &bytes[2], 12u);
+	sample->timestamp_ticks = timestamp_ticks;
+	sample->temperature_raw = little_endian_i16(&bytes[0]);
+	for(unsigned int axis = 0u ; axis < 3u ; axis++)
+	{
+		sample->gyro_raw[axis] = little_endian_i16(&bytes[2u + 2u * axis]);
+		sample->accel_raw[axis] = little_endian_i16(&bytes[8u + 2u * axis]);
+	}
+}
+
+static void read_raw_accel(st_lsm6dsox_raw_sample_t* sample)
+{
+	uint8_t bytes[6];
+	bsp_pmod_spi_read_regs(LSM6DSOX_OUTX_L_A | LSM6DSOX_SPI_READ,
+	                       LSM6DSOX_SPI_DUMMY, bytes, sizeof(bytes));
+	for(unsigned int axis = 0u ; axis < 3u ; axis++)
+	{
+		sample->accel_raw[axis] = little_endian_i16(&bytes[2u * axis]);
+	}
+}
+
+static void read_raw_gyro_temperature(st_lsm6dsox_raw_sample_t* sample)
+{
+	uint8_t bytes[8];
+	bsp_pmod_spi_read_regs(LSM6DSOX_OUT_TEMP_L | LSM6DSOX_SPI_READ,
+	                       LSM6DSOX_SPI_DUMMY, bytes, 2u);
+	bsp_pmod_spi_read_regs(LSM6DSOX_OUTX_L_G | LSM6DSOX_SPI_READ,
+	                       LSM6DSOX_SPI_DUMMY, &bytes[2], 6u);
+	sample->temperature_raw = little_endian_i16(&bytes[0]);
+	for(unsigned int axis = 0u ; axis < 3u ; axis++)
+	{
+		sample->gyro_raw[axis] = little_endian_i16(&bytes[2u + 2u * axis]);
+	}
+}
+
+void st_lsm6dsox_default_config(st_lsm6dsox_config_t* config)
+{
+	if(config == NULL)
+	{
+		return;
+	}
+	config->odr_hz = 104u;
+	config->accel_full_scale_g = 2u;
+	config->gyro_full_scale_dps = 250u;
+}
+
+bool st_lsm6dsox_init(const st_lsm6dsox_config_t* config)
+{
+	uint8_t odr_bits;
+	uint8_t accel_bits;
+	uint8_t gyro_bits;
+	double unused_accel_scale;
+	double unused_gyro_scale;
+	if((config == NULL) || !odr_register_value(config->odr_hz, &odr_bits) ||
+	   !accel_full_scale_bits(config->accel_full_scale_g, &accel_bits,
+	                          &unused_accel_scale) ||
+	   !gyro_full_scale_bits(config->gyro_full_scale_dps, &gyro_bits,
+	                         &unused_gyro_scale))
+	{
+		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_clock_mode(BSP_PMOD_SPI_CLOCK_MODE0);
+	bsp_pmod_spi_set_timing(2u, 2u, 2u, 5u);
+	delay_ms(20u);
+	diagnostics.who_am_i = read_register(LSM6DSOX_WHO_AM_I);
+	if(diagnostics.who_am_i != LSM6DSOX_WHO_AM_I_VALUE)
+	{
+		return false;
+	}
+
+	write_register(LSM6DSOX_CTRL3_C, LSM6DSOX_CTRL3_SW_RESET);
+	uint64_t reset_start = time_now();
+	uint64_t reset_timeout =
+		((uint64_t)BSP_CLINT_TIMEBASE_HZ * 100u + 999u) / 1000u;
+	while((read_register(LSM6DSOX_CTRL3_C) & LSM6DSOX_CTRL3_SW_RESET) != 0u)
+	{
+		if((time_now() - reset_start) >= reset_timeout)
+		{
+			return false;
+		}
+	}
+	delay_ms(10u);
+
+	write_register(LSM6DSOX_CTRL3_C,
+	               LSM6DSOX_CTRL3_BDU | LSM6DSOX_CTRL3_IF_INC);
+	/* Active-high push-pull is the CTRL3_C reset polarity/driver setting. */
+	write_register(LSM6DSOX_INT1_CTRL, 0u);
+	write_register(LSM6DSOX_INT2_CTRL, 0u);
+	write_register(LSM6DSOX_TAP_CFG2,
+	               read_register(LSM6DSOX_TAP_CFG2) &
+	               (uint8_t)~LSM6DSOX_INTERRUPTS_ENABLE);
+	write_register(LSM6DSOX_COUNTER_BDR_REG1, 0u);
+	write_register(LSM6DSOX_CTRL1_XL, (uint8_t)(odr_bits | accel_bits));
+	write_register(LSM6DSOX_CTRL2_G, (uint8_t)(odr_bits | gyro_bits));
+	delay_ms(100u);
+	if((read_register(LSM6DSOX_CTRL3_C) !=
+	    (LSM6DSOX_CTRL3_BDU | LSM6DSOX_CTRL3_IF_INC)) ||
+	   (read_register(LSM6DSOX_CTRL1_XL) != (uint8_t)(odr_bits | accel_bits)) ||
+	   (read_register(LSM6DSOX_CTRL2_G) != (uint8_t)(odr_bits | gyro_bits)) ||
+	   (read_register(LSM6DSOX_INT1_CTRL) != 0u) ||
+	   (read_register(LSM6DSOX_INT2_CTRL) != 0u) ||
+	   ((read_register(LSM6DSOX_TAP_CFG2) &
+	     LSM6DSOX_INTERRUPTS_ENABLE) != 0u) ||
+	   ((read_register(LSM6DSOX_COUNTER_BDR_REG1) &
+	     LSM6DSOX_DATAREADY_PULSED) != 0u))
+	{
+		st_lsm6dsox_stop();
+		return false;
+	}
+	diagnostics.initialized = true;
+	return true;
+}
+
+bool st_lsm6dsox_start(void)
+{
+	if(!diagnostics.initialized || diagnostics.running)
+	{
+		return diagnostics.running;
+	}
+	bsp_plic_disable(BSP_PMOD_IRQ);
+	write_register(LSM6DSOX_INT1_CTRL, 0u);
+	write_register(LSM6DSOX_INT2_CTRL, 0u);
+	write_register(LSM6DSOX_TAP_CFG2,
+	               read_register(LSM6DSOX_TAP_CFG2) &
+	               (uint8_t)~LSM6DSOX_INTERRUPTS_ENABLE);
+	write_register(LSM6DSOX_COUNTER_BDR_REG1, 0u);
+	/* Clear an old latched data-ready before exposing the PLIC source. */
+	if((read_register(LSM6DSOX_STATUS_REG) & LSM6DSOX_STATUS_XLDA) != 0u)
+	{
+		st_lsm6dsox_raw_sample_t stale;
+		read_raw_outputs(&stale, time_now());
+	}
+	/* Route accel DRDY to the wired sensor INT1 pad, then globally enable the
+	 * physical interrupt pads as required by ST's reference driver. */
+	write_register(LSM6DSOX_INT1_CTRL, LSM6DSOX_INT1_DRDY_XL);
+	write_register(LSM6DSOX_TAP_CFG2,
+	               read_register(LSM6DSOX_TAP_CFG2) |
+	               LSM6DSOX_INTERRUPTS_ENABLE);
+	if((read_register(LSM6DSOX_INT1_CTRL) != LSM6DSOX_INT1_DRDY_XL) ||
+	   (read_register(LSM6DSOX_INT2_CTRL) != 0u) ||
+	   ((read_register(LSM6DSOX_TAP_CFG2) &
+	     LSM6DSOX_INTERRUPTS_ENABLE) == 0u) ||
+	   ((read_register(LSM6DSOX_COUNTER_BDR_REG1) &
+	     LSM6DSOX_DATAREADY_PULSED) != 0u))
+	{
+		write_register(LSM6DSOX_INT1_CTRL, 0u);
+		write_register(LSM6DSOX_INT2_CTRL, 0u);
+		write_register(LSM6DSOX_TAP_CFG2,
+		               read_register(LSM6DSOX_TAP_CFG2) &
+		               (uint8_t)~LSM6DSOX_INTERRUPTS_ENABLE);
+		return false;
+	}
+	bsp_plic_set_priority(BSP_PMOD_IRQ, 1u);
+	diagnostics.running = true;
+	bsp_plic_enable(BSP_PMOD_IRQ);
+	return true;
+}
+
+void st_lsm6dsox_stop(void)
+{
+	bsp_plic_disable(BSP_PMOD_IRQ);
+	if(diagnostics.who_am_i == LSM6DSOX_WHO_AM_I_VALUE)
+	{
+		write_register(LSM6DSOX_INT1_CTRL, 0u);
+		write_register(LSM6DSOX_INT2_CTRL, 0u);
+		write_register(LSM6DSOX_TAP_CFG2,
+		               read_register(LSM6DSOX_TAP_CFG2) &
+		               (uint8_t)~LSM6DSOX_INTERRUPTS_ENABLE);
+		write_register(LSM6DSOX_CTRL1_XL, 0u);
+		write_register(LSM6DSOX_CTRL2_G, 0u);
+	}
+	diagnostics.running = false;
+}
+
+void st_lsm6dsox_irq_handler(void)
+{
+	const uint64_t timestamp = time_now();
+	__atomic_fetch_add(&diagnostics.irq_count, 1u, __ATOMIC_RELAXED);
+	if(!__atomic_load_n(&diagnostics.running, __ATOMIC_RELAXED))
+	{
+		__atomic_fetch_add(&diagnostics.spurious_irq_count, 1u,
+		                   __ATOMIC_RELAXED);
+		return;
+	}
+	uint8_t status = read_register(LSM6DSOX_STATUS_REG);
+	st_lsm6dsox_raw_sample_t sample;
+	sample.timestamp_ticks = timestamp;
+	/* Always clear the routed, latched accel source before returning. */
+	read_raw_accel(&sample);
+	if((status & LSM6DSOX_STATUS_XLDA) == 0u)
+	{
+		__atomic_fetch_add(&diagnostics.spurious_irq_count, 1u,
+		                   __ATOMIC_RELAXED);
+		return;
+	}
+	uint64_t wait_start = time_now();
+	uint64_t timeout_ticks =
+		((uint64_t)BSP_CLINT_TIMEBASE_HZ * LSM6DSOX_GYRO_READY_TIMEOUT_US +
+		 999999u) / 1000000u;
+	while((read_register(LSM6DSOX_STATUS_REG) & LSM6DSOX_STATUS_GDA) == 0u)
+	{
+		if((time_now() - wait_start) >= timeout_ticks)
+		{
+			__atomic_fetch_add(&diagnostics.invalid_sample_count, 1u,
+			                   __ATOMIC_RELAXED);
+			return;
+		}
+	}
+	read_raw_gyro_temperature(&sample);
+	if(!raw_sample_valid(&sample))
+	{
+		__atomic_fetch_add(&diagnostics.invalid_sample_count, 1u,
+		                   __ATOMIC_RELAXED);
+		return;
+	}
+	sample.sequence = (uint32_t)(
+		__atomic_load_n(&diagnostics.sample_count, __ATOMIC_RELAXED) + 1u);
+	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);
+}
+
+void st_lsm6dsox_service(void)
+{
+}
+
+bool st_lsm6dsox_take_measurement(
+	fusion_imu_measurement_t* measurement_out)
+{
+	if(measurement_out == NULL)
+	{
+		return false;
+	}
+	st_lsm6dsox_raw_sample_t raw;
+	if(!queue_pop(&raw))
+	{
+		return false;
+	}
+	double accel_mg_per_lsb;
+	double gyro_mdps_per_lsb;
+	uint8_t unused_bits;
+	if(!accel_full_scale_bits(active_config.accel_full_scale_g, &unused_bits,
+	                          &accel_mg_per_lsb) ||
+	   !gyro_full_scale_bits(active_config.gyro_full_scale_dps, &unused_bits,
+	                         &gyro_mdps_per_lsb))
+	{
+		__atomic_fetch_add(&diagnostics.invalid_sample_count, 1u,
+		                   __ATOMIC_RELAXED);
+		return false;
+	}
+	memset(measurement_out, 0, sizeof(*measurement_out));
+	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_mg_per_lsb *
+			ST_GRAVITY_MPS2 / 1000.0;
+		measurement_out->gyro_rps[axis] =
+			(double)raw.gyro_raw[axis] * gyro_mdps_per_lsb *
+			ST_DEGREES_TO_RADIANS / 1000.0;
+	}
+	measurement_out->temperature_c =
+		25.0 + (double)raw.temperature_raw / 256.0;
+	measurement_out->sequence = raw.sequence;
+	return true;
+}
+
+void st_lsm6dsox_get_diagnostics(
+	st_lsm6dsox_diagnostics_t* diagnostics_out)
+{
+	if(diagnostics_out != NULL)
+	{
+		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;
+	}
+}

soc/software/gps_system/tests/Makefile

@@ -0,0 +1,18 @@
+CC ?= gcc
+CFLAGS ?= -O2 -std=c11 -Wall -Wextra -Werror
+
+ROOT := ..
+TARGET := test_imu_calibration
+
+.PHONY: all run clean
+
+all: $(TARGET)
+
+$(TARGET): test_imu_calibration.c $(ROOT)/src/imu_calibration.c
+	$(CC) $(CFLAGS) -I$(ROOT)/header $^ -lm -o $@
+
+run: $(TARGET)
+	./$(TARGET)
+
+clean:
+	rm -f $(TARGET)

soc/software/gps_system/tests/test_imu_calibration.c

@@ -0,0 +1,78 @@
+#include "imu_calibration.h"
+
+#include <assert.h>
+#include <math.h>
+#include <stdio.h>
+#include <string.h>
+
+static bool near(double actual, double expected)
+{
+	return fabs(actual - expected) < 1.0e-12;
+}
+
+static void test_identity_is_noop(void)
+{
+	imu_calibration_t calibration;
+	imu_calibration_default(&calibration);
+	fusion_imu_measurement_t measurement = {
+		.accel_mps2 = {1.0, -2.0, 9.7},
+		.gyro_rps = {0.01, -0.02, 0.03},
+		.temperature_c = 41.0
+	};
+	fusion_imu_measurement_t expected = measurement;
+	assert(imu_calibration_apply(&calibration, &measurement));
+	assert(memcmp(measurement.accel_mps2, expected.accel_mps2,
+	              sizeof(measurement.accel_mps2)) == 0);
+	assert(memcmp(measurement.gyro_rps, expected.gyro_rps,
+	              sizeof(measurement.gyro_rps)) == 0);
+}
+
+static void test_bias_transform_and_temperature(void)
+{
+	imu_calibration_t calibration;
+	imu_calibration_default(&calibration);
+	for(unsigned int axis = 0 ; axis < 3u ; axis++)
+	{
+		calibration.accel_bias_mps2[axis] = 1.0;
+		calibration.gyro_bias_rps[axis] = 0.1;
+		calibration.accel_temperature_coefficient_mps2_per_c[axis] = 0.1;
+		calibration.gyro_temperature_coefficient_rps_per_c[axis] = 0.01;
+	}
+	memset(calibration.accel_transform, 0,
+	       sizeof(calibration.accel_transform));
+	calibration.accel_transform[0][1] = 1.0;
+	calibration.accel_transform[1][0] = -1.0;
+	calibration.accel_transform[2][2] = 2.0;
+	calibration.reference_temperature_c = 25.0;
+	calibration.temperature_compensation_enabled = true;
+
+	fusion_imu_measurement_t measurement = {
+		.accel_mps2 = {3.0, 5.0, 7.0},
+		.gyro_rps = {0.4, 0.5, 0.6},
+		.temperature_c = 35.0
+	};
+	assert(imu_calibration_apply(&calibration, &measurement));
+	assert(near(measurement.accel_mps2[0], 3.0));
+	assert(near(measurement.accel_mps2[1], -1.0));
+	assert(near(measurement.accel_mps2[2], 10.0));
+	assert(near(measurement.gyro_rps[0], 0.2));
+	assert(near(measurement.gyro_rps[1], 0.3));
+	assert(near(measurement.gyro_rps[2], 0.4));
+}
+
+static void test_singular_transform_is_rejected(void)
+{
+	imu_calibration_t calibration;
+	imu_calibration_default(&calibration);
+	memset(calibration.gyro_transform, 0, sizeof(calibration.gyro_transform));
+	assert(!imu_calibration_is_valid(&calibration));
+}
+
+int main(void)
+{
+	test_identity_is_noop();
+	test_bias_transform_and_temperature();
+	test_singular_transform_is_rejected();
+	puts("imu_calibration tests passed");
+	return 0;
+}