124 lines
3.5 KiB
C
124 lines
3.5 KiB
C
#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
|