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,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