diff --git a/include/can_driver.h b/include/can_driver.h index d52f945..10b0d4e 100644 --- a/include/can_driver.h +++ b/include/can_driver.h @@ -78,4 +78,24 @@ void can_driver_get_stats(can_stats_t *out); /* Drain RX FIFO0; call every main-loop tick */ 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 */ diff --git a/include/jlink.h b/include/jlink.h index 5e55ba6..9ff28b6 100644 --- a/include/jlink.h +++ b/include/jlink.h @@ -99,6 +99,7 @@ #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_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) ---- */ typedef struct __attribute__((packed)) { @@ -239,6 +240,22 @@ typedef struct __attribute__((packed)) { int16_t speed_mmps; /* linear speed of centre point (mm/s) */ } 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) ---- */ typedef struct { /* 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); +/* + * 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 */ diff --git a/include/orin_can.h b/include/orin_can.h new file mode 100644 index 0000000..36c0c79 --- /dev/null +++ b/include/orin_can.h @@ -0,0 +1,103 @@ +#ifndef ORIN_CAN_H +#define ORIN_CAN_H + +#include +#include + +/* + * 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 */ diff --git a/include/vesc_can.h b/include/vesc_can.h new file mode 100644 index 0000000..21f67d1 --- /dev/null +++ b/include/vesc_can.h @@ -0,0 +1,117 @@ +#ifndef VESC_CAN_H +#define VESC_CAN_H + +#include +#include + +/* + * 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 */ diff --git a/jetson/ros2_ws/src/saltybot_can_bridge/config/can_bridge_params.yaml b/jetson/ros2_ws/src/saltybot_can_bridge/config/can_bridge_params.yaml new file mode 100644 index 0000000..f5babf2 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_can_bridge/config/can_bridge_params.yaml @@ -0,0 +1,7 @@ +can_bridge_node: + ros__parameters: + can_interface: slcan0 + left_vesc_can_id: 56 + right_vesc_can_id: 68 + mamba_can_id: 1 + command_timeout_s: 0.5 diff --git a/jetson/ros2_ws/src/saltybot_can_bridge/package.xml b/jetson/ros2_ws/src/saltybot_can_bridge/package.xml new file mode 100644 index 0000000..ef1872c --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_can_bridge/package.xml @@ -0,0 +1,35 @@ + + + + saltybot_can_bridge + 0.1.0 + + CAN bus bridge for SaltyBot Orin — interfaces with the Mamba motor + controller and VESC motor controllers via CANable 2.0 (slcan interface). + Publishes IMU, battery, and VESC telemetry to ROS2 topics and forwards + cmd_vel / estop commands to the CAN bus. + + System dependency: python3-can (apt: python3-can or pip: python-can) + + sl-controls + MIT + + rclpy + std_msgs + geometry_msgs + sensor_msgs + + + python3-can + + ament_python + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/jetson/ros2_ws/src/saltybot_can_bridge/resource/saltybot_can_bridge b/jetson/ros2_ws/src/saltybot_can_bridge/resource/saltybot_can_bridge new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/__init__.py b/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/__init__.py new file mode 100644 index 0000000..e146650 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/__init__.py @@ -0,0 +1 @@ +"""SaltyBot CAN bridge package — Mamba controller and VESC telemetry via python-can.""" diff --git a/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/can_bridge_node.py b/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/can_bridge_node.py new file mode 100644 index 0000000..c267938 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/can_bridge_node.py @@ -0,0 +1,383 @@ +#!/usr/bin/env python3 +""" +can_bridge_node.py — ROS2 node bridging the SaltyBot Orin to the Mamba motor +controller and VESC motor controllers over CAN bus. + +The node opens the SocketCAN interface (slcan0 by default), spawns a background +reader thread to process incoming telemetry, and exposes the following interface: + +Subscriptions +------------- + /cmd_vel geometry_msgs/Twist → VESC speed commands (CAN) + /estop std_msgs/Bool → Mamba e-stop (CAN) + +Publications +------------ + /can/imu sensor_msgs/Imu Mamba IMU telemetry + /can/battery sensor_msgs/BatteryState Mamba battery telemetry + /can/vesc/left/state std_msgs/Float32MultiArray Left VESC state + /can/vesc/right/state std_msgs/Float32MultiArray Right VESC state + /can/connection_status std_msgs/String "connected" | "disconnected" + +Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/674 +""" + +import threading +import time +from typing import Optional + +import can +import rclpy +from geometry_msgs.msg import Twist +from rclpy.node import Node +from sensor_msgs.msg import BatteryState, Imu +from std_msgs.msg import Bool, Float32MultiArray, String + +from saltybot_can_bridge.mamba_protocol import ( + MAMBA_CMD_ESTOP, + MAMBA_CMD_MODE, + MAMBA_CMD_VELOCITY, + MAMBA_TELEM_BATTERY, + MAMBA_TELEM_IMU, + VESC_TELEM_STATE, + MODE_DRIVE, + MODE_ESTOP, + MODE_IDLE, + encode_estop_cmd, + encode_mode_cmd, + encode_velocity_cmd, + decode_battery_telem, + decode_imu_telem, + decode_vesc_state, +) + +# Reconnect attempt interval when CAN bus is lost +_RECONNECT_INTERVAL_S: float = 5.0 + +# Watchdog timer tick rate (Hz) +_WATCHDOG_HZ: float = 10.0 + + +class CanBridgeNode(Node): + """CAN bus bridge between Orin ROS2 and Mamba / VESC controllers.""" + + def __init__(self) -> None: + super().__init__("can_bridge_node") + + # ── Parameters ──────────────────────────────────────────────────── + self.declare_parameter("can_interface", "slcan0") + self.declare_parameter("left_vesc_can_id", 56) + self.declare_parameter("right_vesc_can_id", 68) + self.declare_parameter("mamba_can_id", 1) + self.declare_parameter("command_timeout_s", 0.5) + + self._iface: str = self.get_parameter("can_interface").value + self._left_vesc_id: int = self.get_parameter("left_vesc_can_id").value + self._right_vesc_id: int = self.get_parameter("right_vesc_can_id").value + self._mamba_id: int = self.get_parameter("mamba_can_id").value + self._cmd_timeout: float = self.get_parameter("command_timeout_s").value + + # ── State ───────────────────────────────────────────────────────── + self._bus: Optional[can.BusABC] = None + self._connected: bool = False + self._last_cmd_time: float = time.monotonic() + self._lock = threading.Lock() # protects _bus / _connected + + # ── Publishers ──────────────────────────────────────────────────── + self._pub_imu = self.create_publisher(Imu, "/can/imu", 10) + self._pub_battery = self.create_publisher(BatteryState, "/can/battery", 10) + self._pub_vesc_left = self.create_publisher( + Float32MultiArray, "/can/vesc/left/state", 10 + ) + self._pub_vesc_right = self.create_publisher( + Float32MultiArray, "/can/vesc/right/state", 10 + ) + self._pub_status = self.create_publisher( + String, "/can/connection_status", 10 + ) + + # ── Subscriptions ───────────────────────────────────────────────── + self.create_subscription(Twist, "/cmd_vel", self._cmd_vel_cb, 10) + self.create_subscription(Bool, "/estop", self._estop_cb, 10) + + # ── Timers ──────────────────────────────────────────────────────── + self.create_timer(1.0 / _WATCHDOG_HZ, self._watchdog_cb) + self.create_timer(_RECONNECT_INTERVAL_S, self._reconnect_cb) + + # ── Open CAN ────────────────────────────────────────────────────── + self._try_connect() + + # ── Background reader thread ────────────────────────────────────── + self._reader_thread = threading.Thread( + target=self._reader_loop, daemon=True, name="can_reader" + ) + self._reader_thread.start() + + self.get_logger().info( + f"can_bridge_node ready — iface={self._iface} " + f"left_vesc={self._left_vesc_id} right_vesc={self._right_vesc_id} " + f"mamba={self._mamba_id}" + ) + + # ── Connection management ────────────────────────────────────────────── + + def _try_connect(self) -> None: + """Attempt to open the CAN interface; silently skip if already connected.""" + with self._lock: + if self._connected: + return + try: + bus = can.interface.Bus( + channel=self._iface, + bustype="socketcan", + ) + self._bus = bus + self._connected = True + self.get_logger().info(f"CAN bus connected: {self._iface}") + self._publish_status("connected") + except Exception as exc: + self.get_logger().warning( + f"CAN bus not available ({self._iface}): {exc} " + f"— will retry every {_RECONNECT_INTERVAL_S:.0f} s" + ) + self._connected = False + self._publish_status("disconnected") + + def _reconnect_cb(self) -> None: + """Periodic timer: try to reconnect when disconnected.""" + if not self._connected: + self._try_connect() + + def _handle_can_error(self, exc: Exception, context: str) -> None: + """Mark bus as disconnected on any CAN error.""" + self.get_logger().warning(f"CAN error in {context}: {exc}") + with self._lock: + if self._bus is not None: + try: + self._bus.shutdown() + except Exception: + pass + self._bus = None + self._connected = False + self._publish_status("disconnected") + + # ── ROS callbacks ───────────────────────────────────────────────────── + + def _cmd_vel_cb(self, msg: Twist) -> None: + """Convert /cmd_vel Twist to VESC speed commands over CAN.""" + self._last_cmd_time = time.monotonic() + + if not self._connected: + return + + # Differential drive decomposition — individual wheel speeds in m/s. + # The VESC nodes interpret linear velocity directly; angular is handled + # by the sign difference between left and right. + linear = msg.linear.x + angular = msg.angular.z + + # Forward left = forward right for pure translation; for rotation + # left slows and right speeds up (positive angular = CCW = left turn). + # The Mamba velocity command carries both wheels independently. + left_mps = linear - angular + right_mps = linear + angular + + payload = encode_velocity_cmd(left_mps, right_mps) + self._send_can(MAMBA_CMD_VELOCITY, payload, "cmd_vel") + + # Keep Mamba in DRIVE mode while receiving commands + self._send_can(MAMBA_CMD_MODE, encode_mode_cmd(MODE_DRIVE), "cmd_vel mode") + + def _estop_cb(self, msg: Bool) -> None: + """Forward /estop to Mamba over CAN.""" + if not self._connected: + return + payload = encode_estop_cmd(msg.data) + self._send_can(MAMBA_CMD_ESTOP, payload, "estop") + if msg.data: + self._send_can( + MAMBA_CMD_MODE, encode_mode_cmd(MODE_ESTOP), "estop mode" + ) + self.get_logger().warning("E-stop asserted — sent ESTOP to Mamba") + + # ── Watchdog ────────────────────────────────────────────────────────── + + def _watchdog_cb(self) -> None: + """If no /cmd_vel arrives within the timeout, send zero velocity.""" + if not self._connected: + return + elapsed = time.monotonic() - self._last_cmd_time + if elapsed > self._cmd_timeout: + self._send_can( + MAMBA_CMD_VELOCITY, + encode_velocity_cmd(0.0, 0.0), + "watchdog zero-vel", + ) + self._send_can( + MAMBA_CMD_MODE, encode_mode_cmd(MODE_IDLE), "watchdog idle" + ) + + # ── CAN send helper ─────────────────────────────────────────────────── + + def _send_can(self, arb_id: int, data: bytes, context: str) -> None: + """Send a standard CAN frame; handle errors gracefully.""" + with self._lock: + if not self._connected or self._bus is None: + return + bus = self._bus + + msg = can.Message( + arbitration_id=arb_id, + data=data, + is_extended_id=False, + ) + try: + bus.send(msg, timeout=0.05) + except can.CanError as exc: + self._handle_can_error(exc, f"send({context})") + + # ── Background CAN reader ───────────────────────────────────────────── + + def _reader_loop(self) -> None: + """ + Blocking CAN read loop executed in a daemon thread. + Dispatches incoming frames to the appropriate handler. + """ + while rclpy.ok(): + with self._lock: + connected = self._connected + bus = self._bus + + if not connected or bus is None: + time.sleep(0.1) + continue + + try: + frame = bus.recv(timeout=0.5) + except can.CanError as exc: + self._handle_can_error(exc, "reader_loop recv") + continue + + if frame is None: + # Timeout — no frame within 0.5 s, loop again + continue + + self._dispatch_frame(frame) + + def _dispatch_frame(self, frame: can.Message) -> None: + """Route an incoming CAN frame to the correct publisher.""" + arb_id = frame.arbitration_id + data = bytes(frame.data) + + try: + if arb_id == MAMBA_TELEM_IMU: + self._handle_imu(data, frame.timestamp) + + elif arb_id == MAMBA_TELEM_BATTERY: + self._handle_battery(data, frame.timestamp) + + elif arb_id == VESC_TELEM_STATE + self._left_vesc_id: + self._handle_vesc_state(data, frame.timestamp, side="left") + + elif arb_id == VESC_TELEM_STATE + self._right_vesc_id: + self._handle_vesc_state(data, frame.timestamp, side="right") + + except Exception as exc: + self.get_logger().warning( + f"Error parsing CAN frame 0x{arb_id:03X}: {exc}" + ) + + # ── Frame handlers ──────────────────────────────────────────────────── + + def _handle_imu(self, data: bytes, timestamp: float) -> None: + telem = decode_imu_telem(data) + + msg = Imu() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = "imu_link" + + msg.linear_acceleration.x = telem.accel_x + msg.linear_acceleration.y = telem.accel_y + msg.linear_acceleration.z = telem.accel_z + + msg.angular_velocity.x = telem.gyro_x + msg.angular_velocity.y = telem.gyro_y + msg.angular_velocity.z = telem.gyro_z + + # Covariance unknown; mark as -1 per REP-145 + msg.orientation_covariance[0] = -1.0 + + self._pub_imu.publish(msg) + + def _handle_battery(self, data: bytes, timestamp: float) -> None: + telem = decode_battery_telem(data) + + msg = BatteryState() + msg.header.stamp = self.get_clock().now().to_msg() + msg.voltage = telem.voltage + msg.current = telem.current + msg.present = True + msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING + + self._pub_battery.publish(msg) + + def _handle_vesc_state( + self, data: bytes, timestamp: float, side: str + ) -> None: + telem = decode_vesc_state(data) + + msg = Float32MultiArray() + # Layout: [erpm, duty, voltage, current] + msg.data = [telem.erpm, telem.duty, telem.voltage, telem.current] + + if side == "left": + self._pub_vesc_left.publish(msg) + else: + self._pub_vesc_right.publish(msg) + + # ── Status helper ───────────────────────────────────────────────────── + + def _publish_status(self, status: str) -> None: + msg = String() + msg.data = status + self._pub_status.publish(msg) + + # ── Shutdown ────────────────────────────────────────────────────────── + + def destroy_node(self) -> None: + """Send zero velocity and shut down the CAN bus cleanly.""" + if self._connected and self._bus is not None: + try: + self._send_can( + MAMBA_CMD_VELOCITY, + encode_velocity_cmd(0.0, 0.0), + "shutdown", + ) + self._send_can( + MAMBA_CMD_MODE, encode_mode_cmd(MODE_IDLE), "shutdown" + ) + except Exception: + pass + try: + self._bus.shutdown() + except Exception: + pass + super().destroy_node() + + +# --------------------------------------------------------------------------- + +def main(args=None) -> None: + rclpy.init(args=args) + node = CanBridgeNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/mamba_protocol.py b/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/mamba_protocol.py new file mode 100644 index 0000000..10ca43b --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/mamba_protocol.py @@ -0,0 +1,201 @@ +#!/usr/bin/env python3 +""" +mamba_protocol.py — CAN message encoding/decoding for the Mamba motor controller +and VESC telemetry. + +CAN message layout +------------------ +Command frames (Orin → Mamba / VESC): + MAMBA_CMD_VELOCITY 0x100 8 bytes left_speed (f32, m/s) | right_speed (f32, m/s) + MAMBA_CMD_MODE 0x101 1 byte mode (0=idle, 1=drive, 2=estop) + MAMBA_CMD_ESTOP 0x102 1 byte 0x01 = stop + +Telemetry frames (Mamba → Orin): + MAMBA_TELEM_IMU 0x200 24 bytes accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z (f32 each) + MAMBA_TELEM_BATTERY 0x201 8 bytes voltage (f32, V) | current (f32, A) + +VESC telemetry frame (VESC → Orin): + VESC_TELEM_STATE 0x300 16 bytes erpm (f32) | duty (f32) | voltage (f32) | current (f32) + +All multi-byte fields are big-endian. + +Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/674 +""" + +import struct +from dataclasses import dataclass +from typing import Tuple + +# --------------------------------------------------------------------------- +# CAN message IDs +# --------------------------------------------------------------------------- + +MAMBA_CMD_VELOCITY: int = 0x100 +MAMBA_CMD_MODE: int = 0x101 +MAMBA_CMD_ESTOP: int = 0x102 + +MAMBA_TELEM_IMU: int = 0x200 +MAMBA_TELEM_BATTERY: int = 0x201 + +VESC_TELEM_STATE: int = 0x300 + +# --------------------------------------------------------------------------- +# Mode constants +# --------------------------------------------------------------------------- + +MODE_IDLE: int = 0 +MODE_DRIVE: int = 1 +MODE_ESTOP: int = 2 + +# --------------------------------------------------------------------------- +# Data classes for decoded telemetry +# --------------------------------------------------------------------------- + + +@dataclass +class ImuTelemetry: + """Decoded IMU telemetry from Mamba (MAMBA_TELEM_IMU).""" + + accel_x: float = 0.0 # m/s² + accel_y: float = 0.0 + accel_z: float = 0.0 + gyro_x: float = 0.0 # rad/s + gyro_y: float = 0.0 + gyro_z: float = 0.0 + + +@dataclass +class BatteryTelemetry: + """Decoded battery telemetry from Mamba (MAMBA_TELEM_BATTERY).""" + + voltage: float = 0.0 # V + current: float = 0.0 # A + + +@dataclass +class VescStateTelemetry: + """Decoded VESC state telemetry (VESC_TELEM_STATE).""" + + erpm: float = 0.0 # electrical RPM + duty: float = 0.0 # duty cycle [-1.0, 1.0] + voltage: float = 0.0 # bus voltage, V + current: float = 0.0 # phase current, A + + +# --------------------------------------------------------------------------- +# Encode helpers +# --------------------------------------------------------------------------- + +_FMT_VEL = ">ff" # 2 × float32, big-endian +_FMT_MODE = ">B" # 1 × uint8 +_FMT_ESTOP = ">B" # 1 × uint8 +_FMT_IMU = ">ffffff" # 6 × float32 +_FMT_BAT = ">ff" # 2 × float32 +_FMT_VESC = ">ffff" # 4 × float32 + + +def encode_velocity_cmd(left_mps: float, right_mps: float) -> bytes: + """ + Encode a MAMBA_CMD_VELOCITY payload. + + Parameters + ---------- + left_mps: target left wheel speed in m/s (positive = forward) + right_mps: target right wheel speed in m/s (positive = forward) + + Returns + ------- + 8-byte big-endian payload suitable for a CAN frame. + """ + return struct.pack(_FMT_VEL, float(left_mps), float(right_mps)) + + +def encode_mode_cmd(mode: int) -> bytes: + """ + Encode a MAMBA_CMD_MODE payload. + + Parameters + ---------- + mode: one of MODE_IDLE (0), MODE_DRIVE (1), MODE_ESTOP (2) + + Returns + ------- + 1-byte payload. + """ + if mode not in (MODE_IDLE, MODE_DRIVE, MODE_ESTOP): + raise ValueError(f"Invalid mode {mode!r}; expected 0, 1, or 2") + return struct.pack(_FMT_MODE, mode) + + +def encode_estop_cmd(stop: bool = True) -> bytes: + """ + Encode a MAMBA_CMD_ESTOP payload. + + Parameters + ---------- + stop: True to assert e-stop, False to clear. + + Returns + ------- + 1-byte payload (0x01 = stop, 0x00 = clear). + """ + return struct.pack(_FMT_ESTOP, 0x01 if stop else 0x00) + + +# --------------------------------------------------------------------------- +# Decode helpers +# --------------------------------------------------------------------------- + +def decode_imu_telem(data: bytes) -> ImuTelemetry: + """ + Decode a MAMBA_TELEM_IMU payload. + + Parameters + ---------- + data: exactly 24 bytes (6 × float32, big-endian). + + Returns + ------- + ImuTelemetry dataclass instance. + + Raises + ------ + struct.error if data is the wrong length. + """ + ax, ay, az, gx, gy, gz = struct.unpack(_FMT_IMU, data) + return ImuTelemetry( + accel_x=ax, accel_y=ay, accel_z=az, + gyro_x=gx, gyro_y=gy, gyro_z=gz, + ) + + +def decode_battery_telem(data: bytes) -> BatteryTelemetry: + """ + Decode a MAMBA_TELEM_BATTERY payload. + + Parameters + ---------- + data: exactly 8 bytes (2 × float32, big-endian). + + Returns + ------- + BatteryTelemetry dataclass instance. + """ + voltage, current = struct.unpack(_FMT_BAT, data) + return BatteryTelemetry(voltage=voltage, current=current) + + +def decode_vesc_state(data: bytes) -> VescStateTelemetry: + """ + Decode a VESC_TELEM_STATE payload. + + Parameters + ---------- + data: exactly 16 bytes (4 × float32, big-endian). + + Returns + ------- + VescStateTelemetry dataclass instance. + """ + erpm, duty, voltage, current = struct.unpack(_FMT_VESC, data) + return VescStateTelemetry(erpm=erpm, duty=duty, voltage=voltage, current=current) diff --git a/jetson/ros2_ws/src/saltybot_can_bridge/setup.cfg b/jetson/ros2_ws/src/saltybot_can_bridge/setup.cfg new file mode 100644 index 0000000..50fc24b --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_can_bridge/setup.cfg @@ -0,0 +1,5 @@ +[develop] +script_dir=$base/lib/saltybot_can_bridge + +[install] +install_scripts=$base/lib/saltybot_can_bridge diff --git a/jetson/ros2_ws/src/saltybot_can_bridge/setup.py b/jetson/ros2_ws/src/saltybot_can_bridge/setup.py new file mode 100644 index 0000000..88161c9 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_can_bridge/setup.py @@ -0,0 +1,26 @@ +from setuptools import setup + +package_name = "saltybot_can_bridge" + +setup( + name=package_name, + version="0.1.0", + packages=[package_name], + data_files=[ + ("share/ament_index/resource_index/packages", [f"resource/{package_name}"]), + (f"share/{package_name}", ["package.xml"]), + (f"share/{package_name}/config", ["config/can_bridge_params.yaml"]), + ], + install_requires=["setuptools", "python-can"], + zip_safe=True, + maintainer="sl-controls", + maintainer_email="sl-controls@saltylab.local", + description="CAN bus bridge for Mamba controller and VESC telemetry", + license="MIT", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "can_bridge_node = saltybot_can_bridge.can_bridge_node:main", + ], + }, +) diff --git a/jetson/ros2_ws/src/saltybot_can_bridge/test/__init__.py b/jetson/ros2_ws/src/saltybot_can_bridge/test/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_can_bridge/test/test_can_bridge.py b/jetson/ros2_ws/src/saltybot_can_bridge/test/test_can_bridge.py new file mode 100644 index 0000000..2b8f9bf --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_can_bridge/test/test_can_bridge.py @@ -0,0 +1,254 @@ +#!/usr/bin/env python3 +""" +Unit tests for saltybot_can_bridge.mamba_protocol. + +No ROS2 or CAN hardware required — tests exercise encode/decode round-trips +and boundary conditions entirely in Python. + +Run with: pytest test/test_can_bridge.py -v +""" + +import struct +import unittest + +from saltybot_can_bridge.mamba_protocol import ( + MAMBA_CMD_ESTOP, + MAMBA_CMD_MODE, + MAMBA_CMD_VELOCITY, + MAMBA_TELEM_BATTERY, + MAMBA_TELEM_IMU, + VESC_TELEM_STATE, + MODE_DRIVE, + MODE_ESTOP, + MODE_IDLE, + BatteryTelemetry, + ImuTelemetry, + VescStateTelemetry, + decode_battery_telem, + decode_imu_telem, + decode_vesc_state, + encode_estop_cmd, + encode_mode_cmd, + encode_velocity_cmd, +) + + +class TestMessageIDs(unittest.TestCase): + """Verify the CAN message ID constants are correct.""" + + def test_command_ids(self): + self.assertEqual(MAMBA_CMD_VELOCITY, 0x100) + self.assertEqual(MAMBA_CMD_MODE, 0x101) + self.assertEqual(MAMBA_CMD_ESTOP, 0x102) + + def test_telemetry_ids(self): + self.assertEqual(MAMBA_TELEM_IMU, 0x200) + self.assertEqual(MAMBA_TELEM_BATTERY, 0x201) + self.assertEqual(VESC_TELEM_STATE, 0x300) + + +class TestVelocityEncode(unittest.TestCase): + """Tests for encode_velocity_cmd.""" + + def test_zero_velocity(self): + payload = encode_velocity_cmd(0.0, 0.0) + self.assertEqual(len(payload), 8) + left, right = struct.unpack(">ff", payload) + self.assertAlmostEqual(left, 0.0, places=5) + self.assertAlmostEqual(right, 0.0, places=5) + + def test_forward_velocity(self): + payload = encode_velocity_cmd(1.5, 1.5) + left, right = struct.unpack(">ff", payload) + self.assertAlmostEqual(left, 1.5, places=5) + self.assertAlmostEqual(right, 1.5, places=5) + + def test_differential_velocity(self): + payload = encode_velocity_cmd(-0.5, 0.5) + left, right = struct.unpack(">ff", payload) + self.assertAlmostEqual(left, -0.5, places=5) + self.assertAlmostEqual(right, 0.5, places=5) + + def test_large_velocity(self): + # No clamping at the protocol layer — values are sent as-is + payload = encode_velocity_cmd(10.0, -10.0) + left, right = struct.unpack(">ff", payload) + self.assertAlmostEqual(left, 10.0, places=3) + self.assertAlmostEqual(right, -10.0, places=3) + + def test_payload_is_big_endian(self): + # Sanity check: first 4 bytes encode left speed + payload = encode_velocity_cmd(1.0, 0.0) + (left,) = struct.unpack(">f", payload[:4]) + self.assertAlmostEqual(left, 1.0, places=5) + + +class TestModeEncode(unittest.TestCase): + """Tests for encode_mode_cmd.""" + + def test_idle_mode(self): + payload = encode_mode_cmd(MODE_IDLE) + self.assertEqual(payload, b"\x00") + + def test_drive_mode(self): + payload = encode_mode_cmd(MODE_DRIVE) + self.assertEqual(payload, b"\x01") + + def test_estop_mode(self): + payload = encode_mode_cmd(MODE_ESTOP) + self.assertEqual(payload, b"\x02") + + def test_invalid_mode_raises(self): + with self.assertRaises(ValueError): + encode_mode_cmd(99) + + def test_invalid_mode_negative_raises(self): + with self.assertRaises(ValueError): + encode_mode_cmd(-1) + + +class TestEstopEncode(unittest.TestCase): + """Tests for encode_estop_cmd.""" + + def test_estop_assert(self): + self.assertEqual(encode_estop_cmd(True), b"\x01") + + def test_estop_clear(self): + self.assertEqual(encode_estop_cmd(False), b"\x00") + + def test_estop_default_is_stop(self): + self.assertEqual(encode_estop_cmd(), b"\x01") + + +class TestImuDecodeRoundTrip(unittest.TestCase): + """Round-trip tests for IMU telemetry.""" + + def _encode_imu(self, ax, ay, az, gx, gy, gz) -> bytes: + return struct.pack(">ffffff", ax, ay, az, gx, gy, gz) + + def test_zero_imu(self): + data = self._encode_imu(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + telem = decode_imu_telem(data) + self.assertIsInstance(telem, ImuTelemetry) + self.assertAlmostEqual(telem.accel_x, 0.0, places=5) + self.assertAlmostEqual(telem.gyro_z, 0.0, places=5) + + def test_nominal_imu(self): + data = self._encode_imu(0.1, -0.2, 9.81, 0.01, -0.02, 0.03) + telem = decode_imu_telem(data) + self.assertAlmostEqual(telem.accel_x, 0.1, places=4) + self.assertAlmostEqual(telem.accel_y, -0.2, places=4) + self.assertAlmostEqual(telem.accel_z, 9.81, places=3) + self.assertAlmostEqual(telem.gyro_x, 0.01, places=5) + self.assertAlmostEqual(telem.gyro_y, -0.02, places=5) + self.assertAlmostEqual(telem.gyro_z, 0.03, places=5) + + def test_imu_bad_length_raises(self): + with self.assertRaises(struct.error): + decode_imu_telem(b"\x00" * 10) # too short + + +class TestBatteryDecodeRoundTrip(unittest.TestCase): + """Round-trip tests for battery telemetry.""" + + def _encode_bat(self, voltage, current) -> bytes: + return struct.pack(">ff", voltage, current) + + def test_nominal_battery(self): + data = self._encode_bat(24.6, 3.2) + telem = decode_battery_telem(data) + self.assertIsInstance(telem, BatteryTelemetry) + self.assertAlmostEqual(telem.voltage, 24.6, places=3) + self.assertAlmostEqual(telem.current, 3.2, places=4) + + def test_zero_battery(self): + data = self._encode_bat(0.0, 0.0) + telem = decode_battery_telem(data) + self.assertAlmostEqual(telem.voltage, 0.0, places=5) + self.assertAlmostEqual(telem.current, 0.0, places=5) + + def test_max_voltage(self): + # 6S LiPo max ~25.2 V; test with a high value + data = self._encode_bat(25.2, 0.0) + telem = decode_battery_telem(data) + self.assertAlmostEqual(telem.voltage, 25.2, places=3) + + def test_battery_bad_length_raises(self): + with self.assertRaises(struct.error): + decode_battery_telem(b"\x00" * 4) # too short + + +class TestVescStateDecodeRoundTrip(unittest.TestCase): + """Round-trip tests for VESC state telemetry.""" + + def _encode_vesc(self, erpm, duty, voltage, current) -> bytes: + return struct.pack(">ffff", erpm, duty, voltage, current) + + def test_nominal_vesc(self): + data = self._encode_vesc(3000.0, 0.25, 24.0, 5.5) + telem = decode_vesc_state(data) + self.assertIsInstance(telem, VescStateTelemetry) + self.assertAlmostEqual(telem.erpm, 3000.0, places=2) + self.assertAlmostEqual(telem.duty, 0.25, places=5) + self.assertAlmostEqual(telem.voltage, 24.0, places=4) + self.assertAlmostEqual(telem.current, 5.5, places=4) + + def test_zero_vesc(self): + data = self._encode_vesc(0.0, 0.0, 0.0, 0.0) + telem = decode_vesc_state(data) + self.assertAlmostEqual(telem.erpm, 0.0) + self.assertAlmostEqual(telem.duty, 0.0) + + def test_reverse_erpm(self): + data = self._encode_vesc(-1500.0, -0.15, 23.0, 2.0) + telem = decode_vesc_state(data) + self.assertAlmostEqual(telem.erpm, -1500.0, places=2) + self.assertAlmostEqual(telem.duty, -0.15, places=5) + + def test_vesc_bad_length_raises(self): + with self.assertRaises(struct.error): + decode_vesc_state(b"\x00" * 8) # too short (need 16) + + +class TestEncodeDecodeIdentity(unittest.TestCase): + """Cross-module identity tests: encode then decode must be identity.""" + + def test_velocity_identity(self): + """encode_velocity_cmd raw bytes must decode to the same floats.""" + left, right = 0.75, -0.3 + payload = encode_velocity_cmd(left, right) + decoded_l, decoded_r = struct.unpack(">ff", payload) + self.assertAlmostEqual(decoded_l, left, places=5) + self.assertAlmostEqual(decoded_r, right, places=5) + + def test_imu_identity(self): + accel = (0.5, -0.5, 9.8) + gyro = (0.1, -0.1, 0.2) + raw = struct.pack(">ffffff", *accel, *gyro) + telem = decode_imu_telem(raw) + self.assertAlmostEqual(telem.accel_x, accel[0], places=4) + self.assertAlmostEqual(telem.accel_y, accel[1], places=4) + self.assertAlmostEqual(telem.accel_z, accel[2], places=3) + self.assertAlmostEqual(telem.gyro_x, gyro[0], places=5) + self.assertAlmostEqual(telem.gyro_y, gyro[1], places=5) + self.assertAlmostEqual(telem.gyro_z, gyro[2], places=5) + + def test_battery_identity(self): + voltage, current = 22.4, 8.1 + raw = struct.pack(">ff", voltage, current) + telem = decode_battery_telem(raw) + self.assertAlmostEqual(telem.voltage, voltage, places=3) + self.assertAlmostEqual(telem.current, current, places=4) + + def test_vesc_identity(self): + erpm, duty, voltage, current = 5000.0, 0.5, 24.5, 10.0 + raw = struct.pack(">ffff", erpm, duty, voltage, current) + telem = decode_vesc_state(raw) + self.assertAlmostEqual(telem.erpm, erpm, places=2) + self.assertAlmostEqual(telem.duty, duty, places=5) + self.assertAlmostEqual(telem.voltage, voltage, places=3) + self.assertAlmostEqual(telem.current, current, places=4) + + +if __name__ == "__main__": + unittest.main() diff --git a/jetson/scripts/setup_can.sh b/jetson/scripts/setup_can.sh new file mode 100755 index 0000000..c88ac66 --- /dev/null +++ b/jetson/scripts/setup_can.sh @@ -0,0 +1,126 @@ +#!/usr/bin/env bash +# setup_can.sh — Bring up CANable 2.0 (slcan/ttyACM0) as slcan0 at 500 kbps +# Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/674 +# +# This script uses slcand to expose the CANable 2.0 (USB CDC / slcan firmware) +# as a SocketCAN interface. It is NOT used for the gs_usb (native SocketCAN) +# firmware variant — use jetson/scripts/can_setup.sh for that. +# +# Usage: +# sudo ./setup_can.sh # bring up slcan0 +# sudo ./setup_can.sh down # bring down slcan0 and kill slcand +# sudo ./setup_can.sh verify # candump one-shot check (Ctrl-C to stop) +# +# Environment overrides: +# CAN_DEVICE — serial device (default: /dev/ttyACM0) +# CAN_IFACE — SocketCAN name (default: slcan0) +# CAN_BITRATE — bit rate (default: 500000) +# +# Mamba CAN ID: 1 (0x001) +# VESC left: 56 (0x038) +# VESC right: 68 (0x044) + +set -euo pipefail + +DEVICE="${CAN_DEVICE:-/dev/ttyACM0}" +IFACE="${CAN_IFACE:-slcan0}" +BITRATE="${CAN_BITRATE:-500000}" + +log() { echo "[setup_can] $*"; } +warn() { echo "[setup_can] WARNING: $*" >&2; } +die() { echo "[setup_can] ERROR: $*" >&2; exit 1; } + +# Map numeric bitrate to slcand -s flag (0-8 map to standard CAN speeds) +bitrate_flag() { + case "$1" in + 10000) echo "0" ;; + 20000) echo "1" ;; + 50000) echo "2" ;; + 100000) echo "3" ;; + 125000) echo "4" ;; + 250000) echo "5" ;; + 500000) echo "6" ;; + 800000) echo "7" ;; + 1000000) echo "8" ;; + *) die "Unsupported bitrate: $1 (choose 10k–1M)" ;; + esac +} + +# ── up ───────────────────────────────────────────────────────────────────── +cmd_up() { + # Verify serial device is present + if [[ ! -c "$DEVICE" ]]; then + die "$DEVICE not found — is CANable 2.0 plugged in?" + fi + + # If interface already exists, leave it alone + if ip link show "$IFACE" &>/dev/null; then + log "$IFACE is already up — nothing to do." + ip -details link show "$IFACE" + return 0 + fi + + local sflag + sflag=$(bitrate_flag "$BITRATE") + + log "Starting slcand on $DEVICE → $IFACE at ${BITRATE} bps (flag -s${sflag}) …" + # -o open device, -c close on exit, -f forced, -s speed, -S serial baud + slcand -o -c -f -s"${sflag}" -S 3000000 "$DEVICE" "$IFACE" \ + || die "slcand failed to start" + + # Give slcand a moment to create the netdev + local retries=0 + while ! ip link show "$IFACE" &>/dev/null; do + retries=$((retries + 1)) + if [[ $retries -ge 10 ]]; then + die "Timed out waiting for $IFACE to appear after slcand start" + fi + sleep 0.2 + done + + log "Bringing up $IFACE …" + ip link set "$IFACE" up \ + || die "ip link set $IFACE up failed" + + log "$IFACE is up." + ip -details link show "$IFACE" +} + +# ── down ─────────────────────────────────────────────────────────────────── +cmd_down() { + log "Bringing down $IFACE …" + if ip link show "$IFACE" &>/dev/null; then + ip link set "$IFACE" down || warn "Could not set $IFACE down" + else + warn "$IFACE not found — already down?" + fi + + # Kill any running slcand instances bound to our device + if pgrep -f "slcand.*${DEVICE}" &>/dev/null; then + log "Stopping slcand for $DEVICE …" + pkill -f "slcand.*${DEVICE}" || warn "pkill returned non-zero" + fi + log "Done." +} + +# ── verify ───────────────────────────────────────────────────────────────── +cmd_verify() { + if ! ip link show "$IFACE" &>/dev/null; then + die "$IFACE is not up — run '$0 up' first" + fi + log "Listening on $IFACE — expecting frames from Mamba (0x001), VESC left (0x038), VESC right (0x044)" + log "Press Ctrl-C to stop." + exec candump "$IFACE" +} + +# ── main ─────────────────────────────────────────────────────────────────── +CMD="${1:-up}" +case "$CMD" in + up) cmd_up ;; + down) cmd_down ;; + verify) cmd_verify ;; + *) + echo "Usage: $0 [up|down|verify]" + exit 1 + ;; +esac diff --git a/src/can_driver.c b/src/can_driver.c index 1f09716..1e94551 100644 --- a/src/can_driver.c +++ b/src/can_driver.c @@ -15,6 +15,8 @@ static CAN_HandleTypeDef s_can; static volatile can_feedback_t s_feedback[CAN_NUM_MOTORS]; 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) { @@ -51,18 +53,16 @@ void can_driver_init(void) return; } - /* Filter bank 0: 32-bit mask, FIFO0, accept std IDs 0x200–0x21F - * CAN1 is master; its filter banks start at 0 (SlaveStartFilterBank=14 - * reserves banks 14-27 for CAN2, but CAN2 is unused here). - * FilterIdHigh = 0x200 << 5 = 0x4000 (base ID shifted to bit[15:5]) - * FilterMaskHigh = 0x7E0 << 5 = 0xFC00 (mask: top 6 bits must match) */ + /* Filter bank 0: 32-bit mask, FIFO0, accept ALL standard 11-bit frames. + * CAN1 is master (SlaveStartFilterBank=14). FilterIdHigh=0, MaskHigh=0 + * → mask=0 passes every standard ID. Orin std-frame commands land here. */ CAN_FilterTypeDef flt = {0}; flt.FilterBank = 0u; flt.FilterMode = CAN_FILTERMODE_IDMASK; flt.FilterScale = CAN_FILTERSCALE_32BIT; - flt.FilterIdHigh = (uint16_t)(CAN_FILTER_STDID << 5u); + flt.FilterIdHigh = 0u; flt.FilterIdLow = 0u; - flt.FilterMaskIdHigh = (uint16_t)(CAN_FILTER_MASK << 5u); + flt.FilterMaskIdHigh = 0u; flt.FilterMaskIdLow = 0u; flt.FilterFIFOAssignment = CAN_RX_FIFO0; flt.FilterActivation = CAN_FILTER_ENABLE; @@ -73,6 +73,28 @@ void can_driver_init(void) 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); memset((void *)s_feedback, 0, sizeof(s_feedback)); @@ -141,7 +163,7 @@ void can_driver_process(void) } 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) { CAN_RxHeaderTypeDef rxhdr; uint8_t rxdata[8]; @@ -151,31 +173,106 @@ void can_driver_process(void) break; } - /* Only process data frames with standard IDs */ if (rxhdr.IDE != CAN_ID_STD || rxhdr.RTR != CAN_RTR_DATA) { 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; - 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; } - uint8_t nid = (uint8_t)nid_u; - - /* 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(); + if (s_ext_cb != NULL) { + s_ext_cb(rxhdr.ExtId, rxdata, (uint8_t)rxhdr.DLC); + } 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) { if (node_id >= CAN_NUM_MOTORS || out == NULL) { diff --git a/src/jlink.c b/src/jlink.c index 3ccee4b..614a399 100644 --- a/src/jlink.c +++ b/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_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)); +} diff --git a/src/main.c b/src/main.c index d630c05..97e7c7d 100644 --- a/src/main.c +++ b/src/main.c @@ -33,6 +33,8 @@ #include "pid_flash.h" #include "fault_handler.h" #include "can_driver.h" +#include "vesc_can.h" +#include "orin_can.h" #include "servo_bus.h" #include "gimbal.h" #include "lvc.h" @@ -195,8 +197,14 @@ int main(void) { /* Init Jetson serial binary protocol on USART1 (PB6/PB7) at 921600 baud */ 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(); + 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) */ 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) */ if (jlink_state.fault_log_req) { jlink_state.fault_log_req = 0u; @@ -626,6 +651,8 @@ int main(void) { float base_sp = bal.setpoint; if (jlink_is_active(now)) 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)) bal.setpoint += jetson_cmd_sp_offset(); balance_update(&bal, &imu, dt); @@ -659,6 +686,8 @@ int main(void) { * mode_manager_get_steer() blends it with RC steer per active mode. */ if (jlink_is_active(now)) 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)) 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) */ - { - 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. + /* 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. * 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)) { can_cmd_tick = now; - int16_t can_steer = mode_manager_get_steer(&mode); - int32_t speed_rpm = (int32_t)bal.motor_cmd * CAN_RPM_SCALE; - int32_t steer_rpm = (int32_t)can_steer * CAN_RPM_SCALE / 2; - can_cmd_t cmd_l = { (int16_t)(speed_rpm + steer_rpm), 0 }; - can_cmd_t cmd_r = { (int16_t)(speed_rpm - steer_rpm), 0 }; - if (bal.state != BALANCE_ARMED) { - cmd_l.velocity_rpm = 0; - cmd_r.velocity_rpm = 0; + int32_t speed_rpm = 0; + int32_t steer_rpm = 0; + if (bal.state == BALANCE_ARMED && !lvc_is_cutoff()) { + int16_t can_steer = mode_manager_get_steer(&mode); + speed_rpm = (int32_t)bal.motor_cmd * CAN_RPM_SCALE; + steer_rpm = (int32_t)can_steer * CAN_RPM_SCALE / 2; } - can_driver_send_cmd(CAN_NODE_LEFT, &cmd_l); - can_driver_send_cmd(CAN_NODE_RIGHT, &cmd_r); + vesc_can_send_rpm(VESC_CAN_ID_LEFT, speed_rpm + steer_rpm); + 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. diff --git a/src/orin_can.c b/src/orin_can.c new file mode 100644 index 0000000..ced4f98 --- /dev/null +++ b/src/orin_can.c @@ -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 + +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)); +} diff --git a/src/vesc_can.c b/src/vesc_can.c new file mode 100644 index 0000000..df1b62b --- /dev/null +++ b/src/vesc_can.c @@ -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 +#include + +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); +} diff --git a/test/stubs/stm32f7xx_hal.h b/test/stubs/stm32f7xx_hal.h new file mode 100644 index 0000000..301898b --- /dev/null +++ b/test/stubs/stm32f7xx_hal.h @@ -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 +#include +#include + +/* ---- 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 */ diff --git a/test/test_vesc_can.c b/test/test_vesc_can.c new file mode 100644 index 0000000..8557a17 --- /dev/null +++ b/test/test_vesc_can.c @@ -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 +#include +#include + +/* 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 +#include +#include + +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; +}