telemetry.h

Go to the documentation of this file.

 
#ifndef TELEMETRY_H
#define TELEMETRY_H
 
#include <stdint.h>
#include <stddef.h>
 
#define TLM_SYNC0       0xA5u
#define TLM_SYNC1       0x5Au
#define TLM_TYPE_STATE  0x01u
#define TLM_TYPE_INIT   0x02u
#define TLM_TYPE_STATUS 0x03u   
#define TLM_TYPE_REPLAY 0x04u   
#define TLM_OVERHEAD    6u
 
/* STATE packet flags */
#define TLM_F_RUNNING    0x01u
#define TLM_F_MAG_NEW    0x02u   
#define TLM_F_ACC_GATED  0x04u   
#define TLM_F_MAG_GATED  0x08u
#define TLM_F_OVERRUN    0x10u   
#define TLM_F_STILL      0x20u   
/* INIT packet flags */
#define TLM_I_EST_BIAS   0x01u
#define TLM_I_ADAPTIVE   0x02u
#define TLM_I_TILT_COMP  0x04u
#define TLM_I_DECOUPLE   0x08u
#define TLM_I_RESET_JAC  0x10u
#define TLM_I_TANGENT    0x20u   
/* WARNING: "packed" structs = float fields NOT 4-byte aligned
 * (e.g. tlm_state_t.q is at offset 6). Never pass the address of a field to a
 * function that writes floats through a pointer: on Cortex-M4F the compiler uses
 * VSTR, which on an unaligned address raises UsageFault -> HardFault.
 * Always write into a local variable first, then copy with memcpy. */
#pragma pack(push, 1)
typedef struct {
    uint16_t seq;        
    uint32_t t_us;       
    float    q[4];       
    float    bias[3];    
    float    acc[3];     
    float    gyro[3];    
    float    mag[3];     
    uint8_t  flags;
} tlm_state_t;           
typedef struct {
    float   q0[4];
    float   g_ref[3];
    float   m_ref[3];
    float   b0[3];
    float   sigma_acc, sigma_mag, sigma_gyro, sigma_bias;
    float   nom_acc, nom_mag;
    float   fs_hz;
    float   acc_tol, mag_tol;
    float   p0_att, p0_bias;
    float   acc_nis_gate, mag_nis_gate, infl_decay, dip_tol;
    float   infl_max, lockout_p_bump, persist_decay, persist_ratio, persist_min;
    uint16_t lockout_updates, grace_updates;
    uint8_t flags;
} tlm_init_t;            
typedef struct {
    uint32_t uptime_ms;
    uint32_t irq_count;      
    uint32_t bus_err;        
    uint32_t fifo_ovr;       
    uint32_t tlm_drop;       
    uint32_t uart_drop;      
    uint32_t proc_cyc_max;   
    uint32_t n_gyro, n_acc, n_mag, n_mag_dup, n_sh_nack;   
    uint16_t words_max;      
    uint16_t calib_count;    
    int8_t   imu_init;       
    uint8_t  running;        
    /* ---- phase 2: BLE link (all 0 with APP_TLM_BLE = 0) ---- */
    uint32_t ble_drop;       
    uint16_t ble_mtu;        
    uint8_t  ble_state;      /* tlm_ble_state_t: 0 off, 1 advertising,
                                2 connected, 3 streaming                 */
    uint8_t  ble_err;        
} tlm_status_t;              
#pragma pack(pop)
 
#if defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 201112L)
_Static_assert(sizeof(tlm_state_t) == 71, "tlm_state_t layout");
_Static_assert(sizeof(tlm_init_t)  == 137, "tlm_init_t layout");
_Static_assert(sizeof(tlm_status_t) == 62, "tlm_status_t layout");
#endif
 
/* REPLAY (variable length, max TLM_REPLAY_MAX payload bytes):
 *   uint32 win | uint8 nops | uint8 flags(bit0 = trace truncated)
 *   ops[...]   (encoding in est_pipeline.h)
 *   float q[4] b[3] Pdiag[6] acc_infl mag_infl     (state AFTER the window) */
#define TLM_REPLAY_TAIL  60u
#define TLM_REPLAY_MAX   (6u + 140u + TLM_REPLAY_TAIL)
#define TLM_FRAME_MAX    (TLM_REPLAY_MAX + TLM_OVERHEAD)
 
uint16_t tlm_crc16(const uint8_t *data, size_t len);
size_t   tlm_encode(uint8_t type, const void *payload, uint8_t len, uint8_t *out, size_t cap);
 
#endif /* TELEMETRY_H */