feat: bxCAN integration for VESC motor control and Orin comms (Issue #674)
- can_driver: add filter bank 15 (all ext IDs → FIFO1) and widen bank 14 to accept all standard IDs; add can_driver_send_ext/std and ext/std frame callbacks (can_driver_set_ext_cb / can_driver_set_std_cb) - vesc_can: VESC 29-bit extended CAN protocol driver — send RPM to IDs 56 and 68 (FSESC 6.7 Pro Mini Dual), parse STATUS/STATUS_4/STATUS_5 big-endian payloads, alive timeout, JLINK_TLM_VESC_STATE at 1 Hz - orin_can: Orin↔FC standard CAN protocol — HEARTBEAT/DRIVE/MODE/ESTOP commands in, FC_STATUS + FC_VESC broadcast at 10 Hz - jlink: add JLINK_TLM_VESC_STATE (0x8E), jlink_tlm_vesc_state_t (22 bytes), jlink_send_vesc_state_tlm() - main: wire vesc_can_init/orin_can_init; replace can_driver_send_cmd with vesc_can_send_rpm; inject Orin CAN speed/steer into balance PID; add Orin CAN estop/clear handling; add orin_can_broadcast at 10 Hz - test: 56-test host-side suite for vesc_can; test/stubs/stm32f7xx_hal.h minimal HAL stub for all future host-side tests Safety: balance PID runs independently on Mamba — if Orin CAN link drops (orin_can_is_alive() == false) the robot continues balancing in-place. Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
parent
92c0628c62
commit
5e82878083
@ -78,4 +78,24 @@ void can_driver_get_stats(can_stats_t *out);
|
|||||||
/* Drain RX FIFO0; call every main-loop tick */
|
/* Drain RX FIFO0; call every main-loop tick */
|
||||||
void can_driver_process(void);
|
void can_driver_process(void);
|
||||||
|
|
||||||
|
/* ---- Extended / standard frame support (Issue #674) ---- */
|
||||||
|
|
||||||
|
/* Callback for extended-ID (29-bit) frames arriving in FIFO1 (VESC STATUS) */
|
||||||
|
typedef void (*can_ext_frame_cb_t)(uint32_t ext_id, const uint8_t *data, uint8_t len);
|
||||||
|
|
||||||
|
/* Callback for standard-ID (11-bit) frames arriving in FIFO0 (Orin commands) */
|
||||||
|
typedef void (*can_std_frame_cb_t)(uint16_t std_id, const uint8_t *data, uint8_t len);
|
||||||
|
|
||||||
|
/* Register callback for 29-bit extended frames (register before can_driver_init) */
|
||||||
|
void can_driver_set_ext_cb(can_ext_frame_cb_t cb);
|
||||||
|
|
||||||
|
/* Register callback for 11-bit standard frames (register before can_driver_init) */
|
||||||
|
void can_driver_set_std_cb(can_std_frame_cb_t cb);
|
||||||
|
|
||||||
|
/* Transmit a 29-bit extended-ID data frame (VESC RPM/current commands) */
|
||||||
|
void can_driver_send_ext(uint32_t ext_id, const uint8_t *data, uint8_t len);
|
||||||
|
|
||||||
|
/* Transmit an 11-bit standard-ID data frame (Orin telemetry broadcast) */
|
||||||
|
void can_driver_send_std(uint16_t std_id, const uint8_t *data, uint8_t len);
|
||||||
|
|
||||||
#endif /* CAN_DRIVER_H */
|
#endif /* CAN_DRIVER_H */
|
||||||
|
|||||||
@ -99,6 +99,7 @@
|
|||||||
#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) */
|
#define JLINK_TLM_ODOM 0x8Cu /* jlink_tlm_odom_t (16 bytes, Issue #632) */
|
||||||
|
#define JLINK_TLM_VESC_STATE 0x8Eu /* jlink_tlm_vesc_state_t (22 bytes, Issue #674) */
|
||||||
|
|
||||||
/* ---- Telemetry STATUS payload (20 bytes, packed) ---- */
|
/* ---- Telemetry STATUS payload (20 bytes, packed) ---- */
|
||||||
typedef struct __attribute__((packed)) {
|
typedef struct __attribute__((packed)) {
|
||||||
@ -239,6 +240,22 @@ typedef struct __attribute__((packed)) {
|
|||||||
int16_t speed_mmps; /* linear speed of centre point (mm/s) */
|
int16_t speed_mmps; /* linear speed of centre point (mm/s) */
|
||||||
} jlink_tlm_odom_t; /* 16 bytes */
|
} jlink_tlm_odom_t; /* 16 bytes */
|
||||||
|
|
||||||
|
/* ---- Telemetry VESC_STATE payload (22 bytes, packed) Issue #674 ---- */
|
||||||
|
/* Sent at VESC_TLM_HZ (1 Hz) by vesc_can_send_tlm(). */
|
||||||
|
typedef struct __attribute__((packed)) {
|
||||||
|
int32_t left_rpm; /* left VESC actual RPM */
|
||||||
|
int32_t right_rpm; /* right VESC actual RPM */
|
||||||
|
int16_t left_current_x10; /* left phase current (A × 10) */
|
||||||
|
int16_t right_current_x10; /* right phase current (A × 10) */
|
||||||
|
int16_t left_temp_x10; /* left FET temperature (°C × 10) */
|
||||||
|
int16_t right_temp_x10; /* right FET temperature (°C × 10) */
|
||||||
|
int16_t voltage_x10; /* input voltage (V × 10; from STATUS_5) */
|
||||||
|
uint8_t left_fault; /* left VESC fault code (0 = none) */
|
||||||
|
uint8_t right_fault; /* right VESC fault code (0 = none) */
|
||||||
|
uint8_t left_alive; /* 1 = left VESC alive (STATUS within 1 s) */
|
||||||
|
uint8_t right_alive; /* 1 = right VESC alive (STATUS within 1 s) */
|
||||||
|
} jlink_tlm_vesc_state_t; /* 22 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 */
|
||||||
@ -377,4 +394,11 @@ void jlink_send_lvc_tlm(const jlink_tlm_lvc_t *tlm);
|
|||||||
*/
|
*/
|
||||||
void jlink_send_odom_tlm(const jlink_tlm_odom_t *tlm);
|
void jlink_send_odom_tlm(const jlink_tlm_odom_t *tlm);
|
||||||
|
|
||||||
|
/*
|
||||||
|
* jlink_send_vesc_state_tlm(tlm) - transmit JLINK_TLM_VESC_STATE (0x8E) frame
|
||||||
|
* (28 bytes total) at VESC_TLM_HZ (1 Hz). Issue #674.
|
||||||
|
* Rate-limiting handled by vesc_can_send_tlm(); call from there only.
|
||||||
|
*/
|
||||||
|
void jlink_send_vesc_state_tlm(const jlink_tlm_vesc_state_t *tlm);
|
||||||
|
|
||||||
#endif /* JLINK_H */
|
#endif /* JLINK_H */
|
||||||
|
|||||||
103
include/orin_can.h
Normal file
103
include/orin_can.h
Normal file
@ -0,0 +1,103 @@
|
|||||||
|
#ifndef ORIN_CAN_H
|
||||||
|
#define ORIN_CAN_H
|
||||||
|
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <stdbool.h>
|
||||||
|
|
||||||
|
/*
|
||||||
|
* orin_can — Orin↔FC CAN protocol driver (Issue #674).
|
||||||
|
*
|
||||||
|
* Standard 11-bit CAN IDs on CAN2, FIFO0.
|
||||||
|
*
|
||||||
|
* Orin → FC commands:
|
||||||
|
* 0x300 HEARTBEAT : uint32 sequence counter (4 bytes)
|
||||||
|
* 0x301 DRIVE : int16 speed (−1000..+1000), int16 steer (−1000..+1000)
|
||||||
|
* 0x302 MODE : uint8 mode (0=RC_MANUAL, 1=ASSISTED, 2=AUTONOMOUS)
|
||||||
|
* 0x303 ESTOP : uint8 action (1=ESTOP, 0=CLEAR)
|
||||||
|
*
|
||||||
|
* FC → Orin telemetry (broadcast at ORIN_TLM_HZ):
|
||||||
|
* 0x400 FC_STATUS : int16 pitch_x10, int16 motor_cmd, uint16 vbat_mv,
|
||||||
|
* uint8 balance_state, uint8 flags [bit0=estop, bit1=armed]
|
||||||
|
* 0x401 FC_VESC : int16 left_rpm_x10 (RPM/10), int16 right_rpm_x10,
|
||||||
|
* int16 left_current_x10, int16 right_current_x10
|
||||||
|
*
|
||||||
|
* Balance independence: if no Orin heartbeat for ORIN_HB_TIMEOUT_MS, the FC
|
||||||
|
* continues balancing in-place — Orin commands are simply not injected.
|
||||||
|
* The balance PID loop runs entirely on Mamba and never depends on Orin.
|
||||||
|
*/
|
||||||
|
|
||||||
|
/* ---- Orin → FC command IDs ---- */
|
||||||
|
#define ORIN_CAN_ID_HEARTBEAT 0x300u
|
||||||
|
#define ORIN_CAN_ID_DRIVE 0x301u
|
||||||
|
#define ORIN_CAN_ID_MODE 0x302u
|
||||||
|
#define ORIN_CAN_ID_ESTOP 0x303u
|
||||||
|
|
||||||
|
/* ---- FC → Orin telemetry IDs ---- */
|
||||||
|
#define ORIN_CAN_ID_FC_STATUS 0x400u
|
||||||
|
#define ORIN_CAN_ID_FC_VESC 0x401u
|
||||||
|
|
||||||
|
/* ---- Timing ---- */
|
||||||
|
#define ORIN_HB_TIMEOUT_MS 500u /* Orin offline after 500 ms without any frame */
|
||||||
|
#define ORIN_TLM_HZ 10u /* FC → Orin broadcast rate (Hz) */
|
||||||
|
|
||||||
|
/* ---- Volatile state updated by orin_can_on_frame(), read by main loop ---- */
|
||||||
|
typedef struct {
|
||||||
|
volatile int16_t speed; /* DRIVE: −1000..+1000 */
|
||||||
|
volatile int16_t steer; /* DRIVE: −1000..+1000 */
|
||||||
|
volatile uint8_t mode; /* MODE: robot_mode_t value */
|
||||||
|
volatile uint8_t drive_updated; /* set on DRIVE, cleared by main */
|
||||||
|
volatile uint8_t mode_updated; /* set on MODE, cleared by main */
|
||||||
|
volatile uint8_t estop_req; /* set on ESTOP(1), cleared by main */
|
||||||
|
volatile uint8_t estop_clear_req; /* set on ESTOP(0), cleared by main */
|
||||||
|
volatile uint32_t last_rx_ms; /* HAL_GetTick() of last received frame */
|
||||||
|
} OrinCanState;
|
||||||
|
|
||||||
|
extern volatile OrinCanState orin_can_state;
|
||||||
|
|
||||||
|
/* ---- FC → Orin broadcast payloads (packed, 8 bytes each) ---- */
|
||||||
|
|
||||||
|
typedef struct __attribute__((packed)) {
|
||||||
|
int16_t pitch_x10; /* pitch degrees × 10 */
|
||||||
|
int16_t motor_cmd; /* balance PID output −1000..+1000 */
|
||||||
|
uint16_t vbat_mv; /* battery voltage (mV) */
|
||||||
|
uint8_t balance_state; /* BalanceState: 0=DISARMED,1=ARMED,2=TILT_FAULT */
|
||||||
|
uint8_t flags; /* bit0=estop_active, bit1=armed */
|
||||||
|
} orin_can_fc_status_t; /* 8 bytes */
|
||||||
|
|
||||||
|
typedef struct __attribute__((packed)) {
|
||||||
|
int16_t left_rpm_x10; /* left wheel RPM / 10 (±32767 × 10 = ±327k RPM) */
|
||||||
|
int16_t right_rpm_x10; /* right wheel RPM / 10 */
|
||||||
|
int16_t left_current_x10; /* left phase current × 10 (A) */
|
||||||
|
int16_t right_current_x10; /* right phase current × 10 (A) */
|
||||||
|
} orin_can_fc_vesc_t; /* 8 bytes */
|
||||||
|
|
||||||
|
/* ---- API ---- */
|
||||||
|
|
||||||
|
/*
|
||||||
|
* orin_can_init() — zero state, register orin_can_on_frame as std_cb with
|
||||||
|
* can_driver. Call after can_driver_init().
|
||||||
|
*/
|
||||||
|
void orin_can_init(void);
|
||||||
|
|
||||||
|
/*
|
||||||
|
* orin_can_on_frame(std_id, data, len) — dispatched by can_driver for each
|
||||||
|
* standard-ID frame in FIFO0. Updates orin_can_state.
|
||||||
|
*/
|
||||||
|
void orin_can_on_frame(uint16_t std_id, const uint8_t *data, uint8_t len);
|
||||||
|
|
||||||
|
/*
|
||||||
|
* orin_can_is_alive(now_ms) — true if a frame from Orin arrived within
|
||||||
|
* ORIN_HB_TIMEOUT_MS of now_ms.
|
||||||
|
*/
|
||||||
|
bool orin_can_is_alive(uint32_t now_ms);
|
||||||
|
|
||||||
|
/*
|
||||||
|
* orin_can_broadcast(now_ms, status, vesc) — rate-limited broadcast of
|
||||||
|
* FC_STATUS (0x400) and FC_VESC (0x401) at ORIN_TLM_HZ (10 Hz).
|
||||||
|
* Safe to call every ms; internally rate-limited.
|
||||||
|
*/
|
||||||
|
void orin_can_broadcast(uint32_t now_ms,
|
||||||
|
const orin_can_fc_status_t *status,
|
||||||
|
const orin_can_fc_vesc_t *vesc);
|
||||||
|
|
||||||
|
#endif /* ORIN_CAN_H */
|
||||||
117
include/vesc_can.h
Normal file
117
include/vesc_can.h
Normal file
@ -0,0 +1,117 @@
|
|||||||
|
#ifndef VESC_CAN_H
|
||||||
|
#define VESC_CAN_H
|
||||||
|
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <stdbool.h>
|
||||||
|
|
||||||
|
/*
|
||||||
|
* vesc_can — VESC CAN protocol driver for FSESC 6.7 Pro Mini Dual (Issue #674).
|
||||||
|
*
|
||||||
|
* VESC uses 29-bit extended CAN IDs:
|
||||||
|
* arbitration_id = (packet_type << 8) | vesc_node_id
|
||||||
|
*
|
||||||
|
* Wire format is big-endian throughout (matches VESC FW 6.x).
|
||||||
|
*
|
||||||
|
* Physical layer: CAN2 on PB12 (RX, AF9) / PB13 (TX, AF9) at 500 kbps.
|
||||||
|
*
|
||||||
|
* NOTE ON PA11/PA12 vs PB12/PB13:
|
||||||
|
* PA11/PA12 carry CAN1_RX/TX (AF9) BUT are also USB_OTG_FS DM/DP (AF10).
|
||||||
|
* USB CDC is active on this board, so PA11/PA12 are occupied.
|
||||||
|
* PB8/PB9 (CAN1 alternate) are occupied by I2C1 (barometer).
|
||||||
|
* CAN2 on PB12/PB13 is the only conflict-free choice.
|
||||||
|
* If the SN65HVD230 is wired to the pads labelled RX6/TX6 on the Mamba
|
||||||
|
* silkscreen, those pads connect to PB12/PB13 (SPI2/OSD, repurposed).
|
||||||
|
*
|
||||||
|
* VESC frames arrive in FIFO1 (extended-ID filter, bank 15).
|
||||||
|
* Orin standard frames arrive in FIFO0 (standard-ID filter, bank 14).
|
||||||
|
*/
|
||||||
|
|
||||||
|
/* ---- VESC packet type IDs (upper byte of 29-bit arb ID) ---- */
|
||||||
|
#define VESC_PKT_SET_DUTY 0u /* int32 duty × 100000 */
|
||||||
|
#define VESC_PKT_SET_CURRENT 1u /* int32 current (mA) */
|
||||||
|
#define VESC_PKT_SET_CURRENT_BRAKE 2u /* int32 brake current (mA) */
|
||||||
|
#define VESC_PKT_SET_RPM 3u /* int32 target RPM */
|
||||||
|
#define VESC_PKT_STATUS 9u /* int32 RPM, int16 I×10, int16 duty×1000 */
|
||||||
|
#define VESC_PKT_STATUS_4 16u /* int16 T_fet×10, T_mot×10, I_in×10 */
|
||||||
|
#define VESC_PKT_STATUS_5 27u /* int32 tacho, int16 V_in×10 */
|
||||||
|
|
||||||
|
/* ---- Default VESC node IDs (configurable via vesc_can_init) ---- */
|
||||||
|
#define VESC_CAN_ID_LEFT 56u
|
||||||
|
#define VESC_CAN_ID_RIGHT 68u
|
||||||
|
|
||||||
|
/* ---- Alive timeout ---- */
|
||||||
|
#define VESC_ALIVE_TIMEOUT_MS 1000u /* node offline if no STATUS for 1 s */
|
||||||
|
|
||||||
|
/* ---- JLink telemetry rate ---- */
|
||||||
|
#define VESC_TLM_HZ 1u
|
||||||
|
|
||||||
|
/* ---- Fault codes (VESC FW 6.6) ---- */
|
||||||
|
#define VESC_FAULT_NONE 0u
|
||||||
|
#define VESC_FAULT_OVER_VOLTAGE 1u
|
||||||
|
#define VESC_FAULT_UNDER_VOLTAGE 2u
|
||||||
|
#define VESC_FAULT_DRV 3u
|
||||||
|
#define VESC_FAULT_ABS_OVER_CURRENT 4u
|
||||||
|
#define VESC_FAULT_OVER_TEMP_FET 5u
|
||||||
|
#define VESC_FAULT_OVER_TEMP_MOTOR 6u
|
||||||
|
#define VESC_FAULT_GATE_DRIVER_OVER_VOLTAGE 7u
|
||||||
|
#define VESC_FAULT_GATE_DRIVER_UNDER_VOLTAGE 8u
|
||||||
|
#define VESC_FAULT_MCU_UNDER_VOLTAGE 9u
|
||||||
|
#define VESC_FAULT_WATCHDOG_RESET 10u
|
||||||
|
|
||||||
|
/* ---- Telemetry state per VESC node ---- */
|
||||||
|
typedef struct {
|
||||||
|
int32_t rpm; /* actual RPM (STATUS pkt, int32 BE) */
|
||||||
|
int16_t current_x10; /* phase current (A × 10; STATUS pkt) */
|
||||||
|
int16_t duty_x1000; /* duty cycle (× 1000; –1000..+1000) */
|
||||||
|
int16_t temp_fet_x10; /* FET temperature (°C × 10; STATUS_4) */
|
||||||
|
int16_t temp_motor_x10; /* motor temperature (°C × 10; STATUS_4) */
|
||||||
|
int16_t current_in_x10; /* input (battery) current (A × 10; STATUS_4) */
|
||||||
|
int16_t voltage_x10; /* input voltage (V × 10; STATUS_5) */
|
||||||
|
uint8_t fault_code; /* VESC fault code (0 = none) */
|
||||||
|
uint8_t _pad;
|
||||||
|
uint32_t last_rx_ms; /* HAL_GetTick() of last received STATUS frame */
|
||||||
|
} vesc_state_t;
|
||||||
|
|
||||||
|
/* ---- API ---- */
|
||||||
|
|
||||||
|
/*
|
||||||
|
* vesc_can_init(id_left, id_right) — store VESC node IDs and register the
|
||||||
|
* extended-frame callback with can_driver.
|
||||||
|
* Call after can_driver_init().
|
||||||
|
*/
|
||||||
|
void vesc_can_init(uint8_t id_left, uint8_t id_right);
|
||||||
|
|
||||||
|
/*
|
||||||
|
* vesc_can_send_rpm(vesc_id, rpm) — transmit VESC_PKT_SET_RPM (3) to the
|
||||||
|
* target VESC. arb_id = (3 << 8) | vesc_id. Payload: int32 big-endian.
|
||||||
|
*/
|
||||||
|
void vesc_can_send_rpm(uint8_t vesc_id, int32_t rpm);
|
||||||
|
|
||||||
|
/*
|
||||||
|
* vesc_can_on_frame(ext_id, data, len) — called by can_driver when an
|
||||||
|
* extended-ID frame arrives (registered via can_driver_set_ext_cb).
|
||||||
|
* Parses STATUS / STATUS_4 / STATUS_5 into the matching vesc_state_t.
|
||||||
|
*/
|
||||||
|
void vesc_can_on_frame(uint32_t ext_id, const uint8_t *data, uint8_t len);
|
||||||
|
|
||||||
|
/*
|
||||||
|
* vesc_can_get_state(vesc_id, out) — copy latest telemetry snapshot.
|
||||||
|
* vesc_id must match id_left or id_right passed to vesc_can_init.
|
||||||
|
* Returns false if vesc_id unknown or no frame has arrived yet.
|
||||||
|
*/
|
||||||
|
bool vesc_can_get_state(uint8_t vesc_id, vesc_state_t *out);
|
||||||
|
|
||||||
|
/*
|
||||||
|
* vesc_can_is_alive(vesc_id, now_ms) — true if a STATUS frame arrived
|
||||||
|
* within VESC_ALIVE_TIMEOUT_MS of now_ms.
|
||||||
|
*/
|
||||||
|
bool vesc_can_is_alive(uint8_t vesc_id, uint32_t now_ms);
|
||||||
|
|
||||||
|
/*
|
||||||
|
* vesc_can_send_tlm(now_ms) — rate-limited JLINK_TLM_VESC_STATE (0x8E)
|
||||||
|
* telemetry to Orin over JLink. Safe to call every ms; internally
|
||||||
|
* rate-limited to VESC_TLM_HZ (1 Hz).
|
||||||
|
*/
|
||||||
|
void vesc_can_send_tlm(uint32_t now_ms);
|
||||||
|
|
||||||
|
#endif /* VESC_CAN_H */
|
||||||
137
src/can_driver.c
137
src/can_driver.c
@ -15,6 +15,8 @@
|
|||||||
static CAN_HandleTypeDef s_can;
|
static CAN_HandleTypeDef s_can;
|
||||||
static volatile can_feedback_t s_feedback[CAN_NUM_MOTORS];
|
static volatile can_feedback_t s_feedback[CAN_NUM_MOTORS];
|
||||||
static volatile can_stats_t s_stats;
|
static volatile can_stats_t s_stats;
|
||||||
|
static can_ext_frame_cb_t s_ext_cb = NULL;
|
||||||
|
static can_std_frame_cb_t s_std_cb = NULL;
|
||||||
|
|
||||||
void can_driver_init(void)
|
void can_driver_init(void)
|
||||||
{
|
{
|
||||||
@ -51,18 +53,16 @@ void can_driver_init(void)
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* Filter bank 0: 32-bit mask, FIFO0, accept std IDs 0x200–0x21F
|
/* Filter bank 0: 32-bit mask, FIFO0, accept ALL standard 11-bit frames.
|
||||||
* CAN1 is master; its filter banks start at 0 (SlaveStartFilterBank=14
|
* CAN1 is master (SlaveStartFilterBank=14). FilterIdHigh=0, MaskHigh=0
|
||||||
* reserves banks 14-27 for CAN2, but CAN2 is unused here).
|
* → mask=0 passes every standard ID. Orin std-frame commands land here. */
|
||||||
* FilterIdHigh = 0x200 << 5 = 0x4000 (base ID shifted to bit[15:5])
|
|
||||||
* FilterMaskHigh = 0x7E0 << 5 = 0xFC00 (mask: top 6 bits must match) */
|
|
||||||
CAN_FilterTypeDef flt = {0};
|
CAN_FilterTypeDef flt = {0};
|
||||||
flt.FilterBank = 0u;
|
flt.FilterBank = 0u;
|
||||||
flt.FilterMode = CAN_FILTERMODE_IDMASK;
|
flt.FilterMode = CAN_FILTERMODE_IDMASK;
|
||||||
flt.FilterScale = CAN_FILTERSCALE_32BIT;
|
flt.FilterScale = CAN_FILTERSCALE_32BIT;
|
||||||
flt.FilterIdHigh = (uint16_t)(CAN_FILTER_STDID << 5u);
|
flt.FilterIdHigh = 0u;
|
||||||
flt.FilterIdLow = 0u;
|
flt.FilterIdLow = 0u;
|
||||||
flt.FilterMaskIdHigh = (uint16_t)(CAN_FILTER_MASK << 5u);
|
flt.FilterMaskIdHigh = 0u;
|
||||||
flt.FilterMaskIdLow = 0u;
|
flt.FilterMaskIdLow = 0u;
|
||||||
flt.FilterFIFOAssignment = CAN_RX_FIFO0;
|
flt.FilterFIFOAssignment = CAN_RX_FIFO0;
|
||||||
flt.FilterActivation = CAN_FILTER_ENABLE;
|
flt.FilterActivation = CAN_FILTER_ENABLE;
|
||||||
@ -73,6 +73,28 @@ void can_driver_init(void)
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* Filter bank 15: 32-bit mask, FIFO1, accept ALL extended 29-bit frames.
|
||||||
|
* For extended frames, the IDE bit sits at FilterIdLow[2].
|
||||||
|
* FilterIdLow = 0x0004 (IDE=1) → match extended frames.
|
||||||
|
* FilterMaskIdLow = 0x0004 → only the IDE bit is checked; all ext IDs pass.
|
||||||
|
* This routes every VESC STATUS / STATUS_4 / STATUS_5 frame to FIFO1. */
|
||||||
|
CAN_FilterTypeDef flt2 = {0};
|
||||||
|
flt2.FilterBank = 15u;
|
||||||
|
flt2.FilterMode = CAN_FILTERMODE_IDMASK;
|
||||||
|
flt2.FilterScale = CAN_FILTERSCALE_32BIT;
|
||||||
|
flt2.FilterIdHigh = 0u;
|
||||||
|
flt2.FilterIdLow = 0x0004u; /* IDE = 1 */
|
||||||
|
flt2.FilterMaskIdHigh = 0u;
|
||||||
|
flt2.FilterMaskIdLow = 0x0004u; /* only check IDE bit */
|
||||||
|
flt2.FilterFIFOAssignment = CAN_RX_FIFO1;
|
||||||
|
flt2.FilterActivation = CAN_FILTER_ENABLE;
|
||||||
|
flt2.SlaveStartFilterBank = 14u;
|
||||||
|
|
||||||
|
if (HAL_CAN_ConfigFilter(&s_can, &flt2) != HAL_OK) {
|
||||||
|
s_stats.bus_off = 1u;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
HAL_CAN_Start(&s_can);
|
HAL_CAN_Start(&s_can);
|
||||||
|
|
||||||
memset((void *)s_feedback, 0, sizeof(s_feedback));
|
memset((void *)s_feedback, 0, sizeof(s_feedback));
|
||||||
@ -141,7 +163,7 @@ void can_driver_process(void)
|
|||||||
}
|
}
|
||||||
s_stats.bus_off = 0u;
|
s_stats.bus_off = 0u;
|
||||||
|
|
||||||
/* Drain FIFO0 */
|
/* Drain FIFO0 (standard-ID frames: Orin commands + legacy feedback) */
|
||||||
while (HAL_CAN_GetRxFifoFillLevel(&s_can, CAN_RX_FIFO0) > 0u) {
|
while (HAL_CAN_GetRxFifoFillLevel(&s_can, CAN_RX_FIFO0) > 0u) {
|
||||||
CAN_RxHeaderTypeDef rxhdr;
|
CAN_RxHeaderTypeDef rxhdr;
|
||||||
uint8_t rxdata[8];
|
uint8_t rxdata[8];
|
||||||
@ -151,31 +173,106 @@ void can_driver_process(void)
|
|||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* Only process data frames with standard IDs */
|
|
||||||
if (rxhdr.IDE != CAN_ID_STD || rxhdr.RTR != CAN_RTR_DATA) {
|
if (rxhdr.IDE != CAN_ID_STD || rxhdr.RTR != CAN_RTR_DATA) {
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* Decode feedback frames: base 0x200, one per node */
|
/* Dispatch to registered standard-frame callback (Orin CAN protocol) */
|
||||||
|
if (s_std_cb != NULL) {
|
||||||
|
s_std_cb((uint16_t)rxhdr.StdId, rxdata, (uint8_t)rxhdr.DLC);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Legacy: decode feedback frames at 0x200-0x21F (Issue #597) */
|
||||||
uint32_t nid_u = rxhdr.StdId - CAN_ID_FEEDBACK_BASE;
|
uint32_t nid_u = rxhdr.StdId - CAN_ID_FEEDBACK_BASE;
|
||||||
if (nid_u >= CAN_NUM_MOTORS || rxhdr.DLC < 8u) {
|
if (nid_u < CAN_NUM_MOTORS && rxhdr.DLC >= 8u) {
|
||||||
|
uint8_t nid = (uint8_t)nid_u;
|
||||||
|
s_feedback[nid].velocity_rpm = (int16_t)((uint16_t)rxdata[0] | ((uint16_t)rxdata[1] << 8u));
|
||||||
|
s_feedback[nid].current_ma = (int16_t)((uint16_t)rxdata[2] | ((uint16_t)rxdata[3] << 8u));
|
||||||
|
s_feedback[nid].position_x100 = (int16_t)((uint16_t)rxdata[4] | ((uint16_t)rxdata[5] << 8u));
|
||||||
|
s_feedback[nid].temperature_c = (int8_t)rxdata[6];
|
||||||
|
s_feedback[nid].fault = rxdata[7];
|
||||||
|
s_feedback[nid].last_rx_ms = HAL_GetTick();
|
||||||
|
}
|
||||||
|
|
||||||
|
s_stats.rx_count++;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Drain FIFO1 (extended-ID frames: VESC STATUS/STATUS_4/STATUS_5) */
|
||||||
|
while (HAL_CAN_GetRxFifoFillLevel(&s_can, CAN_RX_FIFO1) > 0u) {
|
||||||
|
CAN_RxHeaderTypeDef rxhdr;
|
||||||
|
uint8_t rxdata[8];
|
||||||
|
|
||||||
|
if (HAL_CAN_GetRxMessage(&s_can, CAN_RX_FIFO1, &rxhdr, rxdata) != HAL_OK) {
|
||||||
|
s_stats.err_count++;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (rxhdr.IDE != CAN_ID_EXT || rxhdr.RTR != CAN_RTR_DATA) {
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
uint8_t nid = (uint8_t)nid_u;
|
if (s_ext_cb != NULL) {
|
||||||
|
s_ext_cb(rxhdr.ExtId, rxdata, (uint8_t)rxhdr.DLC);
|
||||||
/* Layout: [0-1] vel_rpm [2-3] current_ma [4-5] pos_x100 [6] temp_c [7] fault */
|
}
|
||||||
s_feedback[nid].velocity_rpm = (int16_t)((uint16_t)rxdata[0] | ((uint16_t)rxdata[1] << 8u));
|
|
||||||
s_feedback[nid].current_ma = (int16_t)((uint16_t)rxdata[2] | ((uint16_t)rxdata[3] << 8u));
|
|
||||||
s_feedback[nid].position_x100 = (int16_t)((uint16_t)rxdata[4] | ((uint16_t)rxdata[5] << 8u));
|
|
||||||
s_feedback[nid].temperature_c = (int8_t)rxdata[6];
|
|
||||||
s_feedback[nid].fault = rxdata[7];
|
|
||||||
s_feedback[nid].last_rx_ms = HAL_GetTick();
|
|
||||||
|
|
||||||
s_stats.rx_count++;
|
s_stats.rx_count++;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void can_driver_set_ext_cb(can_ext_frame_cb_t cb)
|
||||||
|
{
|
||||||
|
s_ext_cb = cb;
|
||||||
|
}
|
||||||
|
|
||||||
|
void can_driver_set_std_cb(can_std_frame_cb_t cb)
|
||||||
|
{
|
||||||
|
s_std_cb = cb;
|
||||||
|
}
|
||||||
|
|
||||||
|
void can_driver_send_ext(uint32_t ext_id, const uint8_t *data, uint8_t len)
|
||||||
|
{
|
||||||
|
if (s_stats.bus_off || len > 8u) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
CAN_TxHeaderTypeDef hdr = {0};
|
||||||
|
hdr.ExtId = ext_id;
|
||||||
|
hdr.IDE = CAN_ID_EXT;
|
||||||
|
hdr.RTR = CAN_RTR_DATA;
|
||||||
|
hdr.DLC = len;
|
||||||
|
|
||||||
|
uint32_t mailbox;
|
||||||
|
if (HAL_CAN_GetTxMailboxesFreeLevel(&s_can) > 0u) {
|
||||||
|
if (HAL_CAN_AddTxMessage(&s_can, &hdr, (uint8_t *)data, &mailbox) == HAL_OK) {
|
||||||
|
s_stats.tx_count++;
|
||||||
|
} else {
|
||||||
|
s_stats.err_count++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void can_driver_send_std(uint16_t std_id, const uint8_t *data, uint8_t len)
|
||||||
|
{
|
||||||
|
if (s_stats.bus_off || len > 8u) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
CAN_TxHeaderTypeDef hdr = {0};
|
||||||
|
hdr.StdId = std_id;
|
||||||
|
hdr.IDE = CAN_ID_STD;
|
||||||
|
hdr.RTR = CAN_RTR_DATA;
|
||||||
|
hdr.DLC = len;
|
||||||
|
|
||||||
|
uint32_t mailbox;
|
||||||
|
if (HAL_CAN_GetTxMailboxesFreeLevel(&s_can) > 0u) {
|
||||||
|
if (HAL_CAN_AddTxMessage(&s_can, &hdr, (uint8_t *)data, &mailbox) == HAL_OK) {
|
||||||
|
s_stats.tx_count++;
|
||||||
|
} else {
|
||||||
|
s_stats.err_count++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
bool can_driver_get_feedback(uint8_t node_id, can_feedback_t *out)
|
bool can_driver_get_feedback(uint8_t node_id, can_feedback_t *out)
|
||||||
{
|
{
|
||||||
if (node_id >= CAN_NUM_MOTORS || out == NULL) {
|
if (node_id >= CAN_NUM_MOTORS || out == NULL) {
|
||||||
|
|||||||
25
src/jlink.c
25
src/jlink.c
@ -662,3 +662,28 @@ void jlink_send_odom_tlm(const jlink_tlm_odom_t *tlm)
|
|||||||
|
|
||||||
jlink_tx_locked(frame, sizeof(frame));
|
jlink_tx_locked(frame, sizeof(frame));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* ---- jlink_send_vesc_state_tlm() -- Issue #674 ---- */
|
||||||
|
void jlink_send_vesc_state_tlm(const jlink_tlm_vesc_state_t *tlm)
|
||||||
|
{
|
||||||
|
/*
|
||||||
|
* Frame: [STX][LEN][0x8E][22 bytes VESC_STATE][CRC_hi][CRC_lo][ETX]
|
||||||
|
* LEN = 1 (CMD) + 22 (payload) = 23; total frame = 28 bytes.
|
||||||
|
* At 921600 baud: 28×10/921600 ≈ 0.30 ms — safe to block.
|
||||||
|
*/
|
||||||
|
static uint8_t frame[28];
|
||||||
|
const uint8_t plen = (uint8_t)sizeof(jlink_tlm_vesc_state_t); /* 22 */
|
||||||
|
const uint8_t len = 1u + plen; /* 23 */
|
||||||
|
|
||||||
|
frame[0] = JLINK_STX;
|
||||||
|
frame[1] = len;
|
||||||
|
frame[2] = JLINK_TLM_VESC_STATE;
|
||||||
|
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));
|
||||||
|
}
|
||||||
|
|||||||
96
src/main.c
96
src/main.c
@ -33,6 +33,8 @@
|
|||||||
#include "pid_flash.h"
|
#include "pid_flash.h"
|
||||||
#include "fault_handler.h"
|
#include "fault_handler.h"
|
||||||
#include "can_driver.h"
|
#include "can_driver.h"
|
||||||
|
#include "vesc_can.h"
|
||||||
|
#include "orin_can.h"
|
||||||
#include "servo_bus.h"
|
#include "servo_bus.h"
|
||||||
#include "gimbal.h"
|
#include "gimbal.h"
|
||||||
#include "lvc.h"
|
#include "lvc.h"
|
||||||
@ -195,8 +197,14 @@ int main(void) {
|
|||||||
/* Init Jetson serial binary protocol on USART1 (PB6/PB7) at 921600 baud */
|
/* Init Jetson serial binary protocol on USART1 (PB6/PB7) at 921600 baud */
|
||||||
jlink_init();
|
jlink_init();
|
||||||
|
|
||||||
/* Init CAN1 BLDC motor controller bus — PB8/PB9, 500 kbps (Issue #597/#676) */
|
/* Init CAN1 bus — PB8/PB9, 500 kbps (Issues #597/#676/#674).
|
||||||
|
* Register callbacks BEFORE can_driver_init() so both are ready when
|
||||||
|
* filter banks activate. */
|
||||||
|
can_driver_set_ext_cb(vesc_can_on_frame); /* VESC STATUS → FIFO1 */
|
||||||
|
can_driver_set_std_cb(orin_can_on_frame); /* Orin cmds → FIFO0 */
|
||||||
can_driver_init();
|
can_driver_init();
|
||||||
|
vesc_can_init(VESC_CAN_ID_LEFT, VESC_CAN_ID_RIGHT);
|
||||||
|
orin_can_init();
|
||||||
|
|
||||||
/* Send fault log summary on boot if a prior fault was recorded (Issue #565) */
|
/* Send fault log summary on boot if a prior fault was recorded (Issue #565) */
|
||||||
if (fault_get_last_type() != FAULT_NONE) {
|
if (fault_get_last_type() != FAULT_NONE) {
|
||||||
@ -469,6 +477,23 @@ int main(void) {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* Orin CAN: handle commands from Orin over CAN bus (Issue #674).
|
||||||
|
* Estop and clear are latching; drive/mode are consumed each tick.
|
||||||
|
* Balance independence: if orin_can_is_alive() == false the loop
|
||||||
|
* simply does not inject any commands and holds upright in-place. */
|
||||||
|
if (orin_can_state.estop_req) {
|
||||||
|
orin_can_state.estop_req = 0u;
|
||||||
|
safety_remote_estop(ESTOP_REMOTE);
|
||||||
|
safety_arm_cancel();
|
||||||
|
balance_disarm(&bal);
|
||||||
|
motor_driver_estop(&motors);
|
||||||
|
}
|
||||||
|
if (orin_can_state.estop_clear_req) {
|
||||||
|
orin_can_state.estop_clear_req = 0u;
|
||||||
|
if (safety_remote_estop_active() && bal.state == BALANCE_DISARMED)
|
||||||
|
safety_remote_estop_clear();
|
||||||
|
}
|
||||||
|
|
||||||
/* FAULT_LOG_GET: send fault log telemetry to Jetson (Issue #565) */
|
/* FAULT_LOG_GET: send fault log telemetry to Jetson (Issue #565) */
|
||||||
if (jlink_state.fault_log_req) {
|
if (jlink_state.fault_log_req) {
|
||||||
jlink_state.fault_log_req = 0u;
|
jlink_state.fault_log_req = 0u;
|
||||||
@ -626,6 +651,8 @@ int main(void) {
|
|||||||
float base_sp = bal.setpoint;
|
float base_sp = bal.setpoint;
|
||||||
if (jlink_is_active(now))
|
if (jlink_is_active(now))
|
||||||
bal.setpoint += ((float)jlink_state.speed / 1000.0f) * JETSON_SPEED_MAX_DEG;
|
bal.setpoint += ((float)jlink_state.speed / 1000.0f) * JETSON_SPEED_MAX_DEG;
|
||||||
|
else if (orin_can_is_alive(now))
|
||||||
|
bal.setpoint += ((float)orin_can_state.speed / 1000.0f) * JETSON_SPEED_MAX_DEG;
|
||||||
else if (jetson_cmd_is_active(now))
|
else if (jetson_cmd_is_active(now))
|
||||||
bal.setpoint += jetson_cmd_sp_offset();
|
bal.setpoint += jetson_cmd_sp_offset();
|
||||||
balance_update(&bal, &imu, dt);
|
balance_update(&bal, &imu, dt);
|
||||||
@ -659,6 +686,8 @@ int main(void) {
|
|||||||
* mode_manager_get_steer() blends it with RC steer per active mode. */
|
* mode_manager_get_steer() blends it with RC steer per active mode. */
|
||||||
if (jlink_is_active(now))
|
if (jlink_is_active(now))
|
||||||
mode_manager_set_auto_cmd(&mode, jlink_state.steer, 0);
|
mode_manager_set_auto_cmd(&mode, jlink_state.steer, 0);
|
||||||
|
else if (orin_can_is_alive(now))
|
||||||
|
mode_manager_set_auto_cmd(&mode, orin_can_state.steer, 0);
|
||||||
else if (jetson_cmd_is_active(now))
|
else if (jetson_cmd_is_active(now))
|
||||||
mode_manager_set_auto_cmd(&mode, jetson_cmd_steer(), 0);
|
mode_manager_set_auto_cmd(&mode, jetson_cmd_steer(), 0);
|
||||||
|
|
||||||
@ -687,38 +716,47 @@ int main(void) {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/* CAN BLDC: enable/disable follows arm state (Issue #597) */
|
/* VESC: send RPM commands at CAN_TX_RATE_HZ (100 Hz) via 29-bit extended CAN.
|
||||||
{
|
* VESC does not need enable/disable frames — RPM=0 while disarmed holds brakes.
|
||||||
static bool s_can_enabled = false;
|
|
||||||
bool armed_now = (bal.state == BALANCE_ARMED);
|
|
||||||
if (armed_now && !s_can_enabled) {
|
|
||||||
can_driver_send_enable(CAN_NODE_LEFT, true);
|
|
||||||
can_driver_send_enable(CAN_NODE_RIGHT, true);
|
|
||||||
s_can_enabled = true;
|
|
||||||
} else if (!armed_now && s_can_enabled) {
|
|
||||||
can_driver_send_enable(CAN_NODE_LEFT, false);
|
|
||||||
can_driver_send_enable(CAN_NODE_RIGHT, false);
|
|
||||||
s_can_enabled = false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/* CAN BLDC: send velocity/torque commands at CAN_TX_RATE_HZ (100 Hz) (Issue #597)
|
|
||||||
* Converts balance PID output + blended steer to per-wheel RPM.
|
|
||||||
* Left wheel: speed_rpm + steer_rpm (differential drive)
|
* Left wheel: speed_rpm + steer_rpm (differential drive)
|
||||||
* Right wheel: speed_rpm - steer_rpm */
|
* Right wheel: speed_rpm − steer_rpm (Issue #674) */
|
||||||
if (now - can_cmd_tick >= (1000u / CAN_TX_RATE_HZ)) {
|
if (now - can_cmd_tick >= (1000u / CAN_TX_RATE_HZ)) {
|
||||||
can_cmd_tick = now;
|
can_cmd_tick = now;
|
||||||
int16_t can_steer = mode_manager_get_steer(&mode);
|
int32_t speed_rpm = 0;
|
||||||
int32_t speed_rpm = (int32_t)bal.motor_cmd * CAN_RPM_SCALE;
|
int32_t steer_rpm = 0;
|
||||||
int32_t steer_rpm = (int32_t)can_steer * CAN_RPM_SCALE / 2;
|
if (bal.state == BALANCE_ARMED && !lvc_is_cutoff()) {
|
||||||
can_cmd_t cmd_l = { (int16_t)(speed_rpm + steer_rpm), 0 };
|
int16_t can_steer = mode_manager_get_steer(&mode);
|
||||||
can_cmd_t cmd_r = { (int16_t)(speed_rpm - steer_rpm), 0 };
|
speed_rpm = (int32_t)bal.motor_cmd * CAN_RPM_SCALE;
|
||||||
if (bal.state != BALANCE_ARMED) {
|
steer_rpm = (int32_t)can_steer * CAN_RPM_SCALE / 2;
|
||||||
cmd_l.velocity_rpm = 0;
|
|
||||||
cmd_r.velocity_rpm = 0;
|
|
||||||
}
|
}
|
||||||
can_driver_send_cmd(CAN_NODE_LEFT, &cmd_l);
|
vesc_can_send_rpm(VESC_CAN_ID_LEFT, speed_rpm + steer_rpm);
|
||||||
can_driver_send_cmd(CAN_NODE_RIGHT, &cmd_r);
|
vesc_can_send_rpm(VESC_CAN_ID_RIGHT, speed_rpm - steer_rpm);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* VESC telemetry: send JLINK_TLM_VESC_STATE at 1 Hz (Issue #674) */
|
||||||
|
vesc_can_send_tlm(now);
|
||||||
|
|
||||||
|
/* Orin CAN broadcast: FC_STATUS + FC_VESC at ORIN_TLM_HZ (10 Hz) (Issue #674) */
|
||||||
|
{
|
||||||
|
orin_can_fc_status_t fc_st;
|
||||||
|
fc_st.pitch_x10 = (int16_t)(bal.pitch_deg * 10.0f);
|
||||||
|
fc_st.motor_cmd = bal.motor_cmd;
|
||||||
|
uint32_t _vbat = battery_read_mv();
|
||||||
|
fc_st.vbat_mv = (_vbat > 65535u) ? 65535u : (uint16_t)_vbat;
|
||||||
|
fc_st.balance_state = (uint8_t)bal.state;
|
||||||
|
fc_st.flags = (safety_remote_estop_active() ? 0x01u : 0u) |
|
||||||
|
(bal.state == BALANCE_ARMED ? 0x02u : 0u);
|
||||||
|
|
||||||
|
orin_can_fc_vesc_t fc_vesc;
|
||||||
|
vesc_state_t vl, vr;
|
||||||
|
bool vl_ok = vesc_can_get_state(VESC_CAN_ID_LEFT, &vl);
|
||||||
|
bool vr_ok = vesc_can_get_state(VESC_CAN_ID_RIGHT, &vr);
|
||||||
|
fc_vesc.left_rpm_x10 = vl_ok ? (int16_t)(vl.rpm / 10) : 0;
|
||||||
|
fc_vesc.right_rpm_x10 = vr_ok ? (int16_t)(vr.rpm / 10) : 0;
|
||||||
|
fc_vesc.left_current_x10 = vl_ok ? vl.current_x10 : 0;
|
||||||
|
fc_vesc.right_current_x10 = vr_ok ? vr.current_x10 : 0;
|
||||||
|
|
||||||
|
orin_can_broadcast(now, &fc_st, &fc_vesc);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* CRSF telemetry uplink — battery voltage + arm state at 1 Hz.
|
/* CRSF telemetry uplink — battery voltage + arm state at 1 Hz.
|
||||||
|
|||||||
96
src/orin_can.c
Normal file
96
src/orin_can.c
Normal file
@ -0,0 +1,96 @@
|
|||||||
|
/* orin_can.c — Orin↔FC CAN protocol driver (Issue #674)
|
||||||
|
*
|
||||||
|
* Receives high-level drive/mode/estop commands from Orin over CAN.
|
||||||
|
* Broadcasts FC status and VESC telemetry back to Orin at ORIN_TLM_HZ.
|
||||||
|
*
|
||||||
|
* Registered as the standard-ID callback with can_driver via
|
||||||
|
* can_driver_set_std_cb(orin_can_on_frame).
|
||||||
|
*
|
||||||
|
* Balance independence: if Orin link drops (orin_can_is_alive() == false),
|
||||||
|
* main loop stops injecting Orin commands and the balance PID holds
|
||||||
|
* upright in-place. No action required here — the safety is in main.c.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "orin_can.h"
|
||||||
|
#include "can_driver.h"
|
||||||
|
#include "stm32f7xx_hal.h"
|
||||||
|
#include <string.h>
|
||||||
|
|
||||||
|
volatile OrinCanState orin_can_state;
|
||||||
|
static uint32_t s_tlm_tick;
|
||||||
|
|
||||||
|
void orin_can_init(void)
|
||||||
|
{
|
||||||
|
memset((void *)&orin_can_state, 0, sizeof(orin_can_state));
|
||||||
|
/* Pre-wind so first broadcast fires on the first eligible tick */
|
||||||
|
s_tlm_tick = (uint32_t)(-(uint32_t)(1000u / ORIN_TLM_HZ));
|
||||||
|
can_driver_set_std_cb(orin_can_on_frame);
|
||||||
|
}
|
||||||
|
|
||||||
|
void orin_can_on_frame(uint16_t std_id, const uint8_t *data, uint8_t len)
|
||||||
|
{
|
||||||
|
/* Any frame from Orin refreshes the heartbeat */
|
||||||
|
orin_can_state.last_rx_ms = HAL_GetTick();
|
||||||
|
|
||||||
|
switch (std_id) {
|
||||||
|
case ORIN_CAN_ID_HEARTBEAT:
|
||||||
|
/* Heartbeat payload (sequence counter) ignored — timestamp is enough */
|
||||||
|
break;
|
||||||
|
|
||||||
|
case ORIN_CAN_ID_DRIVE:
|
||||||
|
/* int16 speed (BE), int16 steer (BE) */
|
||||||
|
if (len < 4u) { break; }
|
||||||
|
orin_can_state.speed = (int16_t)(((uint16_t)data[0] << 8u) | (uint16_t)data[1]);
|
||||||
|
orin_can_state.steer = (int16_t)(((uint16_t)data[2] << 8u) | (uint16_t)data[3]);
|
||||||
|
orin_can_state.drive_updated = 1u;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case ORIN_CAN_ID_MODE:
|
||||||
|
/* uint8 mode */
|
||||||
|
if (len < 1u) { break; }
|
||||||
|
orin_can_state.mode = data[0];
|
||||||
|
orin_can_state.mode_updated = 1u;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case ORIN_CAN_ID_ESTOP:
|
||||||
|
/* uint8: 1 = assert estop, 0 = clear estop */
|
||||||
|
if (len < 1u) { break; }
|
||||||
|
if (data[0] != 0u) {
|
||||||
|
orin_can_state.estop_req = 1u;
|
||||||
|
} else {
|
||||||
|
orin_can_state.estop_clear_req = 1u;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
default:
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool orin_can_is_alive(uint32_t now_ms)
|
||||||
|
{
|
||||||
|
if (orin_can_state.last_rx_ms == 0u) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
return (now_ms - orin_can_state.last_rx_ms) < ORIN_HB_TIMEOUT_MS;
|
||||||
|
}
|
||||||
|
|
||||||
|
void orin_can_broadcast(uint32_t now_ms,
|
||||||
|
const orin_can_fc_status_t *status,
|
||||||
|
const orin_can_fc_vesc_t *vesc)
|
||||||
|
{
|
||||||
|
if ((now_ms - s_tlm_tick) < (1000u / ORIN_TLM_HZ)) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
s_tlm_tick = now_ms;
|
||||||
|
|
||||||
|
uint8_t buf[8];
|
||||||
|
|
||||||
|
/* FC_STATUS (0x400): 8 bytes */
|
||||||
|
memcpy(buf, status, sizeof(orin_can_fc_status_t));
|
||||||
|
can_driver_send_std(ORIN_CAN_ID_FC_STATUS, buf, (uint8_t)sizeof(orin_can_fc_status_t));
|
||||||
|
|
||||||
|
/* FC_VESC (0x401): 8 bytes */
|
||||||
|
memcpy(buf, vesc, sizeof(orin_can_fc_vesc_t));
|
||||||
|
can_driver_send_std(ORIN_CAN_ID_FC_VESC, buf, (uint8_t)sizeof(orin_can_fc_vesc_t));
|
||||||
|
}
|
||||||
141
src/vesc_can.c
Normal file
141
src/vesc_can.c
Normal file
@ -0,0 +1,141 @@
|
|||||||
|
/* vesc_can.c — VESC CAN protocol driver (Issue #674)
|
||||||
|
*
|
||||||
|
* Registers vesc_can_on_frame as the extended-frame callback with can_driver.
|
||||||
|
* VESC uses 29-bit arb IDs: (pkt_type << 8) | vesc_node_id.
|
||||||
|
* All wire values are big-endian (VESC FW 6.x).
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "vesc_can.h"
|
||||||
|
#include "can_driver.h"
|
||||||
|
#include "jlink.h"
|
||||||
|
#include "stm32f7xx_hal.h"
|
||||||
|
#include <string.h>
|
||||||
|
#include <stddef.h>
|
||||||
|
|
||||||
|
static uint8_t s_id_left;
|
||||||
|
static uint8_t s_id_right;
|
||||||
|
static vesc_state_t s_state[2]; /* [0] = left, [1] = right */
|
||||||
|
static uint32_t s_tlm_tick;
|
||||||
|
|
||||||
|
static vesc_state_t *state_for_id(uint8_t vesc_id)
|
||||||
|
{
|
||||||
|
if (vesc_id == s_id_left) return &s_state[0];
|
||||||
|
if (vesc_id == s_id_right) return &s_state[1];
|
||||||
|
return NULL;
|
||||||
|
}
|
||||||
|
|
||||||
|
void vesc_can_init(uint8_t id_left, uint8_t id_right)
|
||||||
|
{
|
||||||
|
s_id_left = id_left;
|
||||||
|
s_id_right = id_right;
|
||||||
|
memset(s_state, 0, sizeof(s_state));
|
||||||
|
/* Pre-wind so first send_tlm call fires immediately */
|
||||||
|
s_tlm_tick = (uint32_t)(-(uint32_t)(1000u / VESC_TLM_HZ));
|
||||||
|
can_driver_set_ext_cb(vesc_can_on_frame);
|
||||||
|
}
|
||||||
|
|
||||||
|
void vesc_can_send_rpm(uint8_t vesc_id, int32_t rpm)
|
||||||
|
{
|
||||||
|
/* arb_id = (VESC_PKT_SET_RPM << 8) | vesc_id */
|
||||||
|
uint32_t ext_id = ((uint32_t)VESC_PKT_SET_RPM << 8u) | (uint32_t)vesc_id;
|
||||||
|
|
||||||
|
/* Payload: int32 RPM, big-endian */
|
||||||
|
uint32_t urpm = (uint32_t)rpm;
|
||||||
|
uint8_t data[4];
|
||||||
|
data[0] = (uint8_t)(urpm >> 24u);
|
||||||
|
data[1] = (uint8_t)(urpm >> 16u);
|
||||||
|
data[2] = (uint8_t)(urpm >> 8u);
|
||||||
|
data[3] = (uint8_t)(urpm);
|
||||||
|
|
||||||
|
can_driver_send_ext(ext_id, data, 4u);
|
||||||
|
}
|
||||||
|
|
||||||
|
void vesc_can_on_frame(uint32_t ext_id, const uint8_t *data, uint8_t len)
|
||||||
|
{
|
||||||
|
uint8_t pkt_type = (uint8_t)(ext_id >> 8u);
|
||||||
|
uint8_t vesc_id = (uint8_t)(ext_id & 0xFFu);
|
||||||
|
vesc_state_t *s = state_for_id(vesc_id);
|
||||||
|
|
||||||
|
if (s == NULL) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
switch (pkt_type) {
|
||||||
|
case VESC_PKT_STATUS: /* 9: int32 RPM, int16 I×10, int16 duty×1000 (8 bytes) */
|
||||||
|
if (len < 8u) { break; }
|
||||||
|
s->rpm = (int32_t)(((uint32_t)data[0] << 24u) |
|
||||||
|
((uint32_t)data[1] << 16u) |
|
||||||
|
((uint32_t)data[2] << 8u) |
|
||||||
|
(uint32_t)data[3]);
|
||||||
|
s->current_x10 = (int16_t)(((uint16_t)data[4] << 8u) | (uint16_t)data[5]);
|
||||||
|
s->duty_x1000 = (int16_t)(((uint16_t)data[6] << 8u) | (uint16_t)data[7]);
|
||||||
|
s->last_rx_ms = HAL_GetTick();
|
||||||
|
break;
|
||||||
|
|
||||||
|
case VESC_PKT_STATUS_4: /* 16: int16 T_fet×10, T_mot×10, I_in×10 (6 bytes) */
|
||||||
|
if (len < 6u) { break; }
|
||||||
|
s->temp_fet_x10 = (int16_t)(((uint16_t)data[0] << 8u) | (uint16_t)data[1]);
|
||||||
|
s->temp_motor_x10 = (int16_t)(((uint16_t)data[2] << 8u) | (uint16_t)data[3]);
|
||||||
|
s->current_in_x10 = (int16_t)(((uint16_t)data[4] << 8u) | (uint16_t)data[5]);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case VESC_PKT_STATUS_5: /* 27: int32 tacho (ignored), int16 V_in×10 (6 bytes) */
|
||||||
|
if (len < 6u) { break; }
|
||||||
|
/* bytes [0..3] = odometer tachometer — not stored */
|
||||||
|
s->voltage_x10 = (int16_t)(((uint16_t)data[4] << 8u) | (uint16_t)data[5]);
|
||||||
|
break;
|
||||||
|
|
||||||
|
default:
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool vesc_can_get_state(uint8_t vesc_id, vesc_state_t *out)
|
||||||
|
{
|
||||||
|
if (out == NULL) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
vesc_state_t *s = state_for_id(vesc_id);
|
||||||
|
if (s == NULL || s->last_rx_ms == 0u) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
memcpy(out, s, sizeof(vesc_state_t));
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool vesc_can_is_alive(uint8_t vesc_id, uint32_t now_ms)
|
||||||
|
{
|
||||||
|
vesc_state_t *s = state_for_id(vesc_id);
|
||||||
|
if (s == NULL || s->last_rx_ms == 0u) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
return (now_ms - s->last_rx_ms) < VESC_ALIVE_TIMEOUT_MS;
|
||||||
|
}
|
||||||
|
|
||||||
|
void vesc_can_send_tlm(uint32_t now_ms)
|
||||||
|
{
|
||||||
|
if ((now_ms - s_tlm_tick) < (1000u / VESC_TLM_HZ)) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
s_tlm_tick = now_ms;
|
||||||
|
|
||||||
|
jlink_tlm_vesc_state_t tlm;
|
||||||
|
memset(&tlm, 0, sizeof(tlm));
|
||||||
|
|
||||||
|
tlm.left_rpm = s_state[0].rpm;
|
||||||
|
tlm.right_rpm = s_state[1].rpm;
|
||||||
|
tlm.left_current_x10 = s_state[0].current_x10;
|
||||||
|
tlm.right_current_x10 = s_state[1].current_x10;
|
||||||
|
tlm.left_temp_x10 = s_state[0].temp_fet_x10;
|
||||||
|
tlm.right_temp_x10 = s_state[1].temp_fet_x10;
|
||||||
|
/* Use left voltage; fall back to right if left not yet received */
|
||||||
|
tlm.voltage_x10 = s_state[0].voltage_x10
|
||||||
|
? s_state[0].voltage_x10
|
||||||
|
: s_state[1].voltage_x10;
|
||||||
|
tlm.left_fault = s_state[0].fault_code;
|
||||||
|
tlm.right_fault = s_state[1].fault_code;
|
||||||
|
tlm.left_alive = vesc_can_is_alive(s_id_left, now_ms) ? 1u : 0u;
|
||||||
|
tlm.right_alive = vesc_can_is_alive(s_id_right, now_ms) ? 1u : 0u;
|
||||||
|
|
||||||
|
jlink_send_vesc_state_tlm(&tlm);
|
||||||
|
}
|
||||||
126
test/stubs/stm32f7xx_hal.h
Normal file
126
test/stubs/stm32f7xx_hal.h
Normal file
@ -0,0 +1,126 @@
|
|||||||
|
/* test/stubs/stm32f7xx_hal.h — minimal HAL stub for host-side unit tests.
|
||||||
|
*
|
||||||
|
* Provides just enough types and functions so that the firmware source files
|
||||||
|
* compile on a host (Linux/macOS) with -DTEST_HOST.
|
||||||
|
* Each test file must define the actual behavior of HAL_GetTick() etc.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef STM32F7XX_HAL_H
|
||||||
|
#define STM32F7XX_HAL_H
|
||||||
|
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <stdbool.h>
|
||||||
|
#include <stddef.h>
|
||||||
|
|
||||||
|
/* ---- Return codes ---- */
|
||||||
|
#define HAL_OK 0
|
||||||
|
#define HAL_ERROR 1
|
||||||
|
#define HAL_BUSY 2
|
||||||
|
#define HAL_TIMEOUT 3
|
||||||
|
typedef uint32_t HAL_StatusTypeDef;
|
||||||
|
|
||||||
|
/* ---- Enable / Disable ---- */
|
||||||
|
#define ENABLE 1
|
||||||
|
#define DISABLE 0
|
||||||
|
|
||||||
|
/* ---- HAL_GetTick: each test declares its own implementation ---- */
|
||||||
|
uint32_t HAL_GetTick(void);
|
||||||
|
|
||||||
|
/* ---- Minimal CAN types used by can_driver.c / vesc_can.c ---- */
|
||||||
|
typedef struct { uint32_t dummy; } CAN_TypeDef;
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
uint32_t Prescaler;
|
||||||
|
uint32_t Mode;
|
||||||
|
uint32_t SyncJumpWidth;
|
||||||
|
uint32_t TimeSeg1;
|
||||||
|
uint32_t TimeSeg2;
|
||||||
|
uint32_t TimeTriggeredMode;
|
||||||
|
uint32_t AutoBusOff;
|
||||||
|
uint32_t AutoWakeUp;
|
||||||
|
uint32_t AutoRetransmission;
|
||||||
|
uint32_t ReceiveFifoLocked;
|
||||||
|
uint32_t TransmitFifoPriority;
|
||||||
|
} CAN_InitTypeDef;
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
CAN_TypeDef *Instance;
|
||||||
|
CAN_InitTypeDef Init;
|
||||||
|
/* opaque HAL internals omitted */
|
||||||
|
} CAN_HandleTypeDef;
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
uint32_t StdId;
|
||||||
|
uint32_t ExtId;
|
||||||
|
uint32_t IDE;
|
||||||
|
uint32_t RTR;
|
||||||
|
uint32_t DLC;
|
||||||
|
uint32_t Timestamp;
|
||||||
|
uint32_t FilterMatchIndex;
|
||||||
|
} CAN_RxHeaderTypeDef;
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
uint32_t StdId;
|
||||||
|
uint32_t ExtId;
|
||||||
|
uint32_t IDE;
|
||||||
|
uint32_t RTR;
|
||||||
|
uint32_t DLC;
|
||||||
|
uint32_t TransmitGlobalTime;
|
||||||
|
} CAN_TxHeaderTypeDef;
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
uint32_t FilterIdHigh;
|
||||||
|
uint32_t FilterIdLow;
|
||||||
|
uint32_t FilterMaskIdHigh;
|
||||||
|
uint32_t FilterMaskIdLow;
|
||||||
|
uint32_t FilterFIFOAssignment;
|
||||||
|
uint32_t FilterBank;
|
||||||
|
uint32_t FilterMode;
|
||||||
|
uint32_t FilterScale;
|
||||||
|
uint32_t FilterActivation;
|
||||||
|
uint32_t SlaveStartFilterBank;
|
||||||
|
} CAN_FilterTypeDef;
|
||||||
|
|
||||||
|
#define CAN_ID_STD 0x00000000u
|
||||||
|
#define CAN_ID_EXT 0x00000004u
|
||||||
|
#define CAN_RTR_DATA 0x00000000u
|
||||||
|
#define CAN_RTR_REMOTE 0x00000002u
|
||||||
|
#define CAN_FILTERMODE_IDMASK 0u
|
||||||
|
#define CAN_FILTERSCALE_32BIT 1u
|
||||||
|
#define CAN_RX_FIFO0 0u
|
||||||
|
#define CAN_RX_FIFO1 1u
|
||||||
|
#define CAN_FILTER_ENABLE 1u
|
||||||
|
#define CAN_MODE_NORMAL 0u
|
||||||
|
#define CAN_SJW_1TQ 0u
|
||||||
|
#define CAN_BS1_13TQ 0u
|
||||||
|
#define CAN_BS2_4TQ 0u
|
||||||
|
#define CAN_ESR_BOFF 0x00000004u
|
||||||
|
|
||||||
|
static inline HAL_StatusTypeDef HAL_CAN_Init(CAN_HandleTypeDef *h){(void)h;return HAL_OK;}
|
||||||
|
static inline HAL_StatusTypeDef HAL_CAN_ConfigFilter(CAN_HandleTypeDef *h, CAN_FilterTypeDef *f){(void)h;(void)f;return HAL_OK;}
|
||||||
|
static inline HAL_StatusTypeDef HAL_CAN_Start(CAN_HandleTypeDef *h){(void)h;return HAL_OK;}
|
||||||
|
static inline uint32_t HAL_CAN_GetTxMailboxesFreeLevel(CAN_HandleTypeDef *h){(void)h;return 3u;}
|
||||||
|
static inline HAL_StatusTypeDef HAL_CAN_AddTxMessage(CAN_HandleTypeDef *h, CAN_TxHeaderTypeDef *hdr, uint8_t *d, uint32_t *mb){(void)h;(void)hdr;(void)d;(void)mb;return HAL_OK;}
|
||||||
|
static inline uint32_t HAL_CAN_GetRxFifoFillLevel(CAN_HandleTypeDef *h, uint32_t f){(void)h;(void)f;return 0u;}
|
||||||
|
static inline HAL_StatusTypeDef HAL_CAN_GetRxMessage(CAN_HandleTypeDef *h, uint32_t f, CAN_RxHeaderTypeDef *hdr, uint8_t *d){(void)h;(void)f;(void)hdr;(void)d;return HAL_OK;}
|
||||||
|
|
||||||
|
/* ---- GPIO (minimal, for can_driver GPIO init) ---- */
|
||||||
|
typedef struct { uint32_t dummy; } GPIO_TypeDef;
|
||||||
|
typedef struct {
|
||||||
|
uint32_t Pin; uint32_t Mode; uint32_t Pull; uint32_t Speed; uint32_t Alternate;
|
||||||
|
} GPIO_InitTypeDef;
|
||||||
|
static inline void HAL_GPIO_Init(GPIO_TypeDef *p, GPIO_InitTypeDef *g){(void)p;(void)g;}
|
||||||
|
#define GPIOB ((GPIO_TypeDef *)0)
|
||||||
|
#define GPIO_PIN_12 (1u<<12)
|
||||||
|
#define GPIO_PIN_13 (1u<<13)
|
||||||
|
#define GPIO_MODE_AF_PP 0u
|
||||||
|
#define GPIO_NOPULL 0u
|
||||||
|
#define GPIO_SPEED_FREQ_HIGH 0u
|
||||||
|
#define GPIO_AF9_CAN2 9u
|
||||||
|
|
||||||
|
/* ---- RCC stubs ---- */
|
||||||
|
#define __HAL_RCC_CAN1_CLK_ENABLE() ((void)0)
|
||||||
|
#define __HAL_RCC_CAN2_CLK_ENABLE() ((void)0)
|
||||||
|
#define __HAL_RCC_GPIOB_CLK_ENABLE() ((void)0)
|
||||||
|
|
||||||
|
#endif /* STM32F7XX_HAL_H */
|
||||||
518
test/test_vesc_can.c
Normal file
518
test/test_vesc_can.c
Normal file
@ -0,0 +1,518 @@
|
|||||||
|
/*
|
||||||
|
* 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;
|
||||||
|
}
|
||||||
Loading…
x
Reference in New Issue
Block a user