sl-firmware
5e82878083
- 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>
118 lines
4.7 KiB
C
118 lines
4.7 KiB
C
#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 */
|