sense_sched.h

Go to the documentation of this file.

 
#pragma once
 
#include <stdbool.h>
#include <stdint.h>
 
#include "as5600.h"
#include "ina228.h"
#include "ina3221.h"
#include "sense_bus.h"
#include "servo_move.h"
 
#ifdef __cplusplus
extern "C" {
#endif
 
#define SENSE_WINDOW_MS      50u   
#define SENSE_ROTATION       20u
#define SENSE_WIN_RING        4u
#define SENSE_CAP_FILTER_N    4u
#define SENSE_ENC_STILL_TOL  12u
#define SENSE_ENC_MAG_EVERY  25u
#define SENSE_CAP_SETTLE_N   10u
#define SENSE_CAP_LAG_MS      5u
#define SENSE_CAP_T_PER_MS   10u
#define SENSE_CAP_EDGE_WAIT_MS SERVO_MOVE_TIMEOUT_MS
#define SENSE_CAP_RISE_AUTO  INT32_MIN
#define SENSE_FAST_HISTORY   SENSE_WINDOW_MS

#define SENSE_I3221_PARTS   4u
#define SENSE_CHANNELS      12u
_Static_assert(SENSE_CHANNELS == SENSE_I3221_PARTS * INA3221_CHANNELS,
               "three channels a part");
static inline unsigned sense_ch(unsigned a, unsigned ch)
{
    return a * INA3221_CHANNELS + ch - 1u;
}
#define SENSE_CH(a, ch)     sense_ch((unsigned)(a), (unsigned)(ch))
#define SENSE_FAST_NONE     0xFFu

#define SENSE_SRC_INA228    SENSE_CHANNELS
#define SENSE_SRC_COUNT     (SENSE_CHANNELS + 1u)

typedef struct {
    sense_i2c_t i2c;
    uint64_t  (*now_us)(void *ctx);  
    void      (*recover)(void *ctx); 
    void       *ctx;                    
} sense_sched_io_t;

typedef struct {
    uint8_t  fitted;                      
    uint32_t shunt_uohm[SENSE_CHANNELS];  
    uint8_t  fast;                        
} sense_i3221_cfg_t;

typedef struct {
    bool     ina228_en;
    uint8_t  ina228_addr;
    uint32_t ina228_shunt_uohm;
    uint32_t ina228_max_ma;
    sense_i3221_cfg_t i3221;
    bool     as5600_en;            
} sense_sched_cfg_t;

void sense_i3221_one(sense_i3221_cfg_t *m, bool en, uint8_t addr,
                     uint32_t shunt_uohm, uint8_t channels);

typedef struct {
    uint16_t number;      
    uint16_t n_i;         
    uint16_t n_v;         
    bool     clip_hi;     
    bool     clip_lo;     
    int32_t i_mean_ua,                      
            i_min_ua,                       
            i_max_ua;                       
    int32_t v_mean_uv,                      
            v_min_uv;                       
} sense_window_t;

typedef struct {
    int64_t  i_sum, v_sum;
    uint16_t n_i, n_v;
    uint16_t n_hi, n_lo;  
    bool clip_hi,                           
         clip_lo;                           
    int32_t i_min,                          
            i_max,                          
            v_min;                          
} sense_acc_t;

typedef struct {
    bool     closed;      
    uint16_t n_i;         
    uint16_t n_clip;      
    uint16_t n_v;         
    bool     clip_hi;     
    bool     clip_lo;     
    int32_t i_mean_ua,                      
            i_min_ua,                       
            i_max_ua;                       
    int32_t v_mean_uv,                      
            v_min_uv;                       
} sense_ring_win_t;

typedef struct {
    bool     have_v;      
    bool     have_i;      
    bool     have_p;      
    bool     i_clipped;   
    int32_t  v_min_uv;                  
    int32_t  i_max_ua;                  
    int64_t  p_max_uw;    
    bool     clear_owed;  
    bool     totals_ok;   
    uint64_t energy_mj;   
    int64_t  charge_uc;   
} sense_run_t;

typedef enum {
    SENSE_CAP_IDLE = 0,
    SENSE_CAP_ARMED,      
    SENSE_CAP_WAITING,    
    SENSE_CAP_MOVING,
    SENSE_CAP_ARRIVED,
    SENSE_CAP_SETTLED,    
    SENSE_CAP_LATE,       
    SENSE_CAP_UNSEEN,     
    SENSE_CAP_LOST,       
} sense_cap_state_t;

typedef struct {
    int32_t rise_ua;      
    int32_t hold_ua;      
    int32_t move_ua;      
    int32_t band_ua;      
} sense_cap_arm_t;

typedef struct {
    float  a;
    int8_t clip;          
} sense_cap_pre_t;

typedef struct {
    uint32_t t;           
    int32_t  ua;          
} sense_fast_t;

typedef struct {
    sense_cap_state_t state;
    uint16_t seq;         
    sense_cap_arm_t arm;                
    uint32_t edge_t;      
    uint32_t arm_t;       
    bool     rise_owed;   
    /* The last SENSE_CAP_FILTER_N FAST samples while armed. */
    sense_cap_pre_t pre[SENSE_CAP_FILTER_N];
    uint8_t  pre_n, pre_head;
    /* Once over: */
    uint32_t move_t;      
    uint32_t arrive_t;    
    int32_t  peak_ua;     
    int32_t  mean_ua;     
    uint32_t samples;     
    bool     clipped;     
    servo_move_t mv;
} sense_cap_t;

typedef struct {
    as5600_t dev;
    bool     have_angle;   
    uint8_t  status;       
    uint16_t raw;          
    uint16_t samples;      
    bool     have_mag;                  
    uint8_t  agc;                       
    uint16_t magnitude;                 
    uint32_t slots;        
    uint16_t anchor;       
    uint64_t anchor_ms;    
    bool     anchored;     
} sense_enc_t;

typedef struct {
    sense_sched_io_t    io;
    sense_sched_cfg_t   cfg;
    sense_bus_t         bus;
    ina228_t            i228;
    ina3221_t           i3221[SENSE_I3221_PARTS];
    ina228_setup_err_t  i228_setup;   
    ina3221_setup_err_t i3221_setup[SENSE_I3221_PARTS]; 
    /* Per channel: a clipped sample is at least clip, the top and bottom
     * codes of the range stand for top and bottom; 0 for a channel off. */
    int32_t  ch_clip_ua[SENSE_CHANNELS];
    int32_t  ch_top_ua[SENSE_CHANNELS];
    int32_t  ch_bottom_ua[SENSE_CHANNELS];
    /* The channels read in turn beside FAST, one a tick. */
    uint8_t  others[SENSE_CHANNELS];
    uint8_t  n_others, other_at;
    uint8_t  volt_at;        
    uint32_t ticks;
    uint32_t gen;            
    uint32_t rot;            
    bool     started;
    uint64_t t0_ms;          
    uint64_t win;            
    sense_acc_t    acc[SENSE_SRC_COUNT];
    sense_window_t last[SENSE_SRC_COUNT];
    bool     have_last;
    uint8_t  last_online;    
    /* FAST's last windows, newest first; ring_at numbers entry 0. */
    sense_ring_win_t ring[SENSE_WIN_RING];
    uint64_t ring_at;
    uint8_t  i228_resets;    
    uint8_t  i3221_resets[SENSE_I3221_PARTS]; 
    bool     have_vbus;      
    int32_t  vbus_uv;
    bool     have_temp, have_diag;
    uint8_t  have_flags;     
    int32_t  temp_mdegc;     
    uint16_t diag;           
    uint16_t flags[SENSE_I3221_PARTS]; 
    sense_run_t run;
    sense_cap_t cap;
    sense_enc_t enc;
    /* The last SENSE_FAST_HISTORY FAST samples, oldest overwritten. */
    sense_fast_t hist[SENSE_FAST_HISTORY];
    uint8_t  hist_n, hist_head;
} sense_sched_t;

void sense_sched_init(sense_sched_t *s, const sense_sched_io_t *io,
                      const sense_sched_cfg_t *cfg);

void sense_sched_tick(sense_sched_t *s);

void sense_sched_arm(sense_sched_t *s);

bool sense_sched_window(const sense_sched_t *s, unsigned src,
                        sense_window_t *out);

bool sense_sched_ring(const sense_sched_t *s, unsigned back,
                      sense_ring_win_t *out);

bool sense_sched_cap_arm(sense_sched_t *s, const sense_cap_arm_t *levels);

void sense_sched_cap_edge(sense_sched_t *s, uint64_t edge_us);

void sense_sched_cap_disarm(sense_sched_t *s);

const ina3221_t *sense_sched_fast_part(const sense_sched_t *s);

uint16_t sense_sched_enc_still_ms(const sense_sched_t *s);
 
#ifdef __cplusplus
}
#endif