Integrate asynchronous GPS IMU fusion

2026-07-16 23:49:43 +03:00
parent 38a96dd88f
commit 7cc3eb2a54
80 changed files with 3901 additions and 553 deletions
@@ -0,0 +1,142 @@
+#ifndef FUSION_ENGINE_H
+#define FUSION_ENGINE_H
+
+#include <stdbool.h>
+#include <stdint.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
+#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;
+	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 {
+	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];
+	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 {
+	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 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 {
+	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_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,
+	                              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);
+
+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