Add bare-metal TDK IMU ESKF fusion

soc/software/gps_system/header/eskf.h

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

soc/software/gps_system/header/fusion_bus.h

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

soc/software/gps_system/header/fusion_engine.h

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

soc/software/gps_system/header/fusion_gps_adapter.h

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

soc/software/gps_system/header/fusion_measurement.h

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

soc/software/gps_system/header/fusion_motion.h

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

soc/software/gps_system/header/gps_fusion_publisher.h

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

soc/software/gps_system/header/gps_ip_defines.h

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

soc/software/gps_system/header/gps_macros.h

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

soc/software/gps_system/header/linked_list.h

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

soc/software/gps_system/header/navigation_fusion.h

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

soc/software/gps_system/header/tdk_icm42688.h

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