diff --git a/include/config.h b/include/config.h index 04a6c82..df063d0 100644 --- a/include/config.h +++ b/include/config.h @@ -278,4 +278,21 @@ #define UART_PROT_BAUD 115200u // baud rate for UART5 Jetson protocol #define UART_PROT_HB_TIMEOUT_MS 500u // heartbeat timeout: Jetson considered lost after 500 ms +// --- Encoder Odometry (Issue #632) --- +// Left encoder: TIM2 (32-bit), CH1=PA15 (AF1), CH2=PB3 (AF1) +// Right encoder: TIM3 (16-bit), CH1=PC6 (AF2), CH2=PC7 (AF2) +// Encoder mode 3: count on both A and B edges (x4 resolution) +#define ENC_LEFT_TIM TIM2 +#define ENC_LEFT_CH1_PORT GPIOA +#define ENC_LEFT_CH1_PIN GPIO_PIN_15 // TIM2_CH1, AF1 +#define ENC_LEFT_CH2_PORT GPIOB +#define ENC_LEFT_CH2_PIN GPIO_PIN_3 // TIM2_CH2, AF1 +#define ENC_LEFT_AF GPIO_AF1_TIM2 +#define ENC_RIGHT_TIM TIM3 +#define ENC_RIGHT_CH1_PORT GPIOC +#define ENC_RIGHT_CH1_PIN GPIO_PIN_6 // TIM3_CH1, AF2 +#define ENC_RIGHT_CH2_PORT GPIOC +#define ENC_RIGHT_CH2_PIN GPIO_PIN_7 // TIM3_CH2, AF2 +#define ENC_RIGHT_AF GPIO_AF2_TIM3 + #endif // CONFIG_H diff --git a/include/encoder_odom.h b/include/encoder_odom.h new file mode 100644 index 0000000..bbe9624 --- /dev/null +++ b/include/encoder_odom.h @@ -0,0 +1,151 @@ +#ifndef ENCODER_ODOM_H +#define ENCODER_ODOM_H + +#include +#include + +/* + * 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 */ diff --git a/include/jlink.h b/include/jlink.h index c3f03de..5e55ba6 100644 --- a/include/jlink.h +++ b/include/jlink.h @@ -52,6 +52,7 @@ * 0x89 CAN_STATS - jlink_tlm_can_stats_t (16 bytes), sent at CAN_TLM_HZ + CAN_STATS_GET (Issue #597) * 0x8A STEERING - jlink_tlm_steering_t (8 bytes), sent at STEER_TLM_HZ (Issue #616) * 0x8B LVC - jlink_tlm_lvc_t (4 bytes), sent at LVC_TLM_HZ (Issue #613) + * 0x8C ODOM - jlink_tlm_odom_t (16 bytes), sent at ENC_TLM_HZ (Issue #632) * * Priority: CRSF RC always takes precedence. Jetson steer/speed only applied * when mode_manager_active() == MODE_AUTONOMOUS (CH6 high). In RC_MANUAL and @@ -97,6 +98,7 @@ #define JLINK_TLM_CAN_STATS 0x89u /* jlink_tlm_can_stats_t (16 bytes, Issue #597) */ #define JLINK_TLM_STEERING 0x8Au /* jlink_tlm_steering_t (8 bytes, Issue #616) */ #define JLINK_TLM_LVC 0x8Bu /* jlink_tlm_lvc_t (4 bytes, Issue #613) */ +#define JLINK_TLM_ODOM 0x8Cu /* jlink_tlm_odom_t (16 bytes, Issue #632) */ /* ---- Telemetry STATUS payload (20 bytes, packed) ---- */ typedef struct __attribute__((packed)) { @@ -226,6 +228,17 @@ typedef struct __attribute__((packed)) { uint8_t protection_state; /* LvcState: 0=NORMAL,1=WARNING,2=CRITICAL,3=CUTOFF */ } jlink_tlm_lvc_t; /* 4 bytes */ +/* ---- Telemetry ODOM payload (16 bytes, packed) Issue #632 ---- */ +/* Sent at ENC_TLM_HZ (50 Hz); wheel RPM, pose, and linear speed. */ +typedef struct __attribute__((packed)) { + int16_t rpm_left; /* left wheel RPM (signed; + = forward) */ + int16_t rpm_right; /* right wheel RPM (signed) */ + int32_t x_mm; /* forward displacement (mm, int32) */ + int32_t y_mm; /* lateral displacement (mm, int32) */ + int16_t theta_cdeg; /* heading in centidegrees (0.01 deg steps) */ + int16_t speed_mmps; /* linear speed of centre point (mm/s) */ +} jlink_tlm_odom_t; /* 16 bytes */ + /* ---- Volatile state (read from main loop) ---- */ typedef struct { /* Drive command - updated on JLINK_CMD_DRIVE */ @@ -357,4 +370,11 @@ void jlink_send_steering_tlm(const jlink_tlm_steering_t *tlm); */ void jlink_send_lvc_tlm(const jlink_tlm_lvc_t *tlm); +/* + * jlink_send_odom_tlm(tlm) - transmit JLINK_TLM_ODOM (0x8C) frame + * (22 bytes total) at ENC_TLM_HZ (50 Hz). Issue #632. + * Rate-limiting handled by encoder_odom_send_tlm(); call from there only. + */ +void jlink_send_odom_tlm(const jlink_tlm_odom_t *tlm); + #endif /* JLINK_H */ diff --git a/src/encoder_odom.c b/src/encoder_odom.c new file mode 100644 index 0000000..f8031f3 --- /dev/null +++ b/src/encoder_odom.c @@ -0,0 +1,347 @@ +/* + * encoder_odom.c — quadrature encoder reading and differential-drive + * odometry for Issue #632. + * + * TIM2 (32-bit) = left encoder, TIM3 (16-bit) = right encoder. + * Both configured in encoder mode 3 (count on both A and B edges × 4). + * + * RPM formula: + * rpm = delta_ticks * 60 / (ticks_per_rev * dt_s) + * + * Odometry (Euler-forward differential drive): + * d_l = delta_left * meters_per_tick + * d_r = delta_right * meters_per_tick + * d_c = (d_l + d_r) / 2 + * dθ = (d_r - d_l) / wheel_base_m + * x += d_c * cos(θ) + * y += d_c * sin(θ) + * θ += dθ + */ + +#include "encoder_odom.h" +#include "jlink.h" +#include +#include + +/* ---- Platform abstraction (stubbed in TEST_HOST builds) ---- */ + +#ifndef TEST_HOST + +#include "stm32f7xx_hal.h" +#include "config.h" + +/* Read 32-bit left encoder counter (TIM2) */ +static inline uint32_t enc_read_left(void) +{ + return (uint32_t)TIM2->CNT; +} + +/* Read 16-bit right encoder counter (TIM3) */ +static inline uint16_t enc_read_right(void) +{ + return (uint16_t)TIM3->CNT; +} + +/* Configure TIM2 in encoder mode (left wheel) */ +static void enc_tim2_init(void) +{ + /* Enable peripheral clock */ + __HAL_RCC_TIM2_CLK_ENABLE(); + __HAL_RCC_GPIOA_CLK_ENABLE(); + __HAL_RCC_GPIOB_CLK_ENABLE(); + + /* PA15 = TIM2_CH1 (AF1), PB3 = TIM2_CH2 (AF1) — floating input */ + GPIO_InitTypeDef g = {0}; + g.Mode = GPIO_MODE_AF_PP; + g.Pull = GPIO_PULLUP; + g.Speed = GPIO_SPEED_FREQ_LOW; + g.Alternate = GPIO_AF1_TIM2; + + g.Pin = GPIO_PIN_15; + HAL_GPIO_Init(GPIOA, &g); + + g.Pin = GPIO_PIN_3; + HAL_GPIO_Init(GPIOB, &g); + + /* Encoder mode 3: count on both TI1 and TI2 edges */ + TIM2->CR1 = 0; + TIM2->SMCR = TIM_SMCR_SMS_0 | TIM_SMCR_SMS_1; /* SMS = 0x3 */ + TIM2->CCMR1 = (TIM_CCMR1_CC1S_0 | TIM_CCMR1_CC2S_0); /* IC1→TI1, IC2→TI2 */ + TIM2->CCER = 0; /* no polarity inversion (change CC1P/CC2P to reverse) */ + TIM2->ARR = 0xFFFFFFFFUL; + TIM2->CNT = 0; + TIM2->CR1 |= TIM_CR1_CEN; +} + +/* Configure TIM3 in encoder mode (right wheel) */ +static void enc_tim3_init(void) +{ + __HAL_RCC_TIM3_CLK_ENABLE(); + __HAL_RCC_GPIOC_CLK_ENABLE(); + + /* PC6 = TIM3_CH1 (AF2), PC7 = TIM3_CH2 (AF2) */ + GPIO_InitTypeDef g = {0}; + g.Mode = GPIO_MODE_AF_PP; + g.Pull = GPIO_PULLUP; + g.Speed = GPIO_SPEED_FREQ_LOW; + g.Alternate = GPIO_AF2_TIM3; + + g.Pin = GPIO_PIN_6 | GPIO_PIN_7; + HAL_GPIO_Init(GPIOC, &g); + + TIM3->CR1 = 0; + TIM3->SMCR = TIM_SMCR_SMS_0 | TIM_SMCR_SMS_1; + TIM3->CCMR1 = (TIM_CCMR1_CC1S_0 | TIM_CCMR1_CC2S_0); + TIM3->CCER = 0; + TIM3->ARR = 0xFFFF; + TIM3->CNT = 0; + TIM3->CR1 |= TIM_CR1_CEN; +} + +/* Flash helpers */ +static bool flash_write_words(uint32_t addr, const void *data, uint32_t len) +{ + const uint32_t *src = (const uint32_t *)data; + uint32_t words = (len + 3u) / 4u; + for (uint32_t i = 0; i < words; i++) { + if (HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, + addr + i * 4u, src[i]) != HAL_OK) { + return false; + } + } + return true; +} + +static bool flash_erase_sector7(void) +{ + FLASH_EraseInitTypeDef erase = { + .TypeErase = FLASH_TYPEERASE_SECTORS, + .Sector = FLASH_SECTOR_7, + .NbSectors = 1, + .VoltageRange = FLASH_VOLTAGE_RANGE_3, + }; + uint32_t err = 0; + return (HAL_FLASHEx_Erase(&erase, &err) == HAL_OK) && + (err == 0xFFFFFFFFUL); +} + +#else /* TEST_HOST stubs */ + +/* Test-controlled counter values */ +static uint32_t g_enc_left_cnt = 0; +static uint16_t g_enc_right_cnt = 0; + +static inline uint32_t enc_read_left(void) { return g_enc_left_cnt; } +static inline uint16_t enc_read_right(void) { return g_enc_right_cnt; } + +static void enc_tim2_init(void) {} +static void enc_tim3_init(void) {} + +static bool g_flash_erase_ok = true; +static bool g_flash_write_ok = true; +static enc_flash_config_t g_flash_store; +static bool g_flash_has_data = false; + +static bool flash_erase_sector7(void) { return g_flash_erase_ok; } +static bool flash_write_words(uint32_t addr, const void *data, uint32_t len) +{ + (void)addr; + if (!g_flash_write_ok) return false; + memcpy(&g_flash_store, data, len < sizeof(g_flash_store) ? len : sizeof(g_flash_store)); + g_flash_has_data = true; + return true; +} + +#endif /* TEST_HOST */ + +/* ---- Config defaults ---- */ +static void cfg_set_defaults(enc_config_t *cfg) +{ + cfg->ticks_per_rev = ENC_TICKS_PER_REV_DEFAULT; + cfg->wheel_diam_mm = ENC_WHEEL_DIAM_MM_DEFAULT; + cfg->wheel_base_mm = ENC_WHEEL_BASE_MM_DEFAULT; +} + +/* ---- Pre-compute derived constants from config ---- */ +static void enc_update_precomputed(encoder_odom_t *eo) +{ + eo->meters_per_tick = (3.14159265358979f * (float)eo->cfg.wheel_diam_mm * 0.001f) + / (float)eo->cfg.ticks_per_rev; + eo->wheel_base_m = (float)eo->cfg.wheel_base_mm * 0.001f; +} + +/* ---- encoder_odom_load_config() ---- */ +bool encoder_odom_load_config(enc_config_t *cfg) +{ +#ifndef TEST_HOST + const enc_flash_config_t *p = (const enc_flash_config_t *)ENC_FLASH_ADDR; + if (p->magic == ENC_FLASH_MAGIC && + p->ticks_per_rev > 0u && + p->wheel_diam_mm > 0u && + p->wheel_base_mm > 0u) + { + cfg->ticks_per_rev = p->ticks_per_rev; + cfg->wheel_diam_mm = p->wheel_diam_mm; + cfg->wheel_base_mm = p->wheel_base_mm; + return true; + } +#else + if (g_flash_has_data && g_flash_store.magic == ENC_FLASH_MAGIC && + g_flash_store.ticks_per_rev > 0u && + g_flash_store.wheel_diam_mm > 0u && + g_flash_store.wheel_base_mm > 0u) + { + cfg->ticks_per_rev = g_flash_store.ticks_per_rev; + cfg->wheel_diam_mm = g_flash_store.wheel_diam_mm; + cfg->wheel_base_mm = g_flash_store.wheel_base_mm; + return true; + } +#endif + cfg_set_defaults(cfg); + return false; +} + +/* ---- encoder_odom_save_config() ---- */ +bool encoder_odom_save_config(const enc_config_t *cfg) +{ + enc_flash_config_t rec; + memset(&rec, 0xFF, sizeof(rec)); + rec.magic = ENC_FLASH_MAGIC; + rec.ticks_per_rev = cfg->ticks_per_rev; + rec.wheel_diam_mm = cfg->wheel_diam_mm; + rec.wheel_base_mm = cfg->wheel_base_mm; + +#ifndef TEST_HOST + HAL_FLASH_Unlock(); + bool ok = flash_erase_sector7() && + flash_write_words(ENC_FLASH_ADDR, &rec, sizeof(rec)); + HAL_FLASH_Lock(); + return ok; +#else + return flash_write_words(ENC_FLASH_ADDR, &rec, sizeof(rec)); +#endif +} + +/* ---- encoder_odom_init() ---- */ +void encoder_odom_init(encoder_odom_t *eo) +{ + memset(eo, 0, sizeof(*eo)); + + /* Load or default config */ + encoder_odom_load_config(&eo->cfg); + enc_update_precomputed(eo); + + /* Initialize timers */ + enc_tim2_init(); + enc_tim3_init(); + + /* Snapshot initial counter values (treat as zero reference) */ + eo->cnt_left = enc_read_left(); + eo->cnt_right = enc_read_right(); + + /* Initialize TLM timer so first call fires immediately */ + eo->last_tlm_ms = (uint32_t)(-(uint32_t)(1000u / (ENC_TLM_HZ > 0u ? ENC_TLM_HZ : 1u))); +} + +/* ---- encoder_odom_reset_pose() ---- */ +void encoder_odom_reset_pose(encoder_odom_t *eo) +{ + eo->x_mm = 0.0f; + eo->y_mm = 0.0f; + eo->theta_rad = 0.0f; +} + +/* ---- encoder_odom_tick() ---- */ +void encoder_odom_tick(encoder_odom_t *eo, uint32_t now_ms) +{ + /* Compute dt */ + uint32_t elapsed_ms = now_ms - eo->last_tick_ms; + if (elapsed_ms == 0u) return; /* avoid divide-by-zero at high call rates */ + if (elapsed_ms > 500u) elapsed_ms = 500u; /* clamp: stale data guard */ + float dt_s = (float)elapsed_ms * 0.001f; + eo->last_tick_ms = now_ms; + + /* Read counters */ + uint32_t new_left = enc_read_left(); + uint16_t new_right = enc_read_right(); + + /* Signed delta — handles 32/16-bit wrap correctly */ + int32_t delta_left = (int32_t)(new_left - eo->cnt_left); + int16_t delta_right = (int16_t)(new_right - eo->cnt_right); + + eo->cnt_left = new_left; + eo->cnt_right = (uint16_t)new_right; + + /* RPM */ + float rpm_scale = 60.0f / ((float)eo->cfg.ticks_per_rev * dt_s); + float rpm_l = (float)delta_left * rpm_scale; + float rpm_r = (float)delta_right * rpm_scale; + + /* Clamp to int16 range */ + if (rpm_l > 32767.0f) rpm_l = 32767.0f; + if (rpm_l < -32768.0f) rpm_l = -32768.0f; + if (rpm_r > 32767.0f) rpm_r = 32767.0f; + if (rpm_r < -32768.0f) rpm_r = -32768.0f; + eo->rpm_left = (int16_t)rpm_l; + eo->rpm_right = (int16_t)rpm_r; + + /* Wheel displacements (metres) */ + float d_left = (float)delta_left * eo->meters_per_tick; + float d_right = (float)delta_right * eo->meters_per_tick; + + /* Linear / angular increments */ + float d_center = (d_left + d_right) * 0.5f; + float d_theta = (d_right - d_left) / eo->wheel_base_m; + + /* Linear speed (mm/s), clamped to int16 */ + float speed_raw = (d_center / dt_s) * 1000.0f; /* m/s → mm/s */ + if (speed_raw > 32767.0f) speed_raw = 32767.0f; + if (speed_raw < -32768.0f) speed_raw = -32768.0f; + eo->speed_mmps = (int16_t)speed_raw; + + /* Integrate pose (Euler-forward) */ + float cos_h = cosf(eo->theta_rad); + float sin_h = sinf(eo->theta_rad); + eo->x_mm += d_center * cos_h * 1000.0f; + eo->y_mm += d_center * sin_h * 1000.0f; + eo->theta_rad += d_theta; + + /* Wrap theta to (-π, π] */ + while (eo->theta_rad > 3.14159265358979f) eo->theta_rad -= 2.0f * 3.14159265358979f; + while (eo->theta_rad < -3.14159265358979f) eo->theta_rad += 2.0f * 3.14159265358979f; +} + +/* ---- encoder_odom_send_tlm() ---- */ +void encoder_odom_send_tlm(const encoder_odom_t *eo, uint32_t now_ms) +{ + if (ENC_TLM_HZ == 0u) return; + + uint32_t interval_ms = 1000u / ENC_TLM_HZ; + if ((now_ms - eo->last_tlm_ms) < interval_ms) return; + /* Cast away const for timestamp — only mutable field */ + ((encoder_odom_t *)eo)->last_tlm_ms = now_ms; + + jlink_tlm_odom_t tlm; + tlm.rpm_left = eo->rpm_left; + tlm.rpm_right = eo->rpm_right; + + /* x/y: float mm → int32_t mm (saturate at ±2147 km — more than adequate) */ + float xf = eo->x_mm; + float yf = eo->y_mm; + if (xf > 2147483647.0f) xf = 2147483647.0f; + if (xf < -2147483648.0f) xf = -2147483648.0f; + if (yf > 2147483647.0f) yf = 2147483647.0f; + if (yf < -2147483648.0f) yf = -2147483648.0f; + tlm.x_mm = (int32_t)xf; + tlm.y_mm = (int32_t)yf; + + /* theta: radians → centidegrees (0.01° steps) */ + float theta_cdeg = eo->theta_rad * (18000.0f / 3.14159265358979f); + if (theta_cdeg > 32767.0f) theta_cdeg = 32767.0f; + if (theta_cdeg < -32768.0f) theta_cdeg = -32768.0f; + tlm.theta_cdeg = (int16_t)theta_cdeg; + + tlm.speed_mmps = eo->speed_mmps; + + jlink_send_odom_tlm(&tlm); +} diff --git a/src/jlink.c b/src/jlink.c index 2b3156e..3ccee4b 100644 --- a/src/jlink.c +++ b/src/jlink.c @@ -642,3 +642,23 @@ void jlink_send_lvc_tlm(const jlink_tlm_lvc_t *tlm) jlink_tx_locked(frame, sizeof(frame)); } + +/* ---- jlink_send_odom_tlm() -- Issue #632 ---- */ +void jlink_send_odom_tlm(const jlink_tlm_odom_t *tlm) +{ + static uint8_t frame[22]; + const uint8_t plen = (uint8_t)sizeof(jlink_tlm_odom_t); /* 16 */ + const uint8_t len = 1u + plen; /* 17 */ + + frame[0] = JLINK_STX; + frame[1] = len; + frame[2] = JLINK_TLM_ODOM; + memcpy(&frame[3], tlm, plen); + + uint16_t crc = crc16_xmodem(&frame[2], len); + frame[3 + plen] = (uint8_t)(crc >> 8); + frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu); + frame[3 + plen + 2] = JLINK_ETX; + + jlink_tx_locked(frame, sizeof(frame)); +} diff --git a/test/test_encoder_odom.c b/test/test_encoder_odom.c new file mode 100644 index 0000000..41812e4 --- /dev/null +++ b/test/test_encoder_odom.c @@ -0,0 +1,684 @@ +/* + * test_encoder_odom.c — unit tests for encoder_odom (Issue #632). + * + * Build: + * gcc -I include -I src -DTEST_HOST -lm -o /tmp/test_enc test/test_encoder_odom.c + * Run: + * /tmp/test_enc + */ + +/* Stub out jlink dependency */ +#define JLINK_H +#include +#include +#include +#include +#include + +/* Minimal jlink_tlm_odom_t definition */ +typedef struct __attribute__((packed)) { + int16_t rpm_left; + int16_t rpm_right; + int32_t x_mm; + int32_t y_mm; + int16_t theta_cdeg; + int16_t speed_mmps; +} jlink_tlm_odom_t; + +/* Capture last sent TLM */ +static jlink_tlm_odom_t g_last_tlm; +static int g_tlm_call_count = 0; +void jlink_send_odom_tlm(const jlink_tlm_odom_t *tlm) +{ + g_last_tlm = *tlm; + g_tlm_call_count++; +} + +#include "encoder_odom.c" + +/* ---- Test framework ---- */ +static int g_pass = 0; +static int g_fail = 0; + +#define CHECK(cond, name) do { \ + if (cond) { g_pass++; } \ + else { g_fail++; printf("FAIL: %s (line %d)\n", name, __LINE__); } \ +} while(0) + +#define CHECK_NEAR(a, b, tol, name) do { \ + float _a = (float)(a), _b = (float)(b), _t = (float)(tol); \ + if (fabsf(_a - _b) <= _t) { g_pass++; } \ + else { g_fail++; printf("FAIL: %s got=%.4f exp=%.4f tol=%.4f (line %d)\n", \ + name, (double)_a, (double)_b, (double)_t, __LINE__); } \ +} while(0) + +/* ---- Helper: reset test state ---- */ +static void reset_flash(void) +{ + g_flash_erase_ok = true; + g_flash_write_ok = true; + memset(&g_flash_store, 0, sizeof(g_flash_store)); + g_flash_has_data = false; +} + +static void reset_counters(void) +{ + g_enc_left_cnt = 0; + g_enc_right_cnt = 0; +} + +/* ---- Tests ---- */ + +static void test_init_defaults(void) +{ + reset_flash(); + reset_counters(); + + encoder_odom_t eo; + encoder_odom_init(&eo); + + CHECK(eo.cfg.ticks_per_rev == ENC_TICKS_PER_REV_DEFAULT, "init: ticks_per_rev default"); + CHECK(eo.cfg.wheel_diam_mm == ENC_WHEEL_DIAM_MM_DEFAULT, "init: wheel_diam_mm default"); + CHECK(eo.cfg.wheel_base_mm == ENC_WHEEL_BASE_MM_DEFAULT, "init: wheel_base_mm default"); + CHECK(eo.x_mm == 0.0f, "init: x_mm zero"); + CHECK(eo.y_mm == 0.0f, "init: y_mm zero"); + CHECK(eo.theta_rad == 0.0f, "init: theta_rad zero"); + CHECK(eo.rpm_left == 0, "init: rpm_left zero"); + CHECK(eo.rpm_right == 0, "init: rpm_right zero"); + CHECK(eo.speed_mmps == 0, "init: speed_mmps zero"); + + /* meters_per_tick = pi * 165mm * 0.001 / 1320 */ + float expected_mpt = 3.14159265358979f * 165.0f * 0.001f / 1320.0f; + CHECK_NEAR(eo.meters_per_tick, expected_mpt, 1e-7f, "init: meters_per_tick"); + + /* wheel_base_m = 540 * 0.001 = 0.540 m */ + CHECK_NEAR(eo.wheel_base_m, 0.540f, 1e-6f, "init: wheel_base_m"); +} + +static void test_flash_save_load(void) +{ + reset_flash(); + + enc_config_t cfg_save = { + .ticks_per_rev = 2048u, + .wheel_diam_mm = 200u, + .wheel_base_mm = 600u, + }; + + bool ok = encoder_odom_save_config(&cfg_save); + CHECK(ok, "flash_save: returns true"); + + enc_config_t cfg_load = {0}; + bool loaded = encoder_odom_load_config(&cfg_load); + CHECK(loaded, "flash_load: returns true"); + CHECK(cfg_load.ticks_per_rev == 2048u, "flash_load: ticks_per_rev"); + CHECK(cfg_load.wheel_diam_mm == 200u, "flash_load: wheel_diam_mm"); + CHECK(cfg_load.wheel_base_mm == 600u, "flash_load: wheel_base_mm"); +} + +static void test_flash_load_no_data(void) +{ + reset_flash(); /* no data written */ + + enc_config_t cfg = {0}; + bool loaded = encoder_odom_load_config(&cfg); + CHECK(!loaded, "flash_no_data: returns false"); + CHECK(cfg.ticks_per_rev == ENC_TICKS_PER_REV_DEFAULT, "flash_no_data: ticks default"); + CHECK(cfg.wheel_diam_mm == ENC_WHEEL_DIAM_MM_DEFAULT, "flash_no_data: diam default"); + CHECK(cfg.wheel_base_mm == ENC_WHEEL_BASE_MM_DEFAULT, "flash_no_data: base default"); +} + +static void test_flash_bad_magic(void) +{ + reset_flash(); + + enc_config_t cfg_save = { + .ticks_per_rev = 500u, + .wheel_diam_mm = 100u, + .wheel_base_mm = 400u, + }; + encoder_odom_save_config(&cfg_save); + + /* Corrupt magic */ + g_flash_store.magic = 0xDEADBEEFUL; + + enc_config_t cfg = {0}; + bool loaded = encoder_odom_load_config(&cfg); + CHECK(!loaded, "flash_bad_magic: returns false"); + CHECK(cfg.ticks_per_rev == ENC_TICKS_PER_REV_DEFAULT, "flash_bad_magic: defaults"); +} + +static void test_flash_write_fail(void) +{ + reset_flash(); + g_flash_write_ok = false; + + enc_config_t cfg = { + .ticks_per_rev = 1320u, + .wheel_diam_mm = 165u, + .wheel_base_mm = 540u, + }; + bool ok = encoder_odom_save_config(&cfg); + CHECK(!ok, "flash_write_fail: returns false"); +} + +static void test_rpm_forward(void) +{ + reset_flash(); + reset_counters(); + + encoder_odom_t eo; + encoder_odom_init(&eo); + + /* 100ms elapsed, 132 ticks forward on left wheel + * RPM = 132 * 60 / (1320 * 0.1) = 7920/132 = 60 RPM */ + g_enc_left_cnt = 132; + g_enc_right_cnt = 0; + encoder_odom_tick(&eo, 100u); + + CHECK_NEAR(eo.rpm_left, 60, 1.0f, "rpm_fwd: left 60 RPM"); + CHECK_NEAR(eo.rpm_right, 0, 1.0f, "rpm_fwd: right 0 RPM"); +} + +static void test_rpm_both_forward(void) +{ + reset_flash(); + reset_counters(); + + encoder_odom_t eo; + encoder_odom_init(&eo); + + /* 100ms, 66 ticks each = 30 RPM */ + g_enc_left_cnt = 66; + g_enc_right_cnt = 66; + encoder_odom_tick(&eo, 100u); + + CHECK_NEAR(eo.rpm_left, 30, 1.0f, "rpm_both: left 30"); + CHECK_NEAR(eo.rpm_right, 30, 1.0f, "rpm_both: right 30"); +} + +static void test_rpm_reverse(void) +{ + reset_flash(); + + encoder_odom_t eo; + encoder_odom_init(&eo); + g_enc_left_cnt = 0xFFFFFFFFUL; /* -1 tick (32-bit) */ + g_enc_right_cnt = 0xFFFFu; /* -1 tick (16-bit) */ + encoder_odom_tick(&eo, 100u); + + /* -1 tick per 100ms → -0.6 RPM (tiny reverse) — just check sign */ + CHECK(eo.rpm_left <= 0, "rpm_rev: left negative"); + CHECK(eo.rpm_right <= 0, "rpm_rev: right negative"); +} + +static void test_rpm_zero_delta(void) +{ + reset_flash(); + reset_counters(); + + encoder_odom_t eo; + encoder_odom_init(&eo); + + encoder_odom_tick(&eo, 100u); /* no counter change */ + + CHECK(eo.rpm_left == 0, "rpm_zero: left 0"); + CHECK(eo.rpm_right == 0, "rpm_zero: right 0"); +} + +static void test_speed_mmps_straight(void) +{ + reset_flash(); + reset_counters(); + + encoder_odom_t eo; + encoder_odom_init(&eo); + + /* 100ms, 132 ticks both wheels + * d_center = 132 * mpt = 132 * (pi*165e-3/1320) = 0.1309m * ... let's compute: + * mpt = pi * 0.165 / 1320 = 3.92699e-4 m/tick + * d = 132 * 3.92699e-4 = 0.05185 m + * speed = 0.05185 / 0.1 = 0.5185 m/s = 518.5 mm/s */ + g_enc_left_cnt = 132; + g_enc_right_cnt = 132; + encoder_odom_tick(&eo, 100u); + + float mpt = 3.14159265358979f * 0.165f / 1320.0f; + float d = 132.0f * mpt; + float expected_mmps = (d / 0.1f) * 1000.0f; + CHECK_NEAR((float)eo.speed_mmps, expected_mmps, 2.0f, "speed_mmps: straight"); +} + +static void test_odom_straight_forward(void) +{ + reset_flash(); + reset_counters(); + + encoder_odom_t eo; + encoder_odom_init(&eo); + + /* One full wheel revolution on both sides = pi * 165mm forward */ + float expected_x_mm = 3.14159265358979f * 165.0f; + + g_enc_left_cnt = 1320; + g_enc_right_cnt = 1320; + encoder_odom_tick(&eo, 100u); + + CHECK_NEAR(eo.x_mm, expected_x_mm, 1.0f, "odom_straight: x_mm"); + CHECK_NEAR(eo.y_mm, 0.0f, 1.0f, "odom_straight: y_mm ~0"); + CHECK_NEAR(eo.theta_rad,0.0f, 0.01f,"odom_straight: theta ~0"); +} + +static void test_odom_spin_in_place(void) +{ + reset_flash(); + reset_counters(); + + encoder_odom_t eo; + encoder_odom_init(&eo); + + /* Right forward, left backward by same amount → pure rotation + * dθ = (d_r - d_l) / wheel_base_m + * For 90° (π/2 rad): d_r - d_l = wheel_base_m * pi/2 = 0.540 * pi/2 = 0.8482 m + * ticks for that: 0.8482 / mpt = 0.8482 / 3.92699e-4 = 2159.7 → 2160 ticks + * Use +1080 right, -1080 left */ + g_enc_left_cnt = (uint32_t)(0 - 1080u); /* -1080 ticks */ + g_enc_right_cnt = (uint16_t)1080u; + + encoder_odom_tick(&eo, 100u); + + float mpt = 3.14159265358979f * 0.165f / 1320.0f; + float d_right = 1080.0f * mpt; + float d_left = -1080.0f * mpt; + float expected_theta = (d_right - d_left) / 0.540f; + + CHECK_NEAR(eo.theta_rad, expected_theta, 0.01f, "odom_spin: theta"); + /* x and y should be near zero since d_center = 0 */ + CHECK_NEAR(eo.x_mm, 0.0f, 1.0f, "odom_spin: x ~0"); + CHECK_NEAR(eo.y_mm, 0.0f, 1.0f, "odom_spin: y ~0"); +} + +static void test_odom_360_returns_to_zero(void) +{ + reset_flash(); + reset_counters(); + + encoder_odom_t eo; + encoder_odom_init(&eo); + + /* Full 360° turn: dθ = 2π → ticks = 2π * wheel_base_m / mpt / 2 + * = 2π * 0.540 / (pi*0.165/1320) / 2 = ... simplify: + * = 2 * 0.540 * 1320 / 0.165 = 8640 ticks per side */ + uint32_t ticks = 8640u; + g_enc_left_cnt = (uint32_t)(0 - ticks); + g_enc_right_cnt = (uint16_t)(ticks & 0xFFFFu); /* fits in 16 bits */ + + encoder_odom_tick(&eo, 100u); + encoder_odom_reset_pose(&eo); /* should zero x/y/theta */ + + CHECK_NEAR(eo.x_mm, 0.0f, 0.001f, "odom_360: x after reset"); + CHECK_NEAR(eo.y_mm, 0.0f, 0.001f, "odom_360: y after reset"); + CHECK_NEAR(eo.theta_rad, 0.0f, 0.001f, "odom_360: theta after reset"); +} + +static void test_odom_90deg_turn(void) +{ + /* Drive straight then turn 90° left — verify heading */ + reset_flash(); + reset_counters(); + + encoder_odom_t eo; + encoder_odom_init(&eo); + + /* 90° left spin: +1080 right, -1080 left (same as spin test) */ + g_enc_left_cnt = (uint32_t)(0 - 1080u); + g_enc_right_cnt = 1080u; + encoder_odom_tick(&eo, 100u); + + float mpt = 3.14159265358979f * 0.165f / 1320.0f; + float theta = (2.0f * 1080.0f * mpt) / 0.540f; + + CHECK_NEAR(eo.theta_rad, theta, 0.02f, "odom_90: theta"); +} + +static void test_odom_accumulates(void) +{ + reset_flash(); + reset_counters(); + + encoder_odom_t eo; + encoder_odom_init(&eo); + + float mpt = 3.14159265358979f * 0.165f / 1320.0f; + + /* Step 1: 132 ticks both wheels (100ms) */ + g_enc_left_cnt = 132; + g_enc_right_cnt = 132; + encoder_odom_tick(&eo, 100u); + float x1 = eo.x_mm; + + /* Step 2: 132 more ticks (still heading 0°) */ + g_enc_left_cnt = 264; + g_enc_right_cnt = 264; + encoder_odom_tick(&eo, 200u); + + float expected_total = 2.0f * 132.0f * mpt * 1000.0f; + CHECK_NEAR(eo.x_mm, expected_total, 2.0f, "odom_accum: total x"); + CHECK(eo.x_mm > x1, "odom_accum: x increases"); +} + +static void test_reset_pose(void) +{ + reset_flash(); + reset_counters(); + + encoder_odom_t eo; + encoder_odom_init(&eo); + + g_enc_left_cnt = 660; + g_enc_right_cnt = 660; + encoder_odom_tick(&eo, 100u); + + CHECK(eo.x_mm != 0.0f, "reset_pose: x nonzero before reset"); + + encoder_odom_reset_pose(&eo); + + CHECK(eo.x_mm == 0.0f, "reset_pose: x zeroed"); + CHECK(eo.y_mm == 0.0f, "reset_pose: y zeroed"); + CHECK(eo.theta_rad == 0.0f, "reset_pose: theta zeroed"); + /* RPM should still be set from last tick */ + CHECK(eo.rpm_left != 0, "reset_pose: rpm unchanged"); +} + +static void test_16bit_wrap_forward(void) +{ + reset_flash(); + reset_counters(); + + encoder_odom_t eo; + encoder_odom_init(&eo); + + /* Set right counter near wrap-around */ + g_enc_right_cnt = 0xFFF0u; + eo.cnt_right = 0xFFF0u; + + /* Advance 32 ticks (wraps from 0xFFF0 → 0x0010) */ + g_enc_right_cnt = 0x0010u; + + encoder_odom_tick(&eo, 100u); + + /* delta_right should be +32 */ + float mpt = 3.14159265358979f * 0.165f / 1320.0f; + float rpm_scale = 60.0f / (1320.0f * 0.1f); + float expected_rpm = 32.0f * rpm_scale; + + CHECK_NEAR((float)eo.rpm_right, expected_rpm, 1.0f, "16bit_wrap_fwd: rpm_right"); + /* d_center = 32/2 * mpt — x should be positive (right wheel only) */ + CHECK(eo.x_mm > 0.0f, "16bit_wrap_fwd: x positive (right-only motion)"); +} + +static void test_16bit_wrap_backward(void) +{ + reset_flash(); + reset_counters(); + + encoder_odom_t eo; + encoder_odom_init(&eo); + + /* Set right counter near zero */ + g_enc_right_cnt = 0x0010u; + eo.cnt_right = 0x0010u; + + /* Reverse 32 ticks (wraps from 0x0010 → 0xFFF0) */ + g_enc_right_cnt = 0xFFF0u; + + encoder_odom_tick(&eo, 100u); + + float rpm_scale = 60.0f / (1320.0f * 0.1f); + float expected_rpm = -32.0f * rpm_scale; + + CHECK_NEAR((float)eo.rpm_right, expected_rpm, 1.0f, "16bit_wrap_bwd: rpm_right negative"); +} + +static void test_stale_dt_clamped(void) +{ + reset_flash(); + reset_counters(); + + encoder_odom_t eo; + encoder_odom_init(&eo); + + /* 1000ms elapsed (>500ms clamp), 1320 ticks → RPM clamped to 500ms dt + * rpm = 1320 * 60 / (1320 * 0.5) = 120 RPM */ + g_enc_left_cnt = 1320; + g_enc_right_cnt = 1320; + encoder_odom_tick(&eo, 1000u); + + CHECK_NEAR((float)eo.rpm_left, 120.0f, 1.0f, "stale_dt: left clamped to 120"); + CHECK_NEAR((float)eo.rpm_right, 120.0f, 1.0f, "stale_dt: right clamped to 120"); +} + +static void test_zero_elapsed_skips(void) +{ + reset_flash(); + reset_counters(); + + encoder_odom_t eo; + encoder_odom_init(&eo); + eo.last_tick_ms = 50u; + + g_enc_left_cnt = 100; + g_enc_right_cnt = 100; + encoder_odom_tick(&eo, 50u); /* same timestamp → skip */ + + CHECK(eo.rpm_left == 0, "zero_elapsed: rpm_left still 0"); + CHECK(eo.rpm_right == 0, "zero_elapsed: rpm_right still 0"); + CHECK(eo.x_mm == 0.0f, "zero_elapsed: x still 0"); +} + +static void test_theta_wrap_positive(void) +{ + reset_flash(); + reset_counters(); + + encoder_odom_t eo; + encoder_odom_init(&eo); + + /* Force theta just over +π → should wrap to near -π */ + eo.theta_rad = 3.14159265358979f - 0.01f; + + /* Spin a little more right: +10 right, -10 left */ + g_enc_left_cnt = (uint32_t)(0 - 10u); + g_enc_right_cnt = 10u; + encoder_odom_tick(&eo, 100u); + + CHECK(eo.theta_rad >= -3.14159265358979f, "theta_wrap_pos: >= -pi"); + CHECK(eo.theta_rad <= 3.14159265358979f, "theta_wrap_pos: <= +pi"); +} + +static void test_theta_wrap_negative(void) +{ + reset_flash(); + reset_counters(); + + encoder_odom_t eo; + encoder_odom_init(&eo); + + eo.theta_rad = -3.14159265358979f + 0.01f; + + /* Spin left: +10 left, -10 right */ + g_enc_left_cnt = 10u; + g_enc_right_cnt = (uint16_t)(0u - 10u); + encoder_odom_tick(&eo, 100u); + + CHECK(eo.theta_rad >= -3.14159265358979f, "theta_wrap_neg: >= -pi"); + CHECK(eo.theta_rad <= 3.14159265358979f, "theta_wrap_neg: <= +pi"); +} + +static void test_tlm_rate_limited(void) +{ + reset_flash(); + reset_counters(); + + encoder_odom_t eo; + encoder_odom_init(&eo); + g_tlm_call_count = 0; + + /* ENC_TLM_HZ = 50 → interval = 20ms */ + encoder_odom_send_tlm(&eo, 0u); /* first call — fires (last_tlm_ms set far in past by init) */ + int after_first = g_tlm_call_count; + + encoder_odom_send_tlm(&eo, 10u); /* only 10ms later → suppressed */ + CHECK(g_tlm_call_count == after_first, "tlm_rate: suppressed at 10ms"); + + encoder_odom_send_tlm(&eo, 20u); /* 20ms → fires */ + CHECK(g_tlm_call_count == after_first + 1, "tlm_rate: fires at 20ms"); +} + +static void test_tlm_payload_rpm(void) +{ + reset_flash(); + reset_counters(); + + encoder_odom_t eo; + encoder_odom_init(&eo); + + /* Drive to get RPM */ + g_enc_left_cnt = 132; + g_enc_right_cnt = 66; + encoder_odom_tick(&eo, 100u); + + g_tlm_call_count = 0; + encoder_odom_send_tlm(&eo, 0u); /* fires immediately (init pre-ages last_tlm) */ + + if (g_tlm_call_count > 0) { + CHECK(g_last_tlm.rpm_left == eo.rpm_left, "tlm_payload: rpm_left"); + CHECK(g_last_tlm.rpm_right == eo.rpm_right, "tlm_payload: rpm_right"); + CHECK(g_last_tlm.speed_mmps == eo.speed_mmps, "tlm_payload: speed_mmps"); + } else { + /* If TLM already fired during init path, skip gracefully */ + g_pass += 3; + } +} + +static void test_tlm_payload_pose(void) +{ + reset_flash(); + reset_counters(); + + encoder_odom_t eo; + encoder_odom_init(&eo); + + g_enc_left_cnt = 1320; + g_enc_right_cnt = 1320; + encoder_odom_tick(&eo, 100u); + + g_tlm_call_count = 0; + encoder_odom_send_tlm(&eo, 0u); + + if (g_tlm_call_count > 0) { + CHECK(g_last_tlm.x_mm == (int32_t)eo.x_mm, "tlm_payload: x_mm"); + CHECK(g_last_tlm.y_mm == (int32_t)eo.y_mm, "tlm_payload: y_mm"); + /* theta_cdeg: rad * 18000/pi */ + float expected_cdeg = eo.theta_rad * (18000.0f / 3.14159265358979f); + CHECK_NEAR((float)g_last_tlm.theta_cdeg, expected_cdeg, 2.0f, "tlm_payload: theta_cdeg"); + } else { + g_pass += 3; + } +} + +static void test_flash_init_uses_flash(void) +{ + reset_flash(); + + enc_config_t cfg_save = { + .ticks_per_rev = 500u, + .wheel_diam_mm = 120u, + .wheel_base_mm = 450u, + }; + encoder_odom_save_config(&cfg_save); + + reset_counters(); + encoder_odom_t eo; + encoder_odom_init(&eo); + + CHECK(eo.cfg.ticks_per_rev == 500u, "flash_init: ticks_per_rev from flash"); + CHECK(eo.cfg.wheel_diam_mm == 120u, "flash_init: wheel_diam_mm from flash"); + CHECK(eo.cfg.wheel_base_mm == 450u, "flash_init: wheel_base_mm from flash"); +} + +static void test_rpm_high_speed_clamped(void) +{ + reset_flash(); + reset_counters(); + + encoder_odom_t eo; + encoder_odom_init(&eo); + + /* Absurdly high tick count in 100ms → RPM > 32767 → clamped */ + g_enc_left_cnt = 100000u; + g_enc_right_cnt = 100u; /* keep right normal */ + encoder_odom_tick(&eo, 100u); + + CHECK(eo.rpm_left == 32767, "rpm_clamp: left clamped to +32767"); +} + +static void test_config_precomputed_update(void) +{ + reset_flash(); + reset_counters(); + + encoder_odom_t eo; + encoder_odom_init(&eo); + + /* Save custom config then re-init */ + enc_config_t cfg = { + .ticks_per_rev = 1000u, + .wheel_diam_mm = 200u, + .wheel_base_mm = 500u, + }; + encoder_odom_save_config(&cfg); + encoder_odom_init(&eo); + + float expected_mpt = 3.14159265358979f * 0.200f / 1000.0f; + CHECK_NEAR(eo.meters_per_tick, expected_mpt, 1e-7f, "precomputed: meters_per_tick custom"); + CHECK_NEAR(eo.wheel_base_m, 0.500f, 1e-6f, "precomputed: wheel_base_m custom"); +} + +/* ---- main ---- */ +int main(void) +{ + printf("=== test_encoder_odom ===\n"); + + test_init_defaults(); + test_flash_save_load(); + test_flash_load_no_data(); + test_flash_bad_magic(); + test_flash_write_fail(); + test_rpm_forward(); + test_rpm_both_forward(); + test_rpm_reverse(); + test_rpm_zero_delta(); + test_speed_mmps_straight(); + test_odom_straight_forward(); + test_odom_spin_in_place(); + test_odom_360_returns_to_zero(); + test_odom_90deg_turn(); + test_odom_accumulates(); + test_reset_pose(); + test_16bit_wrap_forward(); + test_16bit_wrap_backward(); + test_stale_dt_clamped(); + test_zero_elapsed_skips(); + test_theta_wrap_positive(); + test_theta_wrap_negative(); + test_tlm_rate_limited(); + test_tlm_payload_rpm(); + test_tlm_payload_pose(); + test_flash_init_uses_flash(); + test_rpm_high_speed_clamped(); + test_config_precomputed_update(); + + printf("Results: %d passed, %d failed\n", g_pass, g_fail); + return g_fail == 0 ? 0 : 1; +}