alt_est.h

Go to the documentation of this file.

 
#ifndef ALT_EST_H
#define ALT_EST_H
 
#include <stdint.h>
 
typedef struct {
    float sigma_acc;        
    float sigma_acc_bias;   
    float sigma_baro_bias;  
    float sigma_baro;       
    float sigma_tof;        
    float tof_min_m, tof_max_m;
    float tof_max_tilt_cos; 
    float tof_offset_m;     
} alt_est_cfg_t;
 
typedef struct {
    alt_est_cfg_t c;
    float    x[4];
    float    P[4][4];
    float    p_ref;          
    float    p_sum; uint16_t p_n;
    uint8_t  baro_ok;
    float    baro_alt;       
    float    pressure;       
    float    ground;         
    float    tof_h;          
    uint8_t  tof_valid;      
    uint8_t  tof_ground;     
    uint8_t  tof_rejects;
    float    tof_age_s;      
    uint8_t  have_tof;       
    uint8_t  valid;          
} alt_est_t;
 
void alt_est_default_cfg(alt_est_cfg_t *c);
void alt_est_init(alt_est_t *e, const alt_est_cfg_t *c);
void alt_est_predict(alt_est_t *e, float a_up, float dt);
void alt_est_baro(alt_est_t *e, float pa);
void alt_est_tof(alt_est_t *e, float range_m, uint8_t status_ok, float cos_tilt);
void alt_est_zero(alt_est_t *e);
 
float alt_est_accel_up(const float q[4], const float f_body_g[3], float nom_g);
 
static inline float alt_est_h(const alt_est_t *e)  { return e->x[0]; }
static inline float alt_est_vz(const alt_est_t *e) { return e->x[1]; }
 
#endif /* ALT_EST_H */