Integrate asynchronous GPS IMU fusion

soc/software/gps_system/header/fusion/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