saltylab-firmware/test/test_vesc_can.c

/*
 * test_vesc_can.c — Unit tests for VESC CAN protocol driver (Issue #674).
 *
 * Build (host, no hardware):
 *   gcc -I include -I test/stubs -DTEST_HOST -lm \
 *       -o /tmp/test_vesc_can test/test_vesc_can.c
 *
 * All tests are self-contained; no HAL, no CAN peripheral required.
 * vesc_can.c calls can_driver_send_ext / can_driver_set_ext_cb and
 * jlink_send_vesc_state_tlm — all stubbed below.
 */

/* ---- Block HAL and board-specific headers ---- */
/* Must appear before any board include is transitively pulled */
#define STM32F7XX_HAL_H        /* skip stm32f7xx_hal.h */
#define STM32F722xx            /* satisfy any chip guard */
#define JLINK_H                /* skip jlink.h (pid_flash / HAL deps) */
#define CAN_DRIVER_H           /* skip can_driver.h body (we stub functions below) */

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

/* Minimal HAL types needed by vesc_can.c (none for this module, but keep HAL_OK) */
#define HAL_OK 0

/* ---- Minimal type replicas (must match the real packed structs) ---- */

typedef struct __attribute__((packed)) {
    int32_t  left_rpm;
    int32_t  right_rpm;
    int16_t  left_current_x10;
    int16_t  right_current_x10;
    int16_t  left_temp_x10;
    int16_t  right_temp_x10;
    int16_t  voltage_x10;
    uint8_t  left_fault;
    uint8_t  right_fault;
    uint8_t  left_alive;
    uint8_t  right_alive;
} jlink_tlm_vesc_state_t;  /* 22 bytes */

/* ---- Stubs ---- */

/* Simulated tick counter */
static uint32_t g_tick_ms = 0;
uint32_t HAL_GetTick(void) { return g_tick_ms; }

/* Capture last extended CAN TX */
static uint32_t g_last_ext_id  = 0;
static uint8_t  g_last_ext_data[8];
static uint8_t  g_last_ext_len  = 0;
static int      g_ext_tx_count  = 0;

void can_driver_send_ext(uint32_t ext_id, const uint8_t *data, uint8_t len)
{
    g_last_ext_id  = ext_id;
    if (len > 8u) len = 8u;
    for (uint8_t i = 0; i < len; i++) g_last_ext_data[i] = data[i];
    g_last_ext_len = len;
    g_ext_tx_count++;
}

/* Replicate types from can_driver.h (header is blocked by #define CAN_DRIVER_H) */
typedef void (*can_ext_frame_cb_t)(uint32_t ext_id, const uint8_t *data, uint8_t len);
typedef void (*can_std_frame_cb_t)(uint16_t std_id, const uint8_t *data, uint8_t len);

/* Capture registered ext callback */
static can_ext_frame_cb_t g_registered_cb = NULL;
void can_driver_set_ext_cb(can_ext_frame_cb_t cb) { g_registered_cb = cb; }

/* Capture last TLM sent to JLink */
static jlink_tlm_vesc_state_t g_last_tlm;
static int g_tlm_count = 0;
void jlink_send_vesc_state_tlm(const jlink_tlm_vesc_state_t *tlm)
{
    g_last_tlm = *tlm;
    g_tlm_count++;
}

/* ---- Include implementation directly ---- */
#include "../src/vesc_can.c"

/* ---- Test framework ---- */
#include <stdio.h>
#include <string.h>
#include <math.h>

static int g_pass = 0;
static int g_fail = 0;

#define ASSERT(cond, msg) do { \
    if (cond) { g_pass++; } \
    else { g_fail++; printf("FAIL [%s:%d] %s\n", __FILE__, __LINE__, msg); } \
} while(0)

/* ---- Helpers ---- */

static void reset_stubs(void)
{
    g_tick_ms      = 0;
    g_last_ext_id  = 0;
    g_last_ext_len = 0;
    g_ext_tx_count = 0;
    g_tlm_count    = 0;
    g_registered_cb = NULL;
    memset(g_last_ext_data, 0, sizeof(g_last_ext_data));
    memset(&g_last_tlm, 0, sizeof(g_last_tlm));
}

/* Build a STATUS frame for vesc_id with given RPM, current_x10, duty_x1000 */
static void make_status(uint8_t buf[8], int32_t rpm, int16_t cur_x10, int16_t duty)
{
    uint32_t urpm = (uint32_t)rpm;
    buf[0] = (uint8_t)(urpm >> 24u);
    buf[1] = (uint8_t)(urpm >> 16u);
    buf[2] = (uint8_t)(urpm >>  8u);
    buf[3] = (uint8_t)(urpm);
    buf[4] = (uint8_t)((uint16_t)cur_x10 >> 8u);
    buf[5] = (uint8_t)((uint16_t)cur_x10 & 0xFFu);
    buf[6] = (uint8_t)((uint16_t)duty >> 8u);
    buf[7] = (uint8_t)((uint16_t)duty & 0xFFu);
}

/* ---- Tests ---- */

static void test_init_stores_ids(void)
{
    reset_stubs();
    vesc_can_init(56u, 68u);
    ASSERT(s_id_left  == 56u, "init stores left ID");
    ASSERT(s_id_right == 68u, "init stores right ID");
}

static void test_init_zeroes_state(void)
{
    reset_stubs();
    /* Dirty the state first */
    s_state[0].rpm = 9999;
    s_state[1].rpm = -9999;
    vesc_can_init(56u, 68u);
    ASSERT(s_state[0].rpm == 0, "init zeroes left RPM");
    ASSERT(s_state[1].rpm == 0, "init zeroes right RPM");
    ASSERT(s_state[0].last_rx_ms == 0u, "init zeroes left last_rx_ms");
}

static void test_init_registers_ext_callback(void)
{
    reset_stubs();
    vesc_can_init(56u, 68u);
    ASSERT(g_registered_cb == vesc_can_on_frame, "init registers vesc_can_on_frame as ext_cb");
}

static void test_send_rpm_ext_id_left(void)
{
    reset_stubs();
    vesc_can_init(56u, 68u);
    g_ext_tx_count = 0;
    vesc_can_send_rpm(56u, 1000);
    /* ext_id = (VESC_PKT_SET_RPM << 8) | vesc_id = (3 << 8) | 56 = 0x0338 */
    ASSERT(g_last_ext_id == 0x0338u, "send_rpm left: correct ext_id");
    ASSERT(g_ext_tx_count == 1, "send_rpm: one TX frame");
    ASSERT(g_last_ext_len == 4u, "send_rpm: DLC=4");
}

static void test_send_rpm_ext_id_right(void)
{
    reset_stubs();
    vesc_can_init(56u, 68u);
    vesc_can_send_rpm(68u, 2000);
    /* ext_id = (3 << 8) | 68 = 0x0344 */
    ASSERT(g_last_ext_id == 0x0344u, "send_rpm right: correct ext_id");
}

static void test_send_rpm_payload_positive(void)
{
    reset_stubs();
    vesc_can_init(56u, 68u);
    vesc_can_send_rpm(56u, 0x01020304);
    ASSERT(g_last_ext_data[0] == 0x01u, "send_rpm payload byte0");
    ASSERT(g_last_ext_data[1] == 0x02u, "send_rpm payload byte1");
    ASSERT(g_last_ext_data[2] == 0x03u, "send_rpm payload byte2");
    ASSERT(g_last_ext_data[3] == 0x04u, "send_rpm payload byte3");
}

static void test_send_rpm_payload_negative(void)
{
    reset_stubs();
    vesc_can_init(56u, 68u);
    /* -1 as int32 = 0xFFFFFFFF */
    vesc_can_send_rpm(56u, -1);
    ASSERT(g_last_ext_data[0] == 0xFFu, "send_rpm -1 byte0");
    ASSERT(g_last_ext_data[1] == 0xFFu, "send_rpm -1 byte1");
    ASSERT(g_last_ext_data[2] == 0xFFu, "send_rpm -1 byte2");
    ASSERT(g_last_ext_data[3] == 0xFFu, "send_rpm -1 byte3");
}

static void test_send_rpm_zero(void)
{
    reset_stubs();
    vesc_can_init(56u, 68u);
    vesc_can_send_rpm(56u, 0);
    ASSERT(g_last_ext_data[0] == 0u, "send_rpm 0 byte0");
    ASSERT(g_last_ext_data[3] == 0u, "send_rpm 0 byte3");
    ASSERT(g_ext_tx_count == 1, "send_rpm 0: one TX");
}

static void test_on_frame_status_rpm(void)
{
    reset_stubs();
    vesc_can_init(56u, 68u);
    uint8_t buf[8];
    make_status(buf, 12345, 150, 500);
    uint32_t ext_id = ((uint32_t)VESC_PKT_STATUS << 8u) | 56u;
    vesc_can_on_frame(ext_id, buf, 8u);
    ASSERT(s_state[0].rpm == 12345, "on_frame STATUS: RPM parsed");
}

static void test_on_frame_status_current(void)
{
    reset_stubs();
    vesc_can_init(56u, 68u);
    uint8_t buf[8];
    make_status(buf, 0, 250, 0);
    uint32_t ext_id = ((uint32_t)VESC_PKT_STATUS << 8u) | 56u;
    vesc_can_on_frame(ext_id, buf, 8u);
    ASSERT(s_state[0].current_x10 == 250, "on_frame STATUS: current_x10 parsed");
}

static void test_on_frame_status_duty(void)
{
    reset_stubs();
    vesc_can_init(56u, 68u);
    uint8_t buf[8];
    make_status(buf, 0, 0, -300);
    uint32_t ext_id = ((uint32_t)VESC_PKT_STATUS << 8u) | 56u;
    vesc_can_on_frame(ext_id, buf, 8u);
    ASSERT(s_state[0].duty_x1000 == -300, "on_frame STATUS: duty_x1000 parsed");
}

static void test_on_frame_status_updates_timestamp(void)
{
    reset_stubs();
    vesc_can_init(56u, 68u);
    g_tick_ms = 5000u;
    uint8_t buf[8];
    make_status(buf, 100, 0, 0);
    uint32_t ext_id = ((uint32_t)VESC_PKT_STATUS << 8u) | 56u;
    vesc_can_on_frame(ext_id, buf, 8u);
    ASSERT(s_state[0].last_rx_ms == 5000u, "on_frame STATUS: last_rx_ms updated");
}

static void test_on_frame_status_right_node(void)
{
    reset_stubs();
    vesc_can_init(56u, 68u);
    uint8_t buf[8];
    make_status(buf, -9999, 0, 0);
    uint32_t ext_id = ((uint32_t)VESC_PKT_STATUS << 8u) | 68u;
    vesc_can_on_frame(ext_id, buf, 8u);
    ASSERT(s_state[1].rpm == -9999, "on_frame STATUS: right node RPM");
    ASSERT(s_state[0].rpm == 0,     "on_frame STATUS: left unaffected");
}

static void test_on_frame_status4_temps(void)
{
    reset_stubs();
    vesc_can_init(56u, 68u);
    uint8_t buf[8] = {0x00, 0xF0, 0x01, 0x2C, 0x00, 0x64, 0, 0};
    /* T_fet = 0x00F0 = 240 (24.0°C), T_mot = 0x012C = 300 (30.0°C), I_in = 0x0064 = 100 */
    uint32_t ext_id = ((uint32_t)VESC_PKT_STATUS_4 << 8u) | 56u;
    vesc_can_on_frame(ext_id, buf, 6u);
    ASSERT(s_state[0].temp_fet_x10   == 240,  "on_frame STATUS_4: temp_fet_x10");
    ASSERT(s_state[0].temp_motor_x10 == 300,  "on_frame STATUS_4: temp_motor_x10");
    ASSERT(s_state[0].current_in_x10 == 100,  "on_frame STATUS_4: current_in_x10");
}

static void test_on_frame_status5_voltage(void)
{
    reset_stubs();
    vesc_can_init(56u, 68u);
    /* tacho at [0..3], V_in×10 at [4..5] = 0x0100 = 256 (25.6 V) */
    uint8_t buf[8] = {0, 0, 0, 0, 0x01, 0x00, 0, 0};
    uint32_t ext_id = ((uint32_t)VESC_PKT_STATUS_5 << 8u) | 56u;
    vesc_can_on_frame(ext_id, buf, 6u);
    ASSERT(s_state[0].voltage_x10 == 256, "on_frame STATUS_5: voltage_x10");
}

static void test_on_frame_unknown_pkt_type_ignored(void)
{
    reset_stubs();
    vesc_can_init(56u, 68u);
    uint8_t buf[8] = {0};
    uint32_t ext_id = (99u << 8u) | 56u;  /* unknown pkt type 99 */
    vesc_can_on_frame(ext_id, buf, 8u);
    /* No crash, state unmodified (last_rx_ms stays 0) */
    ASSERT(s_state[0].last_rx_ms == 0u, "on_frame: unknown pkt_type ignored");
}

static void test_on_frame_unknown_vesc_id_ignored(void)
{
    reset_stubs();
    vesc_can_init(56u, 68u);
    uint8_t buf[8];
    make_status(buf, 9999, 0, 0);
    uint32_t ext_id = ((uint32_t)VESC_PKT_STATUS << 8u) | 99u;  /* unknown ID */
    vesc_can_on_frame(ext_id, buf, 8u);
    ASSERT(s_state[0].rpm == 0 && s_state[1].rpm == 0, "on_frame: unknown vesc_id ignored");
}

static void test_on_frame_short_status_ignored(void)
{
    reset_stubs();
    vesc_can_init(56u, 68u);
    uint8_t buf[8];
    make_status(buf, 1234, 0, 0);
    uint32_t ext_id = ((uint32_t)VESC_PKT_STATUS << 8u) | 56u;
    vesc_can_on_frame(ext_id, buf, 7u);  /* too short: need 8 */
    ASSERT(s_state[0].rpm == 0, "on_frame STATUS: short frame ignored");
}

static void test_get_state_unknown_id_returns_false(void)
{
    reset_stubs();
    vesc_can_init(56u, 68u);
    vesc_state_t out;
    bool ok = vesc_can_get_state(99u, &out);
    ASSERT(!ok, "get_state: unknown id returns false");
}

static void test_get_state_no_frame_returns_false(void)
{
    reset_stubs();
    vesc_can_init(56u, 68u);
    vesc_state_t out;
    bool ok = vesc_can_get_state(56u, &out);
    ASSERT(!ok, "get_state: no frame yet returns false");
}

static void test_get_state_after_status_returns_true(void)
{
    reset_stubs();
    vesc_can_init(56u, 68u);
    g_tick_ms = 1000u;
    uint8_t buf[8];
    make_status(buf, 4321, 88, -100);
    uint32_t ext_id = ((uint32_t)VESC_PKT_STATUS << 8u) | 56u;
    vesc_can_on_frame(ext_id, buf, 8u);

    vesc_state_t out;
    bool ok = vesc_can_get_state(56u, &out);
    ASSERT(ok,              "get_state: returns true after STATUS");
    ASSERT(out.rpm == 4321, "get_state: RPM correct");
    ASSERT(out.current_x10 == 88, "get_state: current_x10 correct");
}

static void test_is_alive_no_frame(void)
{
    reset_stubs();
    vesc_can_init(56u, 68u);
    ASSERT(!vesc_can_is_alive(56u, 0u), "is_alive: false with no frame");
}

static void test_is_alive_within_timeout(void)
{
    reset_stubs();
    vesc_can_init(56u, 68u);
    g_tick_ms = 5000u;
    uint8_t buf[8];
    make_status(buf, 100, 0, 0);
    vesc_can_on_frame(((uint32_t)VESC_PKT_STATUS << 8u) | 56u, buf, 8u);

    /* Check alive 500 ms later (within 1000 ms timeout) */
    ASSERT(vesc_can_is_alive(56u, 5500u), "is_alive: true within timeout");
}

static void test_is_alive_after_timeout(void)
{
    reset_stubs();
    vesc_can_init(56u, 68u);
    g_tick_ms = 1000u;
    uint8_t buf[8];
    make_status(buf, 100, 0, 0);
    vesc_can_on_frame(((uint32_t)VESC_PKT_STATUS << 8u) | 56u, buf, 8u);

    /* Check alive 1001 ms later — exceeds VESC_ALIVE_TIMEOUT_MS (1000 ms) */
    ASSERT(!vesc_can_is_alive(56u, 2001u), "is_alive: false after timeout");
}

static void test_is_alive_at_exact_timeout_boundary(void)
{
    reset_stubs();
    vesc_can_init(56u, 68u);
    g_tick_ms = 1000u;
    uint8_t buf[8];
    make_status(buf, 100, 0, 0);
    vesc_can_on_frame(((uint32_t)VESC_PKT_STATUS << 8u) | 56u, buf, 8u);

    /* At exactly VESC_ALIVE_TIMEOUT_MS: delta = 1000, condition is < 1000 → false */
    ASSERT(!vesc_can_is_alive(56u, 2000u), "is_alive: false at exact timeout boundary");
}

static void test_send_tlm_rate_limited(void)
{
    reset_stubs();
    vesc_can_init(56u, 68u);
    g_tlm_count = 0;

    /* First call at t=0 should fire immediately (pre-wound s_tlm_tick) */
    vesc_can_send_tlm(0u);
    ASSERT(g_tlm_count == 1, "send_tlm: fires on first call");

    /* Second call immediately after: should NOT fire (within 1s window) */
    vesc_can_send_tlm(500u);
    ASSERT(g_tlm_count == 1, "send_tlm: rate-limited within 1 s");

    /* After 1000 ms: should fire again */
    vesc_can_send_tlm(1000u);
    ASSERT(g_tlm_count == 2, "send_tlm: fires after 1 s");
}

static void test_send_tlm_payload_content(void)
{
    reset_stubs();
    vesc_can_init(56u, 68u);
    g_tick_ms = 100u;

    /* Inject STATUS into left VESC */
    uint8_t buf[8];
    make_status(buf, 5678, 123, 400);
    vesc_can_on_frame(((uint32_t)VESC_PKT_STATUS << 8u) | 56u, buf, 8u);

    /* Inject STATUS into right VESC */
    make_status(buf, -1234, -50, -200);
    vesc_can_on_frame(((uint32_t)VESC_PKT_STATUS << 8u) | 68u, buf, 8u);

    /* Inject STATUS_4 into left (for temps) */
    uint8_t buf4[8] = {0x00, 0xC8, 0x01, 0x2C, 0x00, 0x64, 0, 0};
    /* T_fet=200, T_mot=300, I_in=100 */
    vesc_can_on_frame(((uint32_t)VESC_PKT_STATUS_4 << 8u) | 56u, buf4, 6u);

    /* Inject STATUS_5 into left (for voltage) */
    uint8_t buf5[8] = {0, 0, 0, 0, 0x01, 0x00, 0, 0};
    /* V_in×10 = 256 (25.6 V) */
    vesc_can_on_frame(((uint32_t)VESC_PKT_STATUS_5 << 8u) | 56u, buf5, 6u);

    vesc_can_send_tlm(0u);

    ASSERT(g_tlm_count == 1,                "send_tlm: TLM sent");
    ASSERT(g_last_tlm.left_rpm    ==  5678, "send_tlm: left_rpm");
    ASSERT(g_last_tlm.right_rpm   == -1234, "send_tlm: right_rpm");
    ASSERT(g_last_tlm.left_current_x10  == 123,  "send_tlm: left_current_x10");
    ASSERT(g_last_tlm.right_current_x10 == -50,  "send_tlm: right_current_x10");
    ASSERT(g_last_tlm.left_temp_x10  == 200, "send_tlm: left_temp_x10");
    ASSERT(g_last_tlm.right_temp_x10 == 0,   "send_tlm: right_temp_x10 (no STATUS_4)");
    ASSERT(g_last_tlm.voltage_x10 == 256,    "send_tlm: voltage_x10");
}

static void test_send_tlm_alive_flags(void)
{
    reset_stubs();
    vesc_can_init(56u, 68u);
    g_tick_ms = 1000u;

    /* Only send STATUS for left */
    uint8_t buf[8];
    make_status(buf, 100, 0, 0);
    vesc_can_on_frame(((uint32_t)VESC_PKT_STATUS << 8u) | 56u, buf, 8u);

    /* TLM at t=1100 (100 ms after last frame — within 1000 ms timeout) */
    vesc_can_send_tlm(0u);  /* consume pre-wind */
    g_tlm_count = 0;
    vesc_can_send_tlm(1100u);  /* but only 100ms have passed — still rate-limited */

    /* Force TLM at t=1001 to bypass rate limit */
    s_tlm_tick = (uint32_t)(-2000u);  /* force next call to send */
    vesc_can_send_tlm(1100u);

    ASSERT(g_last_tlm.left_alive  == 1u, "send_tlm: left_alive = 1");
    ASSERT(g_last_tlm.right_alive == 0u, "send_tlm: right_alive = 0 (no STATUS)");
}

/* ---- main ---- */

int main(void)
{
    test_init_stores_ids();
    test_init_zeroes_state();
    test_init_registers_ext_callback();
    test_send_rpm_ext_id_left();
    test_send_rpm_ext_id_right();
    test_send_rpm_payload_positive();
    test_send_rpm_payload_negative();
    test_send_rpm_zero();
    test_on_frame_status_rpm();
    test_on_frame_status_current();
    test_on_frame_status_duty();
    test_on_frame_status_updates_timestamp();
    test_on_frame_status_right_node();
    test_on_frame_status4_temps();
    test_on_frame_status5_voltage();
    test_on_frame_unknown_pkt_type_ignored();
    test_on_frame_unknown_vesc_id_ignored();
    test_on_frame_short_status_ignored();
    test_get_state_unknown_id_returns_false();
    test_get_state_no_frame_returns_false();
    test_get_state_after_status_returns_true();
    test_is_alive_no_frame();
    test_is_alive_within_timeout();
    test_is_alive_after_timeout();
    test_is_alive_at_exact_timeout_boundary();
    test_send_tlm_rate_limited();
    test_send_tlm_payload_content();
    test_send_tlm_alive_flags();

    printf("\n%d passed, %d failed\n", g_pass, g_fail);
    return g_fail ? 1 : 0;
}