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>
104 lines
4.0 KiB
C
104 lines
4.0 KiB
C
#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 */
|