seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests 3 Actions Packages Projects Releases Wiki Activity

feat: Encoder odometry and wheel speed feedback (Issue #632)

Browse Source 
- TIM2 (32-bit) left encoder, TIM3 (16-bit) right encoder in mode 3
- RPM calculation with int16 clamp; 16-bit wrap handled via signed delta
- Differential-drive odometry: x/y/theta Euler-forward integration
- Flash config (sector 7, 0x0807FF00) for ticks_per_rev/wheel_diam/base
- JLINK_TLM_ODOM (0x8C) at 50 Hz: rpm_l/r, x_mm, y_mm, theta_cdeg, speed_mmps
- 75/75 unit tests passing (TEST_HOST build)

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
sl-controls 2026-03-15 14:50:14 -04:00
parent 4c7fa938a5
commit 779f9d00e2
6 changed files with 1239 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
include/config.h
View File

@ -278,4 +278,21 @@
#define UART_PROT_BAUD 115200u // baud rate for UART5 Jetson protocol #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 #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 #endif // CONFIG_H

151
include/encoder_odom.h Normal file
View File

@ -0,0 +1,151 @@
#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 */

20
include/jlink.h
View File

@ -52,6 +52,7 @@
* 0x89 CAN_STATS - jlink_tlm_can_stats_t (16 bytes), sent at CAN_TLM_HZ + CAN_STATS_GET (Issue #597) * 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) * 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) * 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 * Priority: CRSF RC always takes precedence. Jetson steer/speed only applied
* when mode_manager_active() == MODE_AUTONOMOUS (CH6 high). In RC_MANUAL and * 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_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_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_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) ---- */ /* ---- Telemetry STATUS payload (20 bytes, packed) ---- */
typedef struct __attribute__((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 */ uint8_t protection_state; /* LvcState: 0=NORMAL,1=WARNING,2=CRITICAL,3=CUTOFF */
} jlink_tlm_lvc_t; /* 4 bytes */ } 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) ---- */ /* ---- Volatile state (read from main loop) ---- */
typedef struct { typedef struct {
/* Drive command - updated on JLINK_CMD_DRIVE */ /* 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); 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 */ #endif /* JLINK_H */

347
src/encoder_odom.c Normal file
View File

@ -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 <math.h>
#include <string.h>
/* ---- 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);
}

20
src/jlink.c
View File

@ -642,3 +642,23 @@ void jlink_send_lvc_tlm(const jlink_tlm_lvc_t *tlm)
jlink_tx_locked(frame, sizeof(frame)); 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));
}

684
test/test_encoder_odom.c Normal file
View File

@ -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 <stdint.h>
#include <stdbool.h>
#include <string.h>
#include <stdio.h>
#include <math.h>
/* 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;
}