#ifndef FUSION_ENGINE_H #define FUSION_ENGINE_H #include #include #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_accel_offset_allowance_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; double gnss_velocity_motion_timeout_s; 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]; /* * Displacement baseline for GNSS motion evidence. Held separately from * the accepted-fix baseline because the evidence must keep updating while * the innovation gate is rejecting fixes -- that is precisely when it is * the only signal able to release the zero-velocity update. */ double last_evidence_position_m[3]; double last_evidence_position_covariance_m2[9]; double last_gnss_speed_mps; 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_evidence_position_ticks; uint64_t last_gnss_velocity_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 have_evidence_position; bool have_gnss_velocity; /* Latched once GNSS Doppler has shown the platform moving, and cleared * only by a later fix showing it slow. A timeout cannot clear it: an * unaided platform gives no evidence that it has stopped. */ bool gnss_confirmed_moving; 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 velocity_received; uint64_t velocity_nis_rejected; uint64_t velocity_accepted; 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]; /* Running statistics of the accelerometer magnitude. Comparing a sample * against the window's own mean cancels any fixed sensor offset, which * leaves the whole norm tolerance available to detect real acceleration. */ double norm_mean; double norm_m2; 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_navigation_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_push_navigation( fusion_engine_t* engine, const fusion_navigation_measurement_t* measurement, eskf_update_result_t* position_update_result_out, double* position_nis_out, eskf_update_result_t* velocity_update_result_out, double* velocity_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