est_pipeline.h

Go to the documentation of this file.

 
#ifndef EST_PIPELINE_H
#define EST_PIPELINE_H
 
#include <stdint.h>
#include <stddef.h>
#include "iekf.h"
#include "accwin.h"
#include "calib.h"
#include "imu_lsm6dsv16x.h"
 
#define EST_F_RUNNING    0x01u
#define EST_F_MAG_NEW    0x02u
#define EST_F_ACC_GATED  0x04u
#define EST_F_MAG_GATED  0x08u
#define EST_F_STILL      0x20u   
typedef struct {
    uint8_t       gyro_per_step;     
    uint8_t       steps_per_acc;     
    uint16_t      calib_samples;     
    float         calib_gyro_still;  
    float         sigma_vec_min;
    iekf_config_t ekf;               
    float         mag_map[9];        
    float         mag_hard_iron[3];  
    float         mag_scale[3];      /* per-axis soft-iron scale (COMPASS calibration);
                                        0 = 1 (configs that do not set it)       */
    float         mag_latency_s;
    float         mag_latency_std_s;
    float         still_acc_tol;     
    float         still_gyro_thr;    
    float         still_time_s;      
} est_config_t;
 
/* ---- trace of the filter calls (parity test with MATLAB) ----
 * Every iekf_* call within an acc-update window is recorded with its EXACT
 * arguments. MATLAB replays it identically and compares state and covariance:
 * this checks the filter independently of accwin, mag latency, etc.
 * Encoding (little-endian, float32):
 *   'P' dtheta[3] h          propagation                     17 bytes
 *   'A' acc[3] trusted(u8)   accelerometer update            14 bytes
 *   'M' mag[3] extra_var     magnetometer update             17 bytes */
#define EST_TRACE_MAX   140u
#define EST_OP_PROP     0x50u   
#define EST_OP_ACC      0x41u   
#define EST_OP_MAG      0x4Du   
typedef struct {
    uint8_t  buf[EST_TRACE_MAX];
    uint16_t len;
    uint8_t  nops;
    uint8_t  overflow;
} est_trace_t;
 
typedef struct est_s est_t;
typedef void (*est_cb_t)(const est_t *e, void *user);
 
struct est_s {
    est_config_t cfg;
    iekf_t       f;
    calib_t      cal;
    accwin_t     win;
    uint8_t      running;
 
    float    dth[3], gsum[3];
    uint8_t  k;
    uint32_t step;
 
    float    acc_mean[3], gyro_mean[3], mag[3];
    float    omega[3];                
    float    gwin_sum[3];
    uint16_t gwin_n;
    uint8_t  have_acc_mean;
    uint8_t  flags;                  
    float    t;                      
    float    still_t;                
    calib_result_t calres;
 
    uint8_t     trace_enable;
    est_trace_t trace;               
    uint32_t    win_count;           
    est_cb_t on_state;               
    est_cb_t on_init;                
    void    *user;
};
 
void est_init(est_t *e, const est_config_t *cfg, est_cb_t on_state, est_cb_t on_init, void *user);
void est_process(est_t *e, const imu_event_t *ev, size_t n, float dt_sample);
 
#endif /* EST_PIPELINE_H */