64 lines
1.7 KiB
C
64 lines
1.7 KiB
C
#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
|