saltylab-firmware/include/encoder_odom.h

#ifndef ENCODER_ODOM_H
#define ENCODER_ODOM_H

#include <stdint.h>
#include <stdbool.h>

/*
 * encoder_odom — quadrature encoder reading and differential-drive odometry
 * (Issue #632).
 *
 * HARDWARE:
 *   Left  encoder : TIM2 (32-bit) in encoder mode 3
 *                   CH1 = PA15 (AF1),  CH2 = PB3 (AF1)
 *   Right encoder : TIM3 (16-bit) in encoder mode 3
 *                   CH1 = PC6  (AF2),  CH2 = PC7 (AF2)
 *
 *   Both channels count on every edge (×4 resolution).
 *   TIM2 ARR = 0xFFFFFFFF (32-bit, never overflows in practice).
 *   TIM3 ARR = 0xFFFF (16-bit, delta decoded via int16_t subtraction).
 *
 * ODOMETRY MODEL (differential drive):
 *
 *   meters_per_tick = (π × wheel_diam_mm × 1e-3) / ticks_per_rev
 *
 *   d_left  = Δticks_left  × meters_per_tick
 *   d_right = Δticks_right × meters_per_tick
 *
 *   d_center = (d_left + d_right) / 2
 *   dθ       = (d_right - d_left) / wheel_base_mm × 1e-3   (radians)
 *
 *   x     += d_center × cos(θ)
 *   y     += d_center × sin(θ)
 *   θ     += dθ
 *
 *   For small dt this is the standard Euler-forward integration; suitable for
 *   the 50 Hz odometry tick rate.
 *
 * RPM:
 *   rpm = Δticks × 60.0 / (ticks_per_rev × dt_s)
 *
 * FLASH CONFIG  (ENC_FLASH_ADDR in sector 7):
 *   Stores ticks_per_rev, wheel_diam_mm, wheel_base_mm validated by magic.
 *   Falls back to compile-time defaults on magic mismatch.
 *   Sector 7 is shared with PID flash; saving encoder config must be
 *   coordinated with pid_flash_save_all() to avoid mutual erasure.
 *
 * TELEMETRY:
 *   JLINK_TLM_ODOM (0x8C) published at ENC_TLM_HZ (50 Hz):
 *     jlink_tlm_odom_t { int16 rpm_left, int16 rpm_right,
 *                        int32 x_mm, int32 y_mm,
 *                        int16 theta_cdeg, int16 speed_mmps }
 *   16 bytes, 22-byte frame.
 */

/* ---- Default hardware parameters (override in flash config) ---- */
/* Hoverboard 6.5" wheels with typical geared-motor encoder: */
#define ENC_TICKS_PER_REV_DEFAULT   1320u   /* 33 CPR × 40:1 gear = 1320 ticks/rev */
#define ENC_WHEEL_DIAM_MM_DEFAULT    165u   /* 6.5" ≈ 165 mm diameter              */
#define ENC_WHEEL_BASE_MM_DEFAULT    540u   /* ~540 mm axle-to-axle separation      */

/* ---- Flash config ---- */
/* Stored in sector 7 immediately before the PID schedule area (0x0807FF40).
 * 64-byte block: magic(4) + config(12) + pad(48). */
#define ENC_FLASH_ADDR          0x0807FF00UL
#define ENC_FLASH_MAGIC         0x534C4503UL  /* 'SLE\x03' — encoder config v3 */

typedef struct __attribute__((packed)) {
    uint32_t magic;           /* ENC_FLASH_MAGIC when valid                  */
    uint32_t ticks_per_rev;   /* encoder ticks per full wheel revolution      */
    uint16_t wheel_diam_mm;   /* wheel outer diameter (mm)                    */
    uint16_t wheel_base_mm;   /* lateral wheel separation centre-to-centre (mm) */
    uint8_t  _pad[48];        /* reserved — total 64 bytes                    */
} enc_flash_config_t;

/* ---- Runtime configuration ---- */
typedef struct {
    uint32_t ticks_per_rev;
    uint16_t wheel_diam_mm;
    uint16_t wheel_base_mm;
} enc_config_t;

/* ---- Runtime state ---- */
typedef struct {
    /* Encoder counters (last sampled) */
    uint32_t cnt_left;          /* last TIM2->CNT                             */
    uint16_t cnt_right;         /* last TIM3->CNT                             */

    /* Wheel speeds */
    int16_t  rpm_left;          /* left wheel RPM (signed; + = forward)       */
    int16_t  rpm_right;         /* right wheel RPM (signed)                   */
    int16_t  speed_mmps;        /* linear speed of centre point (mm/s)        */

    /* Pose (relative to last reset) */
    float    x_mm;              /* forward displacement (mm)                  */
    float    y_mm;              /* lateral displacement (mm, + = left)        */
    float    theta_rad;         /* heading (radians, + = CCW from start)      */

    /* Internal */
    float    meters_per_tick;   /* pre-computed from config                   */
    float    wheel_base_m;      /* wheel_base_mm / 1000.0                     */
    uint32_t last_tick_ms;      /* HAL_GetTick() at last encoder_odom_tick()  */
    uint32_t last_tlm_ms;       /* HAL_GetTick() at last TLM transmission     */

    enc_config_t cfg;           /* active hardware parameters                 */
} encoder_odom_t;

/* ---- Configuration ---- */
#define ENC_TLM_HZ      50u     /* JLINK_TLM_ODOM transmit rate (Hz)         */

/* ---- API ---- */

/*
 * encoder_odom_init(eo) — configure TIM2/TIM3 in encoder mode, load flash
 * config (falling back to defaults), reset pose.
 * Call once during system init.
 */
void encoder_odom_init(encoder_odom_t *eo);

/*
 * encoder_odom_tick(eo, now_ms) — sample encoder counters, update RPM and
 * integrate odometry.  Call from main loop at any rate ≥ 10 Hz (50 Hz ideal).
 */
void encoder_odom_tick(encoder_odom_t *eo, uint32_t now_ms);

/*
 * encoder_odom_reset_pose(eo) — zero x/y/theta without resetting counters or
 * config.  Call whenever odometry reference frame should be re-anchored.
 */
void encoder_odom_reset_pose(encoder_odom_t *eo);

/*
 * encoder_odom_save_config(cfg) — write enc_flash_config_t to ENC_FLASH_ADDR.
 * WARNING: erases sector 7 — must NOT be called while armed and must be
 * coordinated with PID flash saves (both records are in sector 7).
 * Returns true on success.
 */
bool encoder_odom_save_config(const enc_config_t *cfg);

/*
 * encoder_odom_load_config(cfg) — load config from flash.
 * Returns true if flash magic valid; false = defaults applied to *cfg.
 */
bool encoder_odom_load_config(enc_config_t *cfg);

/*
 * encoder_odom_send_tlm(eo, now_ms) — transmit JLINK_TLM_ODOM (0x8C) frame.
 * Rate-limited to ENC_TLM_HZ; safe to call every tick.
 */
void encoder_odom_send_tlm(const encoder_odom_t *eo, uint32_t now_ms);

#endif /* ENCODER_ODOM_H */