Add generic IMU fusion and PMOD IRQ verification

2026-07-14 11:03:13 +03:00
parent e77651dac3
commit dd6569c025
35 changed files with 2680 additions and 70 deletions
@@ -5,10 +5,11 @@ LDLIBS ?= -lm

 GPS_TIME_SOURCE := ../../gps_system/src/gps_time_anchor.c
 ESKF_SOURCES := ../../gps_system/src/eskf.c ../../gps_system/src/fusion_motion.c ../../gps_system/src/fusion_engine.c ../../gps_system/src/fusion_gps_adapter.c ../../gps_system/src/fusion_bus.c
+IMU_PROFILE_SOURCE := ../../gps_system/src/imu_sensor_profile.c

 .PHONY: all test sanitize profile benchmark clean

-all: standalone_gps_integrity_test eskf_motion_test fusion_synthetic_test
+all: standalone_gps_integrity_test eskf_motion_test fusion_synthetic_test imu_quality_synthetic_test

 standalone_gps_integrity_test: standalone_gps_integrity_test.c $(GPS_TIME_SOURCE)
 	$(CC) $(CFLAGS) $^ -o $@ $(LDLIBS)
@@ -19,10 +20,14 @@ eskf_motion_test: eskf_motion_test.c $(ESKF_SOURCES)
 fusion_synthetic_test: fusion_synthetic_test.c $(ESKF_SOURCES)
 	$(CC) $(CFLAGS) $^ -o $@ $(LDLIBS)

-test: standalone_gps_integrity_test eskf_motion_test fusion_synthetic_test
+imu_quality_synthetic_test: imu_quality_synthetic_test.c $(ESKF_SOURCES) $(IMU_PROFILE_SOURCE)
+	$(CC) $(CFLAGS) $^ -o $@ $(LDLIBS)
+
+test: standalone_gps_integrity_test eskf_motion_test fusion_synthetic_test imu_quality_synthetic_test
 	./standalone_gps_integrity_test
 	./eskf_motion_test
 	./fusion_synthetic_test
+	./imu_quality_synthetic_test

 sanitize:
 	$(CC) -std=c11 -O1 -g -Wall -Wextra -Wpedantic -Werror \
@@ -37,6 +42,12 @@ sanitize:
 		-o fusion_synthetic_test_san $(LDLIBS)
 	ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \
 		UBSAN_OPTIONS=halt_on_error=1 ./fusion_synthetic_test_san
+	$(CC) -std=c11 -O1 -g -Wall -Wextra -Wpedantic -Werror \
+		-fsanitize=address,undefined -fno-omit-frame-pointer \
+		-I../../gps_system/header imu_quality_synthetic_test.c $(ESKF_SOURCES) \
+		$(IMU_PROFILE_SOURCE) -o imu_quality_synthetic_test_san $(LDLIBS)
+	ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \
+		UBSAN_OPTIONS=halt_on_error=1 ./imu_quality_synthetic_test_san

 profile:
 	$(CC) -std=c11 -O2 -g -pg -Wall -Wextra -Wpedantic \
@@ -51,4 +62,4 @@ benchmark: standalone_gps_integrity_test
 	./standalone_gps_integrity_test --bench-double 100000

 clean:
-	rm -f standalone_gps_integrity_test standalone_gps_integrity_test_san standalone_gps_integrity_test_gprof eskf_motion_test eskf_motion_test_san fusion_synthetic_test fusion_synthetic_test_san fusion_synthetic_test_gprof fusion_synthetic_trace gmon.out
+	rm -f standalone_gps_integrity_test standalone_gps_integrity_test_san standalone_gps_integrity_test_gprof eskf_motion_test eskf_motion_test_san fusion_synthetic_test fusion_synthetic_test_san fusion_synthetic_test_gprof fusion_synthetic_trace imu_quality_synthetic_test imu_quality_synthetic_test_san gmon.out
@@ -0,0 +1,418 @@
+#include <math.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <string.h>
+
+#include "fusion_engine.h"
+#include "imu_sensor.h"
+
+#define QUALITY_PI 3.14159265358979323846264338327950288
+#define QUALITY_TIMEBASE_HZ 1000000u
+#define QUALITY_IMU_HZ 200u
+#define QUALITY_DURATION_S 30u
+
+typedef struct {
+	uint32_t state;
+} quality_rng_t;
+
+typedef struct {
+	double position_m[3];
+	double velocity_mps[3];
+	double yaw_rad;
+} quality_truth_t;
+
+typedef struct {
+	const char* name;
+	double horizontal_rmse_m;
+	double final_horizontal_error_m;
+	double final_speed_mps;
+	double outage_peak_error_m;
+	double first_accel_detection_s;
+	double rotation_detection_s;
+	double final_stationary_reentry_s;
+	uint64_t zero_updates;
+	uint64_t accepted_gps;
+	unsigned int dynamic_zero_update_count;
+	unsigned int false_moving_before_motion;
+	bool state_finite;
+} quality_metrics_t;
+
+static int failures;
+
+#define CHECK(condition, message) do { \
+	if(!(condition)) { \
+		fprintf(stderr, "FAIL: %s\n", message); \
+		failures++; \
+	} \
+} while(0)
+
+static double square(double value)
+{
+	return value * value;
+}
+
+static double norm2(double first, double second)
+{
+	return sqrt(first * first + second * second);
+}
+
+static uint32_t rng_next(quality_rng_t* rng)
+{
+	uint32_t value = rng->state;
+	value ^= value << 13;
+	value ^= value >> 17;
+	value ^= value << 5;
+	rng->state = value;
+	return value;
+}
+
+static double rng_uniform(quality_rng_t* rng)
+{
+	return ((double)rng_next(rng) + 0.5) / 4294967296.0;
+}
+
+static double rng_gaussian(quality_rng_t* rng)
+{
+	double first = fmax(rng_uniform(rng), 1.0e-12);
+	double second = rng_uniform(rng);
+	return sqrt(-2.0 * log(first)) * cos(2.0 * QUALITY_PI * second);
+}
+
+static void trajectory_input(double time_s,
+	                         double acceleration_nav_mps2[3],
+	                         double* yaw_rate_rps)
+{
+	memset(acceleration_nav_mps2, 0, 3u * sizeof(double));
+	*yaw_rate_rps = 0.0;
+	if((time_s >= 3.0) && (time_s < 4.0))
+	{
+		acceleration_nav_mps2[0] = 2.0;
+	}
+	else if((time_s >= 6.0) && (time_s < 7.0))
+	{
+		acceleration_nav_mps2[0] = -2.0;
+	}
+	else if((time_s >= 12.0) && (time_s < 13.0))
+	{
+		*yaw_rate_rps = 0.5 * QUALITY_PI;
+	}
+	else if((time_s >= 17.0) && (time_s < 17.5))
+	{
+		acceleration_nav_mps2[1] = -3.0;
+	}
+	else if((time_s >= 19.0) && (time_s < 19.5))
+	{
+		acceleration_nav_mps2[1] = 3.0;
+	}
+}
+
+static bool truth_is_dynamic(const quality_truth_t* truth,
+	                         const double acceleration_nav_mps2[3],
+	                         double yaw_rate_rps)
+{
+	return (fabs(acceleration_nav_mps2[0]) > 0.4) ||
+	       (fabs(acceleration_nav_mps2[1]) > 0.4) ||
+	       (fabs(yaw_rate_rps) > 0.02) ||
+	       (norm2(truth->velocity_mps[0], truth->velocity_mps[1]) > 0.20);
+}
+
+static void truth_step(quality_truth_t* truth, double dt_s,
+	                   const double acceleration_nav_mps2[3],
+	                   double yaw_rate_rps)
+{
+	for(unsigned int axis = 0u ; axis < 3u ; axis++)
+	{
+		truth->position_m[axis] += truth->velocity_mps[axis] * dt_s +
+		                            0.5 * acceleration_nav_mps2[axis] * dt_s * dt_s;
+		truth->velocity_mps[axis] += acceleration_nav_mps2[axis] * dt_s;
+	}
+	truth->yaw_rad += yaw_rate_rps * dt_s;
+}
+
+static void publish_position(fusion_engine_t* engine, uint64_t tick,
+	                         const quality_truth_t* truth,
+	                         quality_rng_t* gps_rng)
+{
+	fusion_position_measurement_t gps;
+	memset(&gps, 0, sizeof(gps));
+	gps.timestamp_ticks = tick;
+	gps.position_enu_m[0] = truth->position_m[0] + 1.5 * rng_gaussian(gps_rng);
+	gps.position_enu_m[1] = truth->position_m[1] + 1.5 * rng_gaussian(gps_rng);
+	gps.position_enu_m[2] = truth->position_m[2] + 2.5 * rng_gaussian(gps_rng);
+	gps.covariance_enu_m2[0] = square(1.5);
+	gps.covariance_enu_m2[4] = square(1.5);
+	gps.covariance_enu_m2[8] = square(2.5);
+	gps.source_quality = 1.0;
+	gps.source_valid = true;
+	eskf_update_result_t result = ESKF_UPDATE_INVALID;
+	CHECK(fusion_engine_push_position(engine, &gps, &result, NULL),
+	      "quality-profile GPS is processed");
+	CHECK((result == ESKF_UPDATE_ACCEPTED) ||
+	      (result == ESKF_UPDATE_REJECTED_NIS),
+	      "quality-profile GPS returns a defined update result");
+}
+
+static quality_metrics_t run_profile(imu_sensor_backend_t backend,
+	                                 uint32_t seed)
+{
+	static fusion_engine_t engine;
+	fusion_imu_quality_profile_t profile;
+	CHECK(imu_sensor_quality_profile_for_backend(backend, &profile),
+	      "sensor fusion quality profile exists");
+	fusion_engine_config_t config;
+	fusion_engine_default_config(&config, QUALITY_TIMEBASE_HZ);
+	CHECK(fusion_engine_apply_imu_quality(&config, &profile),
+	      "sensor quality profile applies to ESKF config");
+	config.calibration_duration_s = 2.0;
+	config.calibration_min_samples = QUALITY_IMU_HZ;
+	CHECK(fusion_engine_init(&engine, &config),
+	      "quality-profile fusion engine initializes");
+
+	fusion_position_measurement_t origin;
+	memset(&origin, 0, sizeof(origin));
+	origin.covariance_enu_m2[0] = square(1.5);
+	origin.covariance_enu_m2[4] = square(1.5);
+	origin.covariance_enu_m2[8] = square(2.5);
+	origin.source_quality = 1.0;
+	origin.source_valid = true;
+	CHECK(fusion_engine_push_position(&engine, &origin, NULL, NULL),
+	      "quality-profile initial GPS is retained");
+
+	quality_rng_t imu_rng = {0x1735a9cdu ^ seed};
+	quality_rng_t gps_rng = {0x9b6d42e1u ^ seed};
+	quality_truth_t truth = {{0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}, 0.0};
+	double accel_bias[3] = {0.030, -0.020, 0.050};
+	double gyro_bias[3] = {0.0008, -0.0005, 0.0007};
+	double quality_factor = (backend == IMU_SENSOR_ST_LSM6DSOX) ? 1.35 : 1.0;
+	for(unsigned int axis = 0u ; axis < 3u ; axis++)
+	{
+		accel_bias[axis] *= quality_factor;
+		gyro_bias[axis] *= quality_factor;
+	}
+
+	const uint64_t imu_period_ticks = QUALITY_TIMEBASE_HZ / QUALITY_IMU_HZ;
+	const double dt_s = 1.0 / (double)QUALITY_IMU_HZ;
+	const double bandwidth_sqrt_hz = sqrt(0.5 * (double)QUALITY_IMU_HZ);
+	quality_metrics_t metrics;
+	memset(&metrics, 0, sizeof(metrics));
+	metrics.name = profile.name;
+	metrics.first_accel_detection_s = NAN;
+	metrics.rotation_detection_s = NAN;
+	metrics.final_stationary_reentry_s = NAN;
+	double squared_horizontal_error_sum = 0.0;
+	uint64_t error_samples = 0u;
+	uint64_t next_gps_tick = 3000000u;
+	uint64_t previous_zero_updates = 0u;
+
+	for(uint64_t tick = 0u ;
+	    tick <= (uint64_t)QUALITY_DURATION_S * QUALITY_TIMEBASE_HZ ;
+	    tick += imu_period_ticks)
+	{
+		double acceleration_nav[3] = {0.0, 0.0, 0.0};
+		double yaw_rate = 0.0;
+		double time_s = (double)tick / (double)QUALITY_TIMEBASE_HZ;
+		if(tick != 0u)
+		{
+			double midpoint_s = time_s - 0.5 * dt_s;
+			trajectory_input(midpoint_s, acceleration_nav, &yaw_rate);
+			truth_step(&truth, dt_s, acceleration_nav, yaw_rate);
+		}
+
+		/* Inject the profile's bias random walk into the synthetic sensor. */
+		for(unsigned int axis = 0u ; axis < 3u ; axis++)
+		{
+			accel_bias[axis] += profile.accel_bias_random_walk_mps2_sqrt_s[axis] *
+			                    sqrt(dt_s) * rng_gaussian(&imu_rng);
+			gyro_bias[axis] += profile.gyro_bias_random_walk_rps_sqrt_s[axis] *
+			                   sqrt(dt_s) * rng_gaussian(&imu_rng);
+		}
+
+		double midpoint_yaw = truth.yaw_rad - 0.5 * yaw_rate * dt_s;
+		double cosine = cos(midpoint_yaw);
+		double sine = sin(midpoint_yaw);
+		fusion_imu_measurement_t imu;
+		memset(&imu, 0, sizeof(imu));
+		imu.timestamp_ticks = tick;
+		imu.accel_mps2[0] = cosine * acceleration_nav[0] +
+		                    sine * acceleration_nav[1];
+		imu.accel_mps2[1] = -sine * acceleration_nav[0] +
+		                     cosine * acceleration_nav[1];
+		imu.accel_mps2[2] = config.eskf.gravity_mps2;
+		imu.gyro_rps[2] = yaw_rate;
+		for(unsigned int axis = 0u ; axis < 3u ; axis++)
+		{
+			double accel_sigma =
+				3.0 * profile.nominal_accel_noise_density_mps2_sqrt_hz[axis] *
+				bandwidth_sqrt_hz;
+			double gyro_sigma =
+				3.0 * profile.nominal_gyro_noise_density_rps_sqrt_hz[axis] *
+				bandwidth_sqrt_hz;
+			imu.accel_mps2[axis] += accel_bias[axis] +
+			                         accel_sigma * rng_gaussian(&imu_rng);
+			imu.gyro_rps[axis] += gyro_bias[axis] +
+			                      gyro_sigma * rng_gaussian(&imu_rng);
+		}
+		imu.temperature_c = 25.0 + 0.05 * time_s;
+		imu.sequence = (uint32_t)(tick / imu_period_ticks);
+		CHECK(fusion_engine_push_imu(&engine, &imu),
+		      "quality-profile IMU is accepted");
+
+		if((tick >= next_gps_tick) &&
+		   !((time_s >= 16.0) && (time_s < 25.0)))
+		{
+			publish_position(&engine, tick, &truth, &gps_rng);
+			next_gps_tick += QUALITY_TIMEBASE_HZ;
+		}
+		else if((tick >= next_gps_tick) && (time_s >= 25.0))
+		{
+			/* Skip the 16--25 s events, then resume at the current epoch. */
+			publish_position(&engine, tick, &truth, &gps_rng);
+			next_gps_tick = tick + QUALITY_TIMEBASE_HZ;
+		}
+
+		if(engine.state != FUSION_ENGINE_RUNNING)
+		{
+			continue;
+		}
+		bool dynamic = truth_is_dynamic(&truth,
+		                                acceleration_nav, yaw_rate);
+		uint64_t zero_updates = engine.diagnostics.zero_velocity_update_count;
+		if(dynamic && (zero_updates > previous_zero_updates))
+		{
+			metrics.dynamic_zero_update_count +=
+				(unsigned int)(zero_updates - previous_zero_updates);
+		}
+		previous_zero_updates = zero_updates;
+		if((time_s > 2.2) && (time_s < 3.0) &&
+		   (engine.current.motion.state != FUSION_MOTION_STATIONARY))
+		{
+			metrics.false_moving_before_motion++;
+		}
+		if(isnan(metrics.first_accel_detection_s) && (time_s >= 3.0) &&
+		   (engine.current.motion.state == FUSION_MOTION_MOVING))
+		{
+			metrics.first_accel_detection_s = time_s;
+		}
+		if(isnan(metrics.rotation_detection_s) && (time_s >= 12.0) &&
+		   (engine.current.motion.state == FUSION_MOTION_MOVING))
+		{
+			metrics.rotation_detection_s = time_s;
+		}
+		if(isnan(metrics.final_stationary_reentry_s) && (time_s >= 19.5) &&
+		   (engine.current.motion.state == FUSION_MOTION_STATIONARY))
+		{
+			metrics.final_stationary_reentry_s = time_s;
+		}
+
+		double horizontal_error = norm2(
+			engine.current.filter.position_m[0] - truth.position_m[0],
+			engine.current.filter.position_m[1] - truth.position_m[1]);
+		if(time_s >= 3.0)
+		{
+			squared_horizontal_error_sum += square(horizontal_error);
+			error_samples++;
+		}
+		if((time_s >= 16.0) && (time_s < 25.0))
+		{
+			metrics.outage_peak_error_m =
+				fmax(metrics.outage_peak_error_m, horizontal_error);
+		}
+	}
+
+	metrics.horizontal_rmse_m =
+		(error_samples != 0u)
+		? sqrt(squared_horizontal_error_sum / (double)error_samples)
+		: NAN;
+	metrics.final_horizontal_error_m = norm2(
+		engine.current.filter.position_m[0] - truth.position_m[0],
+		engine.current.filter.position_m[1] - truth.position_m[1]);
+	metrics.final_speed_mps = eskf_speed_mps(&engine.current.filter);
+	metrics.zero_updates = engine.diagnostics.zero_velocity_update_count;
+	metrics.accepted_gps = engine.diagnostics.position_accepted;
+	metrics.state_finite = eskf_state_is_finite(&engine.current.filter);
+	return metrics;
+}
+
+static void check_metrics(const quality_metrics_t* metrics,
+	                      bool noisier_st_profile)
+{
+	CHECK(metrics->state_finite, "quality-profile ESKF state remains finite");
+	CHECK(metrics->false_moving_before_motion == 0u,
+	      "stationary sensor noise does not create false motion");
+	CHECK(metrics->dynamic_zero_update_count == 0u,
+	      "ZUPT and zero-gyro are disabled throughout true motion");
+	CHECK(isfinite(metrics->first_accel_detection_s) &&
+	      (metrics->first_accel_detection_s <= 3.010),
+	      "sudden acceleration is detected within two samples");
+	CHECK(isfinite(metrics->rotation_detection_s) &&
+	      (metrics->rotation_detection_s <= 12.010),
+	      "sudden rotation is detected within two samples");
+	CHECK(isfinite(metrics->final_stationary_reentry_s) &&
+	      (metrics->final_stationary_reentry_s >= 20.5) &&
+	      (metrics->final_stationary_reentry_s <=
+	       (noisier_st_profile ? 29.5 : 25.0)),
+	      "stationary re-entry waits for motion hysteresis and then recovers");
+	CHECK(metrics->zero_updates > (noisier_st_profile ? 100u : 150u),
+	      "stationary intervals apply rate-limited zero updates");
+	CHECK(metrics->accepted_gps >= 15u,
+	      "GPS updates remain usable with the sensor profile");
+	CHECK(metrics->horizontal_rmse_m < (noisier_st_profile ? 5.0 : 2.0),
+	      "sensor-profile horizontal RMSE remains bounded");
+	CHECK(metrics->outage_peak_error_m < (noisier_st_profile ? 12.0 : 3.0),
+	      "nine-second GPS outage drift remains bounded");
+	CHECK(metrics->final_horizontal_error_m < 2.0,
+	      "post-outage final horizontal error remains bounded");
+	CHECK(metrics->final_speed_mps < 0.05,
+	      "final stationary speed is near zero");
+}
+
+int main(void)
+{
+	fusion_imu_quality_profile_t tdk_profile;
+	fusion_imu_quality_profile_t st_profile;
+	CHECK(imu_sensor_quality_profile_for_backend(IMU_SENSOR_TDK_ICM42688,
+	                                             &tdk_profile),
+	      "TDK profile is valid");
+	CHECK(imu_sensor_quality_profile_for_backend(IMU_SENSOR_ST_LSM6DSOX,
+	                                             &st_profile),
+	      "ST profile is valid");
+	CHECK(tdk_profile.nominal_gyro_noise_density_rps_sqrt_hz[0] <
+	      st_profile.nominal_gyro_noise_density_rps_sqrt_hz[0],
+	      "TDK profile models lower nominal gyro noise than ST");
+	CHECK(tdk_profile.process_gyro_noise_density_rps_sqrt_hz[0] <
+	      st_profile.process_gyro_noise_density_rps_sqrt_hz[0],
+	      "TDK profile gives the ESKF lower gyro process noise");
+
+	quality_metrics_t tdk = run_profile(IMU_SENSOR_TDK_ICM42688, 0x11111111u);
+	quality_metrics_t st = run_profile(IMU_SENSOR_ST_LSM6DSOX, 0x11111111u);
+	check_metrics(&tdk, false);
+	check_metrics(&st, true);
+	CHECK(tdk.outage_peak_error_m <= st.outage_peak_error_m + 0.25,
+	      "TDK-like profile is no worse than ST-like profile during GPS outage");
+
+	printf("Synthetic IMU quality-profile comparison (same production C ESKF)\n");
+	printf("  %-16s RMSE=%5.3f m outage-peak=%5.3f m final=%5.3f m speed=%7.4f m/s GPS=%llu\n",
+	       tdk.name, tdk.horizontal_rmse_m, tdk.outage_peak_error_m,
+	       tdk.final_horizontal_error_m, tdk.final_speed_mps,
+	       (unsigned long long)tdk.accepted_gps);
+	printf("  %-16s RMSE=%5.3f m outage-peak=%5.3f m final=%5.3f m speed=%7.4f m/s GPS=%llu\n",
+	       st.name, st.horizontal_rmse_m, st.outage_peak_error_m,
+	       st.final_horizontal_error_m, st.final_speed_mps,
+	       (unsigned long long)st.accepted_gps);
+	printf("  detection TDK accel=%.3f s turn=%.3f s stop=%.3f s; ST accel=%.3f s turn=%.3f s stop=%.3f s\n",
+	       tdk.first_accel_detection_s, tdk.rotation_detection_s,
+	       tdk.final_stationary_reentry_s, st.first_accel_detection_s,
+	       st.rotation_detection_s, st.final_stationary_reentry_s);
+	printf("  dynamic ZUPT violations TDK=%u ST=%u; zero-updates TDK=%llu ST=%llu\n",
+	       tdk.dynamic_zero_update_count, st.dynamic_zero_update_count,
+	       (unsigned long long)tdk.zero_updates,
+	       (unsigned long long)st.zero_updates);
+	if(failures != 0)
+	{
+		fprintf(stderr, "%d IMU quality-profile check(s) failed\n", failures);
+		return 1;
+	}
+	printf("PASS: both TDK-like and noisier ST-like streams remain bounded.\n");
+	return 0;
+}
@@ -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

@@ -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:
@@ -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)
@@ -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);

@@ -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
@@ -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
@@ -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
@@ -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
@@ -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) &&
@@ -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;
 	}

@@ -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;
+}
@@ -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";
+	}
+}
@@ -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);
+}
@@ -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++;
@@ -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;
+	}
+}
@@ -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)
@@ -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;
+}
@@ -0,0 +1,14 @@
+TARGET := pmod_irq_test
+
+APP_SRCS := main.c
+BSP_SRCS := init.c uart.c plic.c irq.c pmod_spi.c
+
+# Genesys2 Rocket fabric/UART and CLINT clocks.
+BSP_UART_CLOCK_HZ ?= 50000000
+BSP_CLINT_TIMEBASE_HZ ?= 500000
+
+include ../common/app.mk
+
+ifeq ($(PMOD_IRQ_TEST_QUIET),1)
+CFLAGS += -DPMOD_IRQ_TEST_QUIET=1
+endif
@@ -0,0 +1,30 @@
+# Genesys2 JB1 direct PLIC test
+
+This standalone image tests only the asynchronous PMOD interrupt path:
+
+`JB1 (V29) -> wrapper 3FF synchronizer -> Rocket interrupt bus -> PLIC 11`
+
+The input is LVCMOS33 with a weak FPGA pull-down. The Rocket PLIC gateway is
+level-sensitive, so the ISR masks source 11 on its first assertion before it
+completes the claim. This makes a hand-driven HIGH test one-shot and prevents
+an interrupt storm.
+
+Build:
+
+```sh
+make -C soc/software/pmod_irq_test clean all
+```
+
+After loading `build/pmod_irq_test.elf`, wait for `PMOD_IRQ_READY`. Start with
+JB1 disconnected, then connect JB1 to a Genesys2 PMOD 3.3 V pin once. A working
+path prints `PMOD_IRQ_PASS`. Remove the 3.3 V connection before resetting or
+reloading the image.
+
+Never apply 5 V to JB1 and never short a PMOD supply pin to ground.
+
+For a voltage-source-independent loopback test, connect the FPGA-driven JC4
+SPI clock output directly to the JB1 interrupt input. After
+`LOOPBACK_READY`, send `t` over UART. The firmware emits 16 SPI transfers;
+the first SCK HIGH must produce `PMOD_IRQ_PASS`. This loopback is confined to
+the standalone diagnostic and does not add GPIO or alter the production PLIC
+path.
@@ -0,0 +1,136 @@
+#include <stdint.h>
+#include <stdio.h>
+
+#include "bsp/irq.h"
+#include "bsp/mmio.h"
+#include "bsp/plic.h"
+#include "bsp/pmod_spi.h"
+#include "bsp/soc.h"
+#include "bsp/uart.h"
+
+#ifdef PMOD_IRQ_TEST_QUIET
+#define TEST_PRINTF(...) ((void)0)
+#define TEST_UART_FLUSH() ((void)0)
+#else
+#define TEST_PRINTF(...) printf(__VA_ARGS__)
+#define TEST_UART_FLUSH() bsp_uart_wait_tx_idle()
+#endif
+
+static volatile uint64_t irq_count;
+static volatile uint64_t first_irq_timestamp;
+static volatile uint64_t unexpected_mcause;
+static volatile uint32_t unexpected_irq_id;
+
+void pmod_irq_handler(void)
+	__attribute__((interrupt("machine"), aligned(4)));
+
+void pmod_irq_handler(void)
+{
+	asm volatile(
+		".option push\n"
+		".option norelax\n"
+		"la gp, __global_pointer$\n"
+		".option pop\n"
+		:
+		:
+		: "memory");
+
+	const uint64_t timestamp = bsp_read64(BSP_CLINT_MTIME);
+	const uint64_t mcause = bsp_read_mcause();
+	if(((mcause >> 63) == 0u) ||
+	   ((mcause & 0xfffu) != BSP_MCAUSE_MACHINE_EXTERNAL))
+	{
+		unexpected_mcause = mcause;
+		return;
+	}
+
+	const uint32_t irq_id = bsp_plic_claim();
+	if(irq_id == BSP_PMOD_IRQ)
+	{
+		/* PLIC gateway is level-sensitive. Mask on the first assertion so a
+		 * hand-held HIGH wire cannot create an interrupt storm. */
+		bsp_plic_disable(BSP_PMOD_IRQ);
+		if(irq_count == 0u)
+		{
+			first_irq_timestamp = timestamp;
+		}
+		irq_count++;
+	}
+	else if(irq_id != 0u)
+	{
+		unexpected_irq_id = irq_id;
+	}
+
+	if(irq_id != 0u)
+	{
+		bsp_plic_complete(irq_id);
+	}
+}
+
+int main(void)
+{
+	TEST_PRINTF("\nGenesys2 JB1 direct PLIC interrupt test\n");
+	TEST_PRINTF("PATH,JB1/V29,Rocket_interrupt_bus,PLIC%lu\n",
+	       (unsigned long)BSP_PMOD_IRQ);
+	TEST_PRINTF("EXPECTED_RTL,wrapper_3FF,internal_pulldown; verify after synthesis\n");
+	TEST_PRINTF("INPUT,LVCMOS33,active_high,level_sensitive\n");
+
+	irq_count = 0u;
+	first_irq_timestamp = 0u;
+	unexpected_mcause = 0u;
+	unexpected_irq_id = 0u;
+	bsp_plic_disable_all();
+	bsp_plic_set_threshold(0u);
+	bsp_plic_set_priority(BSP_PMOD_IRQ, 1u);
+	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_set_mtvec(pmod_irq_handler);
+	bsp_plic_enable(BSP_PMOD_IRQ);
+	bsp_enable_machine_external_interrupts();
+
+	TEST_PRINTF("PMOD_IRQ_READY,disconnect_JB1_then_touch_JB1_to_3V3_once\n");
+	TEST_PRINTF("LOOPBACK_READY,wire_JC4_SCK_to_JB1_then_send_t\n");
+	TEST_UART_FLUSH();
+
+	while((irq_count == 0u) && (unexpected_mcause == 0u) &&
+	      (unexpected_irq_id == 0u))
+	{
+		const int command = bsp_uart_getc_nonblocking();
+		if((command == 't') || (command == 'T'))
+		{
+			TEST_PRINTF("LOOPBACK_TRIGGER,source=JC4,destination=JB1\n");
+			TEST_UART_FLUSH();
+			for(unsigned int transfer = 0u; transfer < 16u; transfer++)
+			{
+				(void)bsp_pmod_spi_xfer8((uint8_t)(0xa5u ^ transfer));
+			}
+			if(irq_count == 0u)
+			{
+				TEST_PRINTF("LOOPBACK_NO_IRQ,check_JC4_to_JB1_jumper\n");
+				TEST_UART_FLUSH();
+			}
+		}
+		asm volatile("nop");
+	}
+
+	if(irq_count != 0u)
+	{
+		TEST_PRINTF("PMOD_IRQ_PASS,id=%lu,count=%lu,tick=%lu,tick_hz=%lu\n",
+		       (unsigned long)BSP_PMOD_IRQ,
+		       (unsigned long)irq_count,
+		       (unsigned long)first_irq_timestamp,
+		       (unsigned long)BSP_CLINT_TIMEBASE_HZ);
+	}
+	else
+	{
+		TEST_PRINTF("PMOD_IRQ_FAIL,mcause=%lu,unexpected_id=%lu\n",
+		       (unsigned long)unexpected_mcause,
+		       (unsigned long)unexpected_irq_id);
+	}
+	TEST_PRINTF("PMOD_IRQ_MASKED,remove_3V3_before_reset_or_reload\n");
+	TEST_UART_FLUSH();
+	/* The ASIC testbench observes this through mscratch. Returning success is
+	 * therefore reserved for the expected PLIC device-11 ISR path. */
+	return (irq_count != 0u) ? 0 : 1;
+}
@@ -4,7 +4,7 @@ APP_SRCS := \
 	main.c \
 	src/lsm6dsox.c

-BSP_SRCS := init.c uart.c pmod_spi.c
+BSP_SRCS := init.c uart.c plic.c irq.c pmod_spi.c

 # Genesys2 fabric/UART and CLINT defaults. Top-level board builds may override.
 BSP_UART_CLOCK_HZ ?= 50000000
@@ -19,7 +19,7 @@ verified.
 | VDDIO | 3.3 V | digital I/O supply |
 | GND | GND | common ground |

-`CS` is mandatory for four-wire SPI. Never feed 5 V to VDDIO. JC9 is the
+`CS` is mandatory for four-wire SPI. Never feed 5 V to VDDIO. JB1 is the
 optional sensor interrupt pin but is deliberately unused here.

 ## Hardware bring-up results (2026-07-13)
@@ -30,6 +30,15 @@ uint8_t lsm6dsox_read_register(uint8_t reg);
 /* Return 1 for a new coherent gyro+accelerometer sample, 0 if not ready. */
 int lsm6dsox_poll_sample(lsm6dsox_sample_t *sample);

+/* Route accel data-ready to sensor INT1 in latched, active-high mode. The ISR
+ * clears that source first, then waits briefly for the same-ODR gyro block. */
+int lsm6dsox_enable_data_ready_interrupt(void);
+void lsm6dsox_disable_data_ready_interrupt(void);
+
+/* Read one coherent sample for a timestamp captured at ISR entry. */
+int lsm6dsox_read_interrupt_sample(lsm6dsox_sample_t *sample,
+                                   uint64_t interrupt_timestamp_ticks);
+
 void lsm6dsox_power_down(void);
 const lsm6dsox_scale_t *lsm6dsox_scale(void);

@@ -1,7 +1,9 @@
 #include <stdint.h>
 #include <stdio.h>

+#include "bsp/irq.h"
 #include "bsp/mmio.h"
+#include "bsp/plic.h"
 #include "bsp/soc.h"
 #include "bsp/uart.h"
 #include "lsm6dsox.h"
@@ -14,11 +16,106 @@
 #define CAPTURE_DURATION_S 60u
 #endif

+#ifndef LSM6DSOX_IRQ_QUEUE_CAPACITY
+#define LSM6DSOX_IRQ_QUEUE_CAPACITY 256u
+#endif
+
+#if (LSM6DSOX_IRQ_QUEUE_CAPACITY < 2u)
+#error "LSM6DSOX_IRQ_QUEUE_CAPACITY must be at least 2"
+#endif
+
+typedef struct {
+  lsm6dsox_sample_t sample;
+  uint32_t sequence;
+} queued_sample_t;
+
+static queued_sample_t sample_queue[LSM6DSOX_IRQ_QUEUE_CAPACITY];
+static volatile unsigned sample_head;
+static volatile unsigned sample_tail;
+static volatile uint64_t interrupt_count;
+static volatile uint64_t interrupt_sample_count;
+static volatile uint64_t interrupt_read_fail_count;
+static volatile uint64_t queue_overflow_count;
+static volatile uint64_t unexpected_irq_count;
+static volatile uint64_t unexpected_trap_cause;
+
+void sensor_irq_handler(void)
+  __attribute__((interrupt("machine"), aligned(4)));
+
 static uint64_t time_now(void)
 {
  return bsp_read64(BSP_CLINT_MTIME);
 }

+static unsigned next_queue_index(unsigned index)
+{
+  index++;
+  return (index == LSM6DSOX_IRQ_QUEUE_CAPACITY) ? 0u : index;
+}
+
+static void queue_push_isr(const lsm6dsox_sample_t *sample,
+                           uint32_t sequence)
+{
+  unsigned head = __atomic_load_n(&sample_head, __ATOMIC_RELAXED);
+  const unsigned next = next_queue_index(head);
+  if (next == __atomic_load_n(&sample_tail, __ATOMIC_ACQUIRE)) {
+    __atomic_fetch_add(&queue_overflow_count, 1u, __ATOMIC_RELAXED);
+    return;
+  }
+  sample_queue[head].sample = *sample;
+  sample_queue[head].sequence = sequence;
+  __atomic_store_n(&sample_head, next, __ATOMIC_RELEASE);
+}
+
+static int queue_pop(queued_sample_t *queued)
+{
+  unsigned tail = __atomic_load_n(&sample_tail, __ATOMIC_RELAXED);
+  if (tail == __atomic_load_n(&sample_head, __ATOMIC_ACQUIRE)) {
+    return 0;
+  }
+  *queued = sample_queue[tail];
+  __atomic_store_n(&sample_tail, next_queue_index(tail), __ATOMIC_RELEASE);
+  return 1;
+}
+
+void sensor_irq_handler(void)
+{
+  asm volatile(
+    ".option push\n"
+    ".option norelax\n"
+    "la gp, __global_pointer$\n"
+    ".option pop\n"
+    : : : "memory");
+
+  const uint64_t interrupt_timestamp = time_now();
+  const uint64_t mcause = bsp_read_mcause();
+  if (((mcause >> 63) == 0u) ||
+      ((mcause & 0xfffu) != BSP_MCAUSE_MACHINE_EXTERNAL)) {
+    unexpected_trap_cause = mcause;
+    return;
+  }
+
+  const uint32_t irq_id = bsp_plic_claim();
+  if (irq_id == 0u) {
+    __atomic_fetch_add(&unexpected_irq_count, 1u, __ATOMIC_RELAXED);
+    return;
+  }
+  if (irq_id == BSP_PMOD_IRQ) {
+    __atomic_fetch_add(&interrupt_count, 1u, __ATOMIC_RELAXED);
+    lsm6dsox_sample_t sample;
+    if (lsm6dsox_read_interrupt_sample(&sample, interrupt_timestamp)) {
+      const uint64_t sample_number =
+          __atomic_fetch_add(&interrupt_sample_count, 1u, __ATOMIC_RELAXED);
+      queue_push_isr(&sample, (uint32_t)sample_number);
+    } else {
+      __atomic_fetch_add(&interrupt_read_fail_count, 1u, __ATOMIC_RELAXED);
+    }
+  } else {
+    __atomic_fetch_add(&unexpected_irq_count, 1u, __ATOMIC_RELAXED);
+  }
+  bsp_plic_complete(irq_id);
+}
+
 static void print_identity_probe(void)
 {
  printf("WHO_AM_I probes:");
@@ -30,9 +127,10 @@ static void print_identity_probe(void)

 int main(void)
 {
-  printf("\nSTEVAL-MKI217V1 / LSM6DSOX SPI polling test\n");
+  printf("\nSTEVAL-MKI217V1 / LSM6DSOX INT1 data-ready test\n");
  printf("Genesys2 JC: CS=JC1 SDA/MOSI=JC2 SDO/MISO=JC3 SCL/SCK=JC4\n");
-  printf("Power: VDD=3.3V VDDIO=3.3V common GND; INT is not used\n");
+  printf("Power: VDD=3.3V VDDIO=3.3V common GND; sensor INT1=JB1/PLIC%lu\n",
+         (unsigned long)BSP_PMOD_IRQ);
  printf("SPI MMIO=0x%08lx CLINT=%lu Hz UART=%lu baud\n",
         (unsigned long)BSP_PMOD_SPI_BASE,
         (unsigned long)BSP_CLINT_TIMEBASE_HZ,
@@ -53,6 +151,42 @@ int main(void)
         lsm6dsox_read_register(0x12u),
         lsm6dsox_read_register(0x1eu));

+  sample_head = 0u;
+  sample_tail = 0u;
+  interrupt_count = 0u;
+  interrupt_sample_count = 0u;
+  interrupt_read_fail_count = 0u;
+  queue_overflow_count = 0u;
+  unexpected_irq_count = 0u;
+  unexpected_trap_cause = 0u;
+  bsp_plic_disable_all();
+  bsp_plic_set_threshold(0u);
+  bsp_plic_set_priority(BSP_PMOD_IRQ, 1u);
+  /* Recover the PMOD LevelGateway if a previous GDB-loaded image stopped
+   * after claim but before complete. The sensor route is still disabled. */
+  bsp_plic_complete(BSP_PMOD_IRQ);
+  bsp_set_mtvec(sensor_irq_handler);
+  bsp_plic_enable(BSP_PMOD_IRQ);
+  bsp_enable_machine_external_interrupts();
+  if (!lsm6dsox_enable_data_ready_interrupt()) {
+    printf("INT INIT FAIL: INT1_CTRL=%02x INT2_CTRL=%02x TAP_CFG2=%02x "
+           "COUNTER_BDR_REG1=%02x\n",
+           lsm6dsox_read_register(0x0du),
+           lsm6dsox_read_register(0x0eu),
+           lsm6dsox_read_register(0x58u),
+           lsm6dsox_read_register(0x0bu));
+    bsp_plic_disable(BSP_PMOD_IRQ);
+    lsm6dsox_power_down();
+    bsp_uart_wait_tx_idle();
+    return 2;
+  }
+  printf("INT_CONFIG,pin=INT1,INT1_CTRL=%02x,INT2_CTRL=%02x,"
+         "TAP_CFG2=%02x,COUNTER_BDR_REG1=%02x,mode=latched\n",
+         lsm6dsox_read_register(0x0du),
+         lsm6dsox_read_register(0x0eu),
+         lsm6dsox_read_register(0x58u),
+         lsm6dsox_read_register(0x0bu));
+
  const lsm6dsox_scale_t *scale = lsm6dsox_scale();
  printf("IMU_META,LSM6DSOX,tick_hz=%lu,odr_hz=%lu,"
         "accel_ug_lsb=%lu,gyro_udps_lsb=%lu,temp_lsb_c=%lu,"
@@ -70,37 +204,57 @@ int main(void)
  const uint64_t duration_ticks =
      (uint64_t)BSP_CLINT_TIMEBASE_HZ * CAPTURE_DURATION_S;
  uint32_t samples = 0u;
+  uint32_t sequence_discontinuities = 0u;
+  uint32_t expected_sequence = 0u;
  uint64_t previous_tick = 0u;
  uint64_t maximum_gap = 0u;

  while ((time_now() - start) < duration_ticks) {
-    lsm6dsox_sample_t sample;
-    if (!lsm6dsox_poll_sample(&sample)) {
-      continue;
-    }
-
-    if (previous_tick != 0u) {
-      const uint64_t gap = sample.timestamp_ticks - previous_tick;
-      if (gap > maximum_gap) {
-        maximum_gap = gap;
+    /* This diagnostic must still time out and report zero IRQs when INT1 is
+     * disconnected. The production application can sleep on its other IRQs. */
+    asm volatile("nop");
+    queued_sample_t queued;
+    while (queue_pop(&queued)) {
+      const lsm6dsox_sample_t *sample = &queued.sample;
+      if (queued.sequence != expected_sequence) {
+        sequence_discontinuities++;
      }
-    }
-    previous_tick = sample.timestamp_ticks;
+      expected_sequence = queued.sequence + 1u;

-    printf("IMU_RAW,LSM6DSOX,%lu,%lu,%d,%d,%d,%d,%d,%d,%d\n",
-           (unsigned long)samples,
-           (unsigned long)sample.timestamp_ticks,
-           (int)sample.accel[0], (int)sample.accel[1],
-           (int)sample.accel[2], (int)sample.gyro[0],
-           (int)sample.gyro[1], (int)sample.gyro[2],
-           (int)sample.temperature);
-    samples++;
+      if (previous_tick != 0u) {
+        const uint64_t gap = sample->timestamp_ticks - previous_tick;
+        if (gap > maximum_gap) {
+          maximum_gap = gap;
+        }
+      }
+      previous_tick = sample->timestamp_ticks;
+
+      printf("IMU_RAW,LSM6DSOX,%lu,%lu,%d,%d,%d,%d,%d,%d,%d\n",
+             (unsigned long)queued.sequence,
+             (unsigned long)sample->timestamp_ticks,
+             (int)sample->accel[0], (int)sample->accel[1],
+             (int)sample->accel[2], (int)sample->gyro[0],
+             (int)sample->gyro[1], (int)sample->gyro[2],
+             (int)sample->temperature);
+      samples++;
+    }
  }

+  bsp_plic_disable(BSP_PMOD_IRQ);
+  lsm6dsox_disable_data_ready_interrupt();
  lsm6dsox_power_down();
-  printf("IMU_DONE,samples=%lu,duration_s=%u,max_gap_ticks=%lu\n",
+  printf("IMU_DONE,samples=%lu,duration_s=%u,max_gap_ticks=%lu,"
+         "irq=%lu,irq_samples=%lu,read_fail=%lu,queue_overflow=%lu,"
+         "seq_discontinuities=%lu,unexpected_irq=%lu,trap=%lu\n",
         (unsigned long)samples, (unsigned)CAPTURE_DURATION_S,
-         (unsigned long)maximum_gap);
+         (unsigned long)maximum_gap,
+         (unsigned long)interrupt_count,
+         (unsigned long)interrupt_sample_count,
+         (unsigned long)interrupt_read_fail_count,
+         (unsigned long)queue_overflow_count,
+         (unsigned long)sequence_discontinuities,
+         (unsigned long)unexpected_irq_count,
+         (unsigned long)unexpected_trap_cause);
  bsp_uart_wait_tx_idle();
  return 0;
 }
@@ -9,12 +9,17 @@
 #define LSM6DSOX_SPI_READ 0x80u
 #define LSM6DSOX_SPI_DUMMY 0xffu

+#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_WHO_AM_I 0x0fu

 #define LSM6DSOX_CTRL3_SW_RESET 0x01u
@@ -22,6 +27,9 @@
 #define LSM6DSOX_CTRL3_BDU 0x40u
 #define LSM6DSOX_STATUS_XLDA 0x01u
 #define LSM6DSOX_STATUS_GDA 0x02u
+#define LSM6DSOX_INT1_DRDY_XL 0x01u
+#define LSM6DSOX_INTERRUPTS_ENABLE 0x80u
+#define LSM6DSOX_GYRO_READY_TIMEOUT_US 100u

 /* CTRL1_XL/CTRL2_G ODR fields (bits 7:4). */
 #define LSM6DSOX_ODR_52 0x30u
@@ -147,21 +155,9 @@ int lsm6dsox_init(uint32_t odr_hz)
  return 1;
 }

-int lsm6dsox_poll_sample(lsm6dsox_sample_t *sample)
+static void read_output_registers(lsm6dsox_sample_t *sample)
 {
  uint8_t raw[14];
-  if (sample == NULL) {
-    return 0;
-  }
-
-  const uint8_t status = read_reg(LSM6DSOX_STATUS_REG);
-  if ((status & (LSM6DSOX_STATUS_XLDA | LSM6DSOX_STATUS_GDA)) !=
-      (LSM6DSOX_STATUS_XLDA | LSM6DSOX_STATUS_GDA)) {
-    return 0;
-  }
-
-  /* Timestamp the measurement before the foreground SPI burst. */
-  sample->timestamp_ticks = time_now();
  bsp_pmod_spi_read_regs(LSM6DSOX_OUT_TEMP_L | LSM6DSOX_SPI_READ,
                         LSM6DSOX_SPI_DUMMY,
                         raw,
@@ -180,11 +176,111 @@ int lsm6dsox_poll_sample(lsm6dsox_sample_t *sample)
  sample->accel[0] = little_endian_i16(&raw[8]);
  sample->accel[1] = little_endian_i16(&raw[10]);
  sample->accel[2] = little_endian_i16(&raw[12]);
+}
+
+static void read_accel_registers(lsm6dsox_sample_t *sample)
+{
+  uint8_t raw[6];
+  bsp_pmod_spi_read_regs(LSM6DSOX_OUTX_L_A | LSM6DSOX_SPI_READ,
+                         LSM6DSOX_SPI_DUMMY, raw, sizeof(raw));
+  sample->accel[0] = little_endian_i16(&raw[0]);
+  sample->accel[1] = little_endian_i16(&raw[2]);
+  sample->accel[2] = little_endian_i16(&raw[4]);
+}
+
+static void read_gyro_and_temperature_registers(lsm6dsox_sample_t *sample)
+{
+  uint8_t raw[8];
+  bsp_pmod_spi_read_regs(LSM6DSOX_OUT_TEMP_L | LSM6DSOX_SPI_READ,
+                         LSM6DSOX_SPI_DUMMY, raw, 2u);
+  bsp_pmod_spi_read_regs(LSM6DSOX_OUTX_L_G | LSM6DSOX_SPI_READ,
+                         LSM6DSOX_SPI_DUMMY, &raw[2], 6u);
+  sample->temperature = little_endian_i16(&raw[0]);
+  sample->gyro[0] = little_endian_i16(&raw[2]);
+  sample->gyro[1] = little_endian_i16(&raw[4]);
+  sample->gyro[2] = little_endian_i16(&raw[6]);
+}
+
+int lsm6dsox_poll_sample(lsm6dsox_sample_t *sample)
+{
+  if (sample == NULL) {
+    return 0;
+  }
+
+  const uint8_t status = read_reg(LSM6DSOX_STATUS_REG);
+  if ((status & (LSM6DSOX_STATUS_XLDA | LSM6DSOX_STATUS_GDA)) !=
+      (LSM6DSOX_STATUS_XLDA | LSM6DSOX_STATUS_GDA)) {
+    return 0;
+  }
+
+  /* Timestamp the measurement before the foreground SPI burst. */
+  sample->timestamp_ticks = time_now();
+  read_output_registers(sample);
+  return 1;
+}
+
+int lsm6dsox_enable_data_ready_interrupt(void)
+{
+  /* Explicitly select latched data-ready. Reading the associated output
+   * register clears INT1; this matches the level-sensitive Rocket PLIC. */
+  write_reg(LSM6DSOX_INT1_CTRL, 0u);
+  write_reg(LSM6DSOX_INT2_CTRL, 0u);
+  write_reg(LSM6DSOX_TAP_CFG2,
+            read_reg(LSM6DSOX_TAP_CFG2) &
+            (uint8_t)~LSM6DSOX_INTERRUPTS_ENABLE);
+  write_reg(LSM6DSOX_COUNTER_BDR_REG1, 0u);
+  if ((read_reg(LSM6DSOX_STATUS_REG) & LSM6DSOX_STATUS_XLDA) != 0u) {
+    lsm6dsox_sample_t stale;
+    read_output_registers(&stale);
+  }
+  write_reg(LSM6DSOX_INT1_CTRL, LSM6DSOX_INT1_DRDY_XL);
+  write_reg(LSM6DSOX_TAP_CFG2,
+            read_reg(LSM6DSOX_TAP_CFG2) | LSM6DSOX_INTERRUPTS_ENABLE);
+  return (read_reg(LSM6DSOX_INT1_CTRL) == LSM6DSOX_INT1_DRDY_XL) &&
+         (read_reg(LSM6DSOX_INT2_CTRL) == 0u) &&
+         ((read_reg(LSM6DSOX_TAP_CFG2) & LSM6DSOX_INTERRUPTS_ENABLE) != 0u) &&
+         ((read_reg(LSM6DSOX_COUNTER_BDR_REG1) & 0x80u) == 0u);
+}
+
+void lsm6dsox_disable_data_ready_interrupt(void)
+{
+  write_reg(LSM6DSOX_INT1_CTRL, 0u);
+  write_reg(LSM6DSOX_INT2_CTRL, 0u);
+  write_reg(LSM6DSOX_TAP_CFG2,
+            read_reg(LSM6DSOX_TAP_CFG2) &
+            (uint8_t)~LSM6DSOX_INTERRUPTS_ENABLE);
+}
+
+int lsm6dsox_read_interrupt_sample(lsm6dsox_sample_t *sample,
+                                   uint64_t interrupt_timestamp_ticks)
+{
+  if (sample == NULL) {
+    return 0;
+  }
+  const uint8_t status = read_reg(LSM6DSOX_STATUS_REG);
+  sample->timestamp_ticks = interrupt_timestamp_ticks;
+  /* Accel DRDY is the routed source, so clear its entire output block first.
+   * Then wait only long enough to form a coherent same-ODR gyro pair. */
+  read_accel_registers(sample);
+  if ((status & LSM6DSOX_STATUS_XLDA) == 0u) {
+    return 0;
+  }
+  const uint64_t wait_start = time_now();
+  const uint64_t timeout_ticks =
+      ((uint64_t)BSP_CLINT_TIMEBASE_HZ * LSM6DSOX_GYRO_READY_TIMEOUT_US +
+       999999u) / 1000000u;
+  while ((read_reg(LSM6DSOX_STATUS_REG) & LSM6DSOX_STATUS_GDA) == 0u) {
+    if ((time_now() - wait_start) >= timeout_ticks) {
+      return 0;
+    }
+  }
+  read_gyro_and_temperature_registers(sample);
  return 1;
 }

 void lsm6dsox_power_down(void)
 {
+  lsm6dsox_disable_data_ready_interrupt();
  write_reg(LSM6DSOX_CTRL1_XL, 0u);
  write_reg(LSM6DSOX_CTRL2_G, 0u);
 }