servo_sync.h

Go to the documentation of this file.

 
#pragma once
 
#include <stdbool.h>
#include <stdint.h>
 
typedef enum {
    SERVO_SYNC_RUNNING = 0,
    SERVO_SYNC_DONE,
    SERVO_SYNC_FAULT,
} servo_sync_state_t;
 
typedef enum {
    SERVO_SYNC_FAULT_NONE = 0,
    SERVO_SYNC_FAULT_OVERCURRENT,
    SERVO_SYNC_FAULT_NO_MINIMUM,
} servo_sync_fault_t;

typedef enum {
    SERVO_SYNC_STAGE_CENTRE = 0,
    SERVO_SYNC_STAGE_HIGH,
    SERVO_SYNC_STAGE_LOW,
    SERVO_SYNC_STAGE_COUNT,
} servo_sync_stage_t;

#define SERVO_SYNC_MAX_POINTS 9
 
typedef struct {
    uint16_t centre_us;
    uint16_t deflection_us; 
 
    int16_t  window_us;     
    uint8_t  points;        
    uint8_t  rounds;        

    uint16_t settle_ms;
    float    slew_us_per_ms;
    uint16_t measure_ms;

    float    min_depth_a;
 
    float    hard_limit_a;
} servo_sync_cfg_t;

#define SERVO_SYNC_MIN_US 500u
#define SERVO_SYNC_MAX_US 2500u
 
typedef struct {
    servo_sync_cfg_t   cfg;
    servo_sync_state_t state;
    servo_sync_fault_t fault;
 
    /* The answers. All three are what the *second* servo should be commanded
     * to, given the first is at centre, at +deflection and at -deflection. */
    int16_t  trim_us;       
    int16_t  travel_hi_us;  
    int16_t  travel_lo_us;  

    float    best_a[SERVO_SYNC_STAGE_COUNT];
    float    worst_a[SERVO_SYNC_STAGE_COUNT];
 
    /* Internals. */
    servo_sync_stage_t stage;
    uint8_t  round;
    uint8_t  point;
    int32_t  win_centre_us;
    int32_t  win_half_us;
    float    scan_a[SERVO_SYNC_MAX_POINTS];
    uint32_t phase_start_ms;
    uint32_t travel_ms;     
    bool     measuring;
    double   sum_a;
    uint32_t samples;
    bool     started;
    uint16_t cmd_a_us;
    uint16_t cmd_b_us;
} servo_sync_t;

void servo_sync_defaults(servo_sync_cfg_t *cfg, uint16_t centre_us);

void servo_sync_init(servo_sync_t *sy, const servo_sync_cfg_t *cfg);

void servo_sync_step(servo_sync_t *sy, float total_a, uint32_t now_ms,
                     uint16_t *cmd_a, uint16_t *cmd_b);

float servo_sync_progress(const servo_sync_t *sy);