Integrate asynchronous GPS IMU fusion

soc/software/gps_system/header/adapters/gps_fusion_publisher.h

@@ -0,0 +1,17 @@
+#ifndef GPS_FUSION_PUBLISHER_H
+#define GPS_FUSION_PUBLISHER_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "gps/NavEquations.h"
+#include "fusion/fusion_measurement.h"
+
+/* Thin boundary adapter used by main.  GPS and the ESKF do not call each
+ * other; main converts the completed WLS output to the shared measurement. */
+bool gps_fusion_build_measurement(
+	const gps_nav_solution_t* solution,
+	uint64_t measurement_timestamp_ticks,
+	fusion_gps_measurement_t* measurement_out);
+
+#endif

soc/software/gps_system/header/adapters/gps_system_uart.h

@@ -0,0 +1,27 @@
+#ifndef GPS_SYSTEM_UART_H
+#define GPS_SYSTEM_UART_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "fusion/navigation_fusion.h"
+#include "gps/NavEquations.h"
+
+void gps_system_uart_publish_location(
+	const char* source,
+	double latitude_deg,
+	double longitude_deg,
+	double height_m,
+	const gps_nav_solution_t* gps);
+
+void gps_system_uart_publish_startup(
+	bool fusion_enabled,
+	bool fusion_initialized,
+	bool imu_initialized,
+	bool imu_running);
+
+void gps_system_uart_publish_periodic_fusion_status(
+	navigation_fusion_output_t* output,
+	uint64_t* last_status_tick);
+
+#endif

soc/software/gps_system/header/adapters/imu_fusion_profile.h

@@ -0,0 +1,14 @@
+#ifndef IMU_FUSION_PROFILE_H
+#define IMU_FUSION_PROFILE_H
+
+#include <stdbool.h>
+
+#include "fusion/fusion_imu_quality.h"
+#include "imu/imu_sensor.h"
+
+/* Explicit adapter from a selected sensor backend to ESKF tuning values. */
+bool imu_fusion_quality_profile_for_backend(
+	imu_sensor_backend_t backend,
+	fusion_imu_quality_profile_t* profile_out);
+
+#endif

soc/software/gps_system/header/drivers/imu/st_lsm6dsox.h

@@ -4,7 +4,7 @@
 #include <stdbool.h>
 #include <stdint.h>
 
-#include "fusion_measurement.h"
+#include "imu/imu_sample.h"
 
 #ifndef ST_IMU_RAW_QUEUE_CAPACITY
 #define ST_IMU_RAW_QUEUE_CAPACITY 256u
@@ -46,7 +46,7 @@ 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);
+	imu_sample_t* measurement_out);
 void st_lsm6dsox_get_diagnostics(
 	st_lsm6dsox_diagnostics_t* diagnostics_out);
 

soc/software/gps_system/header/drivers/imu/tdk_icm42688.h

@@ -4,7 +4,7 @@
 #include <stdbool.h>
 #include <stdint.h>
 
-#include "fusion_measurement.h"
+#include "imu/imu_sample.h"
 
 #ifndef TDK_IMU_RAW_QUEUE_CAPACITY
 #define TDK_IMU_RAW_QUEUE_CAPACITY 256u
@@ -69,7 +69,7 @@ void tdk_icm42688_stop(void);
 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);
+bool tdk_icm42688_take_measurement(imu_sample_t* measurement_out);
 void tdk_icm42688_get_diagnostics(tdk_icm42688_diagnostics_t* diagnostics_out);
 
 #endif

soc/software/gps_system/header/fusion/eskf.h

@@ -65,6 +65,18 @@ bool eskf_propagate(eskf_t* filter,
 	                const double gyro_rps[ESKF_VECTOR_DIM],
 	                double dt_s);
 
+/* Midpoint inertial integration over two timestamped raw samples.  The
+ * constant-input eskf_propagate() entry point remains available for callers
+ * that already provide an interval-integrated/held measurement. */
+bool eskf_propagate_interval(
+	eskf_t* filter,
+	const eskf_config_t* config,
+	const double start_accel_mps2[ESKF_VECTOR_DIM],
+	const double start_gyro_rps[ESKF_VECTOR_DIM],
+	const double end_accel_mps2[ESKF_VECTOR_DIM],
+	const double end_gyro_rps[ESKF_VECTOR_DIM],
+	double dt_s);
+
 eskf_update_result_t eskf_position_update(
 	eskf_t* filter,
 	const eskf_config_t* config,
@@ -72,6 +84,15 @@ eskf_update_result_t eskf_position_update(
 	const double covariance_m2[ESKF_VECTOR_DIM * ESKF_VECTOR_DIM],
 	double* nis_out);
 
+/* Re-anchor position after a higher-level fusion timeout has established that
+ * repeated, valid position observations are mutually coherent.  Velocity,
+ * attitude and IMU biases are preserved; position cross-covariances are
+ * cleared because they are no longer valid after the state discontinuity. */
+bool eskf_reset_position(
+	eskf_t* filter,
+	const double position_m[ESKF_VECTOR_DIM],
+	const double covariance_m2[ESKF_VECTOR_DIM * ESKF_VECTOR_DIM]);
+
 bool eskf_zero_velocity_update(eskf_t* filter, double velocity_std_mps);
 
 bool eskf_zero_angular_rate_update(

soc/software/gps_system/header/fusion/fusion_bus.h

@@ -4,7 +4,7 @@
 #include <stdbool.h>
 #include <stdint.h>
 
-#include "fusion_measurement.h"
+#include "fusion/fusion_measurement.h"
 
 #ifndef FUSION_IMU_QUEUE_CAPACITY
 #define FUSION_IMU_QUEUE_CAPACITY 256u

soc/software/gps_system/header/fusion/fusion_engine.h

@@ -4,10 +4,10 @@
 #include <stdbool.h>
 #include <stdint.h>
 
-#include "eskf.h"
-#include "fusion_imu_quality.h"
-#include "fusion_measurement.h"
-#include "fusion_motion.h"
+#include "fusion/eskf.h"
+#include "fusion/fusion_imu_quality.h"
+#include "fusion/fusion_measurement.h"
+#include "fusion/fusion_motion.h"
 
 #ifndef FUSION_IMU_HISTORY_CAPACITY
 #define FUSION_IMU_HISTORY_CAPACITY 2048u
@@ -38,8 +38,13 @@ typedef struct {
 	double zero_gyro_std_rps[3];
 	double gnss_motion_speed_threshold_mps;
 	double gnss_displacement_nis_gate;
+	double gnss_motion_window_s;
 	unsigned int gnss_moving_confirmations;
 	unsigned int gnss_still_confirmations;
+	double position_recovery_timeout_s;
+	double position_recovery_max_speed_mps;
+	double position_recovery_sigma_multiplier;
+	unsigned int position_recovery_min_samples;
 } fusion_engine_config_t;
 
 typedef struct {
@@ -49,13 +54,17 @@ typedef struct {
 	fusion_gnss_motion_evidence_t gnss_evidence;
 	double last_accepted_position_m[3];
 	double last_accepted_position_covariance_m2[9];
+	double zero_gyro_integral_rad[3];
+	double zero_gyro_duration_s;
 	uint64_t filter_timestamp_ticks;
 	uint64_t last_zero_update_ticks;
 	uint64_t last_accepted_position_ticks;
+	uint64_t last_position_fuse_ticks;
 	unsigned int gnss_moving_count;
 	unsigned int gnss_still_count;
 	bool have_held_imu;
 	bool have_accepted_position;
+	bool vehicle_at_rest;
 } fusion_runtime_t;
 
 typedef struct {
@@ -70,9 +79,11 @@ typedef struct {
 	uint64_t position_stale_rejected;
 	uint64_t position_pending_overflow;
 	uint64_t position_accepted;
+	uint64_t position_recovery_count;
 	uint64_t replay_count;
 	uint64_t replayed_imu_count;
 	uint64_t zero_velocity_update_count;
+	uint64_t zero_gyro_update_count;
 } fusion_engine_diagnostics_t;
 
 typedef struct {
@@ -95,12 +106,16 @@ typedef struct {
 	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_position_measurement_t position_recovery_candidate;
 	fusion_engine_diagnostics_t diagnostics;
+	uint64_t position_recovery_start_ticks;
 	unsigned int imu_history_head;
 	unsigned int imu_history_count;
 	unsigned int pending_position_count;
+	unsigned int position_recovery_consistent_count;
 	bool checkpoint_valid;
 	bool have_initial_position;
+	bool position_recovery_active;
 } fusion_engine_t;
 
 void fusion_engine_default_config(fusion_engine_config_t* config,

soc/software/gps_system/header/fusion/fusion_gps_adapter.h

@@ -3,7 +3,7 @@
 
 #include <stdbool.h>
 
-#include "fusion_measurement.h"
+#include "fusion/fusion_measurement.h"
 
 typedef struct {
 	double minimum_horizontal_std_m;

soc/software/gps_system/header/fusion/fusion_measurement.h

@@ -4,6 +4,8 @@
 #include <stdbool.h>
 #include <stdint.h>
 
+#include "imu/imu_sample.h"
+
 /*
  * Hardware- and solver-neutral messages shared at the GPS/ESKF boundary.
  * timestamp_ticks always belongs to the common local monotonic timebase; GPS
@@ -23,12 +25,15 @@ typedef struct {
 	bool solution_valid;
 } fusion_gps_measurement_t;
 
+/* Timestamped IMU plus the independently supervised platform state consumed
+ * by fusion. Drivers publish imu_sample_t and do not decide `vehicle_at_rest`. */
 typedef struct {
 	uint64_t timestamp_ticks;
 	double accel_mps2[3];
 	double gyro_rps[3];
 	double temperature_c;
 	uint32_t sequence;
+	bool vehicle_at_rest;
 } fusion_imu_measurement_t;
 
 /* Generic local-navigation measurement consumed by the ESKF coordinator. */

soc/software/gps_system/header/fusion/fusion_rest_detector.h

@@ -0,0 +1,50 @@
+#ifndef FUSION_REST_DETECTOR_H
+#define FUSION_REST_DETECTOR_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "imu/imu_sample.h"
+
+/*
+ * PX4-style platform rest supervisor.  The ESKF consumes the resulting
+ * at-rest flag; it does not infer that flag from its own velocity state.
+ * `landed` is an external platform fact (unarmed bench prototype, wheel/leg
+ * contact, land detector, and so on), not a GNSS/ESKF estimate.
+ */
+typedef struct {
+	double vibration_filter_time_constant_s;
+	double accel_vibration_limit_mps2;
+	double gyro_vibration_limit_rps;
+	double rest_dwell_s;
+} fusion_rest_detector_config_t;
+
+typedef struct {
+	fusion_rest_detector_config_t config;
+	double previous_accel_mps2[3];
+	double previous_gyro_rps[3];
+	double accel_vibration_metric_mps2;
+	double gyro_vibration_metric_rps;
+	uint64_t timebase_hz;
+	uint64_t last_sample_ticks;
+	uint64_t last_motion_ticks;
+	bool have_previous_sample;
+	bool landed;
+	bool at_rest;
+} fusion_rest_detector_t;
+
+void fusion_rest_detector_default_config(fusion_rest_detector_config_t* config);
+bool fusion_rest_detector_init(
+	fusion_rest_detector_t* detector,
+	const fusion_rest_detector_config_t* config,
+	uint64_t timebase_hz,
+	bool landed);
+void fusion_rest_detector_set_landed(
+	fusion_rest_detector_t* detector,
+	bool landed,
+	uint64_t timestamp_ticks);
+bool fusion_rest_detector_update(
+	fusion_rest_detector_t* detector,
+	const imu_sample_t* sample);
+
+#endif

soc/software/gps_system/header/fusion/navigation_fusion.h

@@ -0,0 +1,64 @@
+#ifndef NAVIGATION_FUSION_H
+#define NAVIGATION_FUSION_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "fusion/fusion_engine.h"
+#include "fusion/fusion_measurement.h"
+#include "fusion/fusion_motion.h"
+#include "imu/imu_sample.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;
+	uint64_t gps_position_recoveries;
+	uint64_t zero_update_count;
+	uint64_t replay_count;
+	uint64_t stale_gps_count;
+	uint64_t imu_processed_count;
+	unsigned int calibration_sample_count;
+	fusion_engine_state_t engine_state;
+	bool local_valid;
+	bool global_valid;
+	bool vehicle_at_rest;
+	/* Compatibility alias for UART publishers: valid means global LLH valid. */
+	bool valid;
+} navigation_fusion_output_t;
+
+/* Initialize the sensor-independent 15-state ESKF and GPS-to-ENU adapter. */
+bool navigation_fusion_init(
+	uint64_t local_timebase_hz,
+	const fusion_imu_quality_profile_t* imu_quality,
+	bool platform_landed);
+
+/* PX4-style external platform fact. For this bench prototype `true` means
+ * unarmed/on-ground; a mobile integration must drive it from its own
+ * arm/contact/land supervisor. */
+void navigation_fusion_set_platform_landed(
+	bool platform_landed,
+	uint64_t timestamp_ticks);
+
+/* Prediction input: every timestamped IMU sample is integrated; it is not an
+ * ESKF observation and is never rate-selected in this layer. */
+bool navigation_fusion_push_imu(const imu_sample_t* measurement);
+
+/* Observation input: convert a valid LLH fix to local ENU, apply it at the
+ * captured GPS epoch, then replay newer IMU history to current time. */
+bool navigation_fusion_push_gps(const fusion_gps_measurement_t* measurement);
+
+/* local_valid can exist without GPS. global_valid/valid require a GPS origin. */
+bool navigation_fusion_get_output(navigation_fusion_output_t* output);
+const char* navigation_fusion_state_name(fusion_engine_state_t state);
+
+#endif

soc/software/gps_system/header/gps/NavEquations.h

@@ -1,11 +1,11 @@
-#ifndef NAVEQUATIONS_H 
+#ifndef NAVEQUATIONS_H
 	#define NAVEQUATIONS_H
 
 		#include "gps.h"
-		#include "gps_ip_defines.h"
-		#include "matrixLib.h"
+		#include "gps/gps_ip_defines.h"
+		#include "gps/matrixLib.h"
 		
-		typedef struct {
+		typedef struct gps_nav_solution {
 			double ecef_m[3];
 			double clock_bias_m;
 			double latitude_deg;
@@ -39,6 +39,7 @@
 			double quality_score;
 			unsigned int available_satellite_count;
 			unsigned int used_satellite_count;
+			unsigned int tracked_satellite_count;
 			unsigned int degrees_of_freedom;
 			uint32_t used_row_mask;
 			uint32_t excluded_row_mask;

soc/software/gps_system/header/gps/Navigation.h

@@ -558,4 +558,3 @@
 	
 
 #endif
-

soc/software/gps_system/header/gps/gps.h

@@ -4,7 +4,7 @@
 #define GPS_H
 
 	//#include "plutogpssim.h"
-	#include "gps_ip_defines.h"
+	#include "gps/gps_ip_defines.h"
 	
 	//#define GM_EARTH 3.986005e14
 	//#define OMEGA_EARTH 7.2921151467e-5

soc/software/gps_system/header/gps/gps_interrupt_handlers.h

@@ -1,7 +1,7 @@
 #ifndef GPS_INT_HAND
 	#define GPS_INT_HAND
  
-	#include "gps_ip_defines.h"
+	#include "gps/gps_ip_defines.h"
 
 	typedef struct {
 		uint32_t event_overflow_total;
@@ -16,9 +16,14 @@
 		uint64_t last_lost_tick;
 	} gps_runtime_diagnostics_t;
 
+	typedef void (*gps_external_irq_handler_t)(void);
+
 	bool assign_tracking_from_result(const gps_acq_result_t* result);
 	void acq_intr_handler(unsigned char engine_id);
 	void gps_dispatch_irq(unsigned int irq_id);
+	bool gps_interrupt_attach_external_source(
+		unsigned int irq_id,
+		gps_external_irq_handler_t handler);
 	void gps_irq_handler(void) GPS_IRQ_HANDLER_ATTR;
 	void gps_process_isr_events(void);
 	void gps_acq_scheduler_run(void);

soc/software/gps_system/header/gps/gps_ip_defines.h

@@ -1,4 +1,4 @@
-#ifndef GPS_IP_DEFINES 
+#ifndef GPS_IP_DEFINES
 	#define GPS_IP_DEFINES
 
 	#include <stdbool.h>
@@ -6,8 +6,8 @@
 	#include <float.h>
 	#include <stdint.h>
 	#include <bsp/soc.h>
-	#include "matrixLib.h"
-	#include "gps_macros.h"
+	#include "gps/matrixLib.h"
+	#include "gps/gps_macros.h"
 
 	#define GPS_IRQ_HANDLER_ATTR __attribute__((interrupt("machine"), aligned(4)))
 
@@ -49,6 +49,7 @@
 			#ifndef GPS_NAV_SOLVE_TIMER_TICKS
 			#define GPS_NAV_SOLVE_TIMER_TICKS   50u
 			#endif
+			#define GPS_NAV_BITS_PER_WEEK       30240000u
 			#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
@@ -64,7 +65,11 @@
 			#define GPS_NAV_TOW_CLUSTER_TOLERANCE_SEC 0.001
 			#define GPS_HOW_ANCHOR_MAX_AGE_SEC         12.0
 
-			_Static_assert(GPS_NAV_SOLVE_TIMER_TICKS > 0u, "NAV solve timer must be non-zero");
+			#if (GPS_NAV_SOLVE_TIMER_TICKS == 0u)
+			#error "GPS_NAV_SOLVE_TIMER_TICKS must be non-zero"
+			#elif ((GPS_NAV_BITS_PER_WEEK % GPS_NAV_SOLVE_TIMER_TICKS) != 0u)
+			#error "GPS_NAV_SOLVE_TIMER_TICKS must divide 30240000 GPS navigation bits per week"
+			#endif
 			_Static_assert((GPS_NAV_ENABLE_FDE == 0u) || (GPS_NAV_ENABLE_FDE == 1u),
 			               "GPS_NAV_ENABLE_FDE must be zero or one");
 
@@ -460,10 +465,14 @@
 	// parse functions. Generic parse function.
 		unsigned char parse_subframe(int unsigned* subframe, gps_frame_t* gps_frame,
 		                             unsigned int* how_tow_sec_out);
+	struct gps_nav_solution;
 #ifdef EKF_ENABLE
-void calculate_position(Ekf* g_ekf);
+	bool calculate_position(Ekf* g_ekf,
+	                        struct gps_nav_solution* solution_out,
+	                        uint64_t* measurement_timestamp_ticks_out);
 #else
-void calculate_position();
+	bool calculate_position(struct gps_nav_solution* solution_out,
+	                        uint64_t* measurement_timestamp_ticks_out);
 #endif
 	void gps_startup_initialization(unsigned int nsp_tick);
 	void gps_interrupt_init(void);

soc/software/gps_system/header/gps/gps_macros.h

@@ -5,8 +5,9 @@
 	
 	/*
 	 * Verbosity is cumulative:
-	 *   1: navigation result (position and DOP)
-	 *   2: tracked channel/PRN summary
+	 *   1: navigation result (position and all DOP values) plus tracked
+	 *      satellite count/PRN summary
+	 *   2: reserved for additional status detail
 	 *   3: state transitions, warnings/errors and integrity summary
 	 *   4: performance, NAV satellite and low-level diagnostic detail
 	 *
@@ -14,7 +15,7 @@
 	 * levels. GPS_DEBUG_LEVEL should normally be in the range 1..4.
 	 */
 		#define DEBUG_STATUS   1
-		#define DEBUG_CHANNELS 2
+		#define DEBUG_CHANNELS DEBUG_STATUS
 		#define DEBUG_EVENTS   3
 		#define DEBUG_TRACE    4
 
@@ -31,6 +32,7 @@
 		#define GPS_LOG_CAT_MEM   "MEM"
 		#define GPS_LOG_CAT_HWREG "HWREG"
 		#define GPS_LOG_CAT_FUSION "FUSION"
+		#define GPS_LOG_CAT_LOCATION "LOCATION"
 	
 		// Set the current verbosity level
 		#ifdef DEBUG

soc/software/gps_system/header/gps/gps_receiver.h

@@ -0,0 +1,41 @@
+#ifndef GPS_RECEIVER_H
+#define GPS_RECEIVER_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+struct gps_nav_solution;
+
+/*
+ * High-level boundary around the legacy GPS acquisition/tracking subsystem.
+ *
+ * initialize(): resets GPS hardware/software state and installs the machine
+ * interrupt handler used by the receiver.
+ *
+ * start_acquisition(): schedules the initial PRN acquisition work.
+ *
+ * process_pending_events(): foreground half of GPS interrupt processing.  It
+ * consumes ISR events, handles channel loss/reacquisition and timer work.
+ *
+ * solve_ready_epoch(): when at least four coherent channels report the same
+ * nav-solve epoch, snapshots them and runs WLS.  The returned timestamp is the
+ * measurement epoch, not the later time at which WLS completed.
+ */
+void gps_receiver_initialize(unsigned int nav_solve_timer_ticks);
+typedef void (*gps_receiver_external_irq_handler_t)(void);
+/* Attach one application-owned PLIC source to the installed trap dispatcher.
+ * The source driver remains responsible for its priority and enable state. */
+bool gps_receiver_attach_external_irq(
+	unsigned int irq_id,
+	gps_receiver_external_irq_handler_t handler);
+void gps_receiver_start_acquisition(void);
+void gps_receiver_process_pending_events(void);
+bool gps_receiver_solve_ready_epoch(
+	struct gps_nav_solution* solution_out,
+	uint64_t* measurement_timestamp_ticks_out);
+
+/* Slow status clock maintained by the GPS machine-timer interrupt. */
+uint64_t gps_receiver_status_ticks(void);
+uint64_t gps_receiver_status_ticks_per_second(void);
+
+#endif

soc/software/gps_system/header/gps/linked_list.h

@@ -1,7 +1,7 @@
 #ifndef LINKED_LIST_H_INCLUDED
 #define LINKED_LIST_H_INCLUDED
 
-#include "gps_ip_defines.h"
+#include "gps/gps_ip_defines.h"
 
 typedef struct gps_ephem_ll {
 	volatile gps_ephemeris_t* data_p;

soc/software/gps_system/header/gps_fusion_publisher.h

@@ -1,12 +0,0 @@
-#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_system.h

@@ -0,0 +1,28 @@
+#ifndef GPS_SYSTEM_APP_H
+#define GPS_SYSTEM_APP_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "fusion/fusion_measurement.h"
+#include "fusion/navigation_fusion.h"
+#include "gps/NavEquations.h"
+#include "imu/imu_sample.h"
+
+/* Application-owned messages and lifecycle state at the GPS/IMU/fusion
+ * boundary.  Solver, driver and ESKF private state remain in their modules. */
+typedef struct {
+	gps_nav_solution_t gps_solution;
+	fusion_gps_measurement_t gps_for_fusion;
+	imu_sample_t imu_sample;
+	navigation_fusion_output_t fused_solution;
+	uint64_t gps_measurement_ticks;
+	uint64_t last_status_tick;
+	bool gps_epoch_ready;
+	bool fusion_enabled;
+	bool imu_initialized;
+	bool fusion_initialized;
+	bool imu_running;
+} gps_system_state_t;
+
+#endif

soc/software/gps_system/header/imu/imu_calibration.h

@@ -3,7 +3,7 @@
 
 #include <stdbool.h>
 
-#include "fusion_measurement.h"
+#include "imu/imu_sample.h"
 
 /*
  * Sensor-independent calibration in the IMU body frame.
@@ -28,6 +28,6 @@ typedef struct {
 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);
+	                       imu_sample_t* measurement);
 
 #endif

soc/software/gps_system/header/imu/imu_sample.h

@@ -0,0 +1,18 @@
+#ifndef IMU_SAMPLE_H
+#define IMU_SAMPLE_H
+
+#include <stdint.h>
+
+/*
+ * Sensor-independent, calibrated inertial sample in SI units.  Drivers and
+ * IMU management publish this type without depending on GPS or ESKF headers.
+ */
+typedef struct {
+	uint64_t timestamp_ticks;
+	double accel_mps2[3];
+	double gyro_rps[3];
+	double temperature_c;
+	uint32_t sequence;
+} imu_sample_t;
+
+#endif

soc/software/gps_system/header/imu/imu_sensor.h

@@ -4,9 +4,8 @@
 #include <stdbool.h>
 #include <stdint.h>
 
-#include "fusion_measurement.h"
-#include "fusion_imu_quality.h"
-#include "imu_calibration.h"
+#include "imu/imu_calibration.h"
+#include "imu/imu_sample.h"
 
 typedef enum {
 	IMU_SENSOR_NONE = 0,
@@ -16,7 +15,8 @@ typedef enum {
 } imu_sensor_backend_t;
 
 typedef struct {
-	/* Zero selects the active backend's safe default. */
+	/* Zero selects the active backend's safe default.  A non-zero target may
+	 * be quantized to a nearby hardware ODR; diagnostics reports both values. */
 	uint32_t odr_hz;
 	unsigned int accel_full_scale_g;
 	unsigned int gyro_full_scale_dps;
@@ -32,6 +32,7 @@ typedef struct {
 	uint64_t queue_overflow_count;
 	uint64_t estimated_missed_sample_count;
 	uint64_t last_timestamp_ticks;
+	uint32_t requested_odr_hz;
 	uint32_t configured_odr_hz;
 	uint8_t who_am_i;
 	imu_sensor_backend_t backend;
@@ -41,25 +42,27 @@ typedef struct {
 } imu_sensor_diagnostics_t;
 
 void imu_sensor_default_config(imu_sensor_config_t* config);
+
+/* Probe the compiled backend(s), select one, validate the requested ODR/range
+ * and prepare calibration.  This does not enable data-ready interrupts. */
 bool imu_sensor_init(const imu_sensor_config_t* config);
+
+/* Configure streaming and enable BSP_PMOD_IRQ.  The application must attach
+ * imu_sensor_irq_handler to the active machine-interrupt router first. */
 bool imu_sensor_start(void);
 void imu_sensor_stop(void);
 
-/* Interrupt backend hook. Safe no-op for a foreground-polled backend. */
+/* PLIC/ISR half: read and timestamp one raw sample, then enqueue it. */
 void imu_sensor_irq_handler(void);
 
-/* Foreground acquisition hook. Safe no-op for an interrupt backend. */
+/* Foreground half. Safe no-op for the current interrupt-driven backends. */
 void imu_sensor_service(void);
 bool imu_sensor_requires_foreground_service(void);
 
-bool imu_sensor_take_measurement(fusion_imu_measurement_t* measurement_out);
+/* Pop the oldest sample, convert to SI/common axes and apply the selected
+ * backend's calibration.  No averaging or motion decision occurs here. */
+bool imu_sensor_take_measurement(imu_sample_t* measurement_out);
 void imu_sensor_get_diagnostics(imu_sensor_diagnostics_t* diagnostics_out);
 imu_sensor_backend_t imu_sensor_active_backend(void);
 const char* imu_sensor_backend_name(imu_sensor_backend_t backend);
-bool imu_sensor_quality_profile_for_backend(
-	imu_sensor_backend_t backend,
-	fusion_imu_quality_profile_t* profile_out);
-bool imu_sensor_active_quality_profile(
-	fusion_imu_quality_profile_t* profile_out);
-
 #endif

soc/software/gps_system/header/navigation_fusion.h

@@ -1,33 +0,0 @@
-#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