From 06db56103f8d235e9e27916e47c4b4c234c1fb39 Mon Sep 17 00:00:00 2001 From: sl-webui Date: Tue, 17 Mar 2026 22:49:06 -0400 Subject: [PATCH 1/2] feat: Enable VESC driver telemetry publishing (Issue #681) vesc_driver_node.py: - Add VescState dataclass with to_dict() serialization - Add CAN_PACKET_STATUS/STATUS_4/STATUS_5 (9/16/27) RX constants - Add FAULT_NAMES lookup (11 VESC FW 6.6 fault codes) - Add background CAN RX thread (_rx_loop / _dispatch_frame) that parses STATUS broadcast frames using struct.unpack - Add publishers for /saltybot/vesc/left and /saltybot/vesc/right (std_msgs/String JSON) at configurable telemetry_rate_hz (default 10 Hz) - Combine watchdog + publish into single timer callback - Proper thread cleanup in destroy_node() Co-Authored-By: Claude Sonnet 4.6 --- .../saltybot_vesc_driver/vesc_driver_node.py | 178 ++++++++++++++++-- 1 file changed, 159 insertions(+), 19 deletions(-) diff --git a/jetson/ros2_ws/src/saltybot_vesc_driver/saltybot_vesc_driver/vesc_driver_node.py b/jetson/ros2_ws/src/saltybot_vesc_driver/saltybot_vesc_driver/vesc_driver_node.py index e9e047f..d5ff4f6 100644 --- a/jetson/ros2_ws/src/saltybot_vesc_driver/saltybot_vesc_driver/vesc_driver_node.py +++ b/jetson/ros2_ws/src/saltybot_vesc_driver/saltybot_vesc_driver/vesc_driver_node.py @@ -4,23 +4,84 @@ VESC CAN driver node using python-can with SocketCAN. Subscribes to /cmd_vel (geometry_msgs/Twist) and sends duty cycle commands to two VESC motor controllers via CAN bus. +Also receives VESC STATUS broadcast frames and publishes telemetry: + /saltybot/vesc/left (std_msgs/String JSON) + /saltybot/vesc/right (std_msgs/String JSON) + VESC CAN protocol: Extended frame ID = (CAN_PACKET_ID << 8) | VESC_ID CAN_PACKET_SET_DUTY = 0 -> payload: int32 (duty * 100000) """ +import json import struct +import threading import time +from dataclasses import dataclass, field +from typing import Optional import can import rclpy from geometry_msgs.msg import Twist from rclpy.node import Node +from std_msgs.msg import String -# VESC CAN packet IDs -CAN_PACKET_SET_DUTY = 0 -CAN_PACKET_SET_RPM = 3 +# ── VESC CAN packet IDs ──────────────────────────────────────────────────────── +CAN_PACKET_SET_DUTY = 0 +CAN_PACKET_SET_RPM = 3 +CAN_PACKET_STATUS = 9 # RPM, phase current, duty +CAN_PACKET_STATUS_4 = 16 # FET temp, motor temp, input current +CAN_PACKET_STATUS_5 = 27 # Input voltage + +FAULT_NAMES = { + 0: "NONE", 1: "OVER_VOLTAGE", + 2: "UNDER_VOLTAGE", 3: "DRV", + 4: "ABS_OVER_CURRENT", 5: "OVER_TEMP_FET", + 6: "OVER_TEMP_MOTOR", 7: "GATE_DRIVER_OVER_VOLTAGE", + 8: "GATE_DRIVER_UNDER_VOLTAGE_3V3", 9: "MCU_UNDER_VOLTAGE", + 10: "BOOTING_FROM_WATCHDOG_RESET", 11: "ENCODER_SPI_FAULT", +} + +ALIVE_TIMEOUT_S = 1.0 + + +@dataclass +class VescState: + can_id: int + rpm: int = 0 + current_a: float = 0.0 + current_in_a: float = 0.0 + duty_cycle: float = 0.0 + voltage_v: float = 0.0 + temp_fet_c: float = 0.0 + temp_motor_c: float = 0.0 + fault_code: int = 0 + last_ts: float = field(default_factory=lambda: 0.0) + + @property + def is_alive(self) -> bool: + return (time.monotonic() - self.last_ts) < ALIVE_TIMEOUT_S + + @property + def fault_name(self) -> str: + return FAULT_NAMES.get(self.fault_code, f"UNKNOWN_{self.fault_code}") + + def to_dict(self) -> dict: + return { + "can_id": self.can_id, + "rpm": self.rpm, + "current_a": round(self.current_a, 2), + "current_in_a": round(self.current_in_a, 2), + "duty_cycle": round(self.duty_cycle, 4), + "voltage_v": round(self.voltage_v, 2), + "temp_fet_c": round(self.temp_fet_c, 1), + "temp_motor_c": round(self.temp_motor_c, 1), + "fault_code": self.fault_code, + "fault_name": self.fault_name, + "alive": self.is_alive, + "stamp": time.time(), + } def make_can_id(packet_id: int, vesc_id: int) -> int: @@ -40,14 +101,16 @@ class VescCanDriver(Node): self.declare_parameter('wheel_radius', 0.1) self.declare_parameter('max_speed', 5.0) self.declare_parameter('command_timeout', 1.0) + self.declare_parameter('telemetry_rate_hz', 10) # Read parameters - self.interface = self.get_parameter('interface').value - self.left_id = self.get_parameter('left_motor_id').value - self.right_id = self.get_parameter('right_motor_id').value - self.wheel_sep = self.get_parameter('wheel_separation').value - self.max_speed = self.get_parameter('max_speed').value + self.interface = self.get_parameter('interface').value + self.left_id = self.get_parameter('left_motor_id').value + self.right_id = self.get_parameter('right_motor_id').value + self.wheel_sep = self.get_parameter('wheel_separation').value + self.max_speed = self.get_parameter('max_speed').value self.cmd_timeout = self.get_parameter('command_timeout').value + tel_hz = int(self.get_parameter('telemetry_rate_hz').value) # Open CAN bus try: @@ -62,43 +125,118 @@ class VescCanDriver(Node): self._last_cmd_time = time.monotonic() + # Per-VESC telemetry state + self._state: dict[int, VescState] = { + self.left_id: VescState(can_id=self.left_id), + self.right_id: VescState(can_id=self.right_id), + } + self._state_lock = threading.Lock() + + # Telemetry publishers + self._pub_left = self.create_publisher(String, '/saltybot/vesc/left', 10) + self._pub_right = self.create_publisher(String, '/saltybot/vesc/right', 10) + + # CAN RX background thread + self._running = True + self._rx_thread = threading.Thread(target=self._rx_loop, name='vesc_rx', daemon=True) + self._rx_thread.start() + # Subscriber self.create_subscription(Twist, '/cmd_vel', self._cmd_vel_cb, 10) - # Watchdog timer (10 Hz) - self.create_timer(0.1, self._watchdog_cb) + # Watchdog + telemetry publish timer + self.create_timer(1.0 / max(1, tel_hz), self._watchdog_and_publish_cb) self.get_logger().info( - f'VESC CAN driver ready — left={self.left_id} right={self.right_id}' + f'VESC CAN driver ready — left={self.left_id} right={self.right_id} ' + f'telemetry={tel_hz} Hz' ) # ------------------------------------------------------------------ + # Command velocity callback + # ------------------------------------------------------------------ def _cmd_vel_cb(self, msg: Twist): self._last_cmd_time = time.monotonic() - linear = msg.linear.x + linear = msg.linear.x angular = msg.angular.z - left_speed = linear - (angular * self.wheel_sep / 2.0) + left_speed = linear - (angular * self.wheel_sep / 2.0) right_speed = linear + (angular * self.wheel_sep / 2.0) - left_duty = max(-1.0, min(1.0, left_speed / self.max_speed)) + left_duty = max(-1.0, min(1.0, left_speed / self.max_speed)) right_duty = max(-1.0, min(1.0, right_speed / self.max_speed)) - self._send_duty(self.left_id, left_duty) + self._send_duty(self.left_id, left_duty) self._send_duty(self.right_id, right_duty) - def _watchdog_cb(self): + # ------------------------------------------------------------------ + # Watchdog + telemetry publish (combined timer callback) + # ------------------------------------------------------------------ + + def _watchdog_and_publish_cb(self): elapsed = time.monotonic() - self._last_cmd_time if elapsed > self.cmd_timeout: - self._send_duty(self.left_id, 0.0) + self._send_duty(self.left_id, 0.0) self._send_duty(self.right_id, 0.0) + with self._state_lock: + l_dict = self._state[self.left_id].to_dict() + r_dict = self._state[self.right_id].to_dict() + + self._pub_left.publish(String(data=json.dumps(l_dict))) + self._pub_right.publish(String(data=json.dumps(r_dict))) + + # ------------------------------------------------------------------ + # CAN RX background thread + # ------------------------------------------------------------------ + + def _rx_loop(self) -> None: + """Receive VESC STATUS broadcast frames and update telemetry state.""" + while self._running: + try: + msg = self.bus.recv(timeout=0.1) + except Exception: + continue + if msg is None or not msg.is_extended_id: + continue + self._dispatch_frame(msg.arbitration_id, bytes(msg.data)) + + def _dispatch_frame(self, arb_id: int, data: bytes) -> None: + packet_type = (arb_id >> 8) & 0xFFFF + vesc_id = arb_id & 0xFF + + if vesc_id not in self._state: + return + + now = time.monotonic() + with self._state_lock: + s = self._state[vesc_id] + + if packet_type == CAN_PACKET_STATUS and len(data) >= 8: + rpm = struct.unpack('>i', data[0:4])[0] + current_a = struct.unpack('>h', data[4:6])[0] / 10.0 + duty_cycle = struct.unpack('>h', data[6:8])[0] / 1000.0 + s.rpm = rpm + s.current_a = current_a + s.duty_cycle = duty_cycle + s.last_ts = now + + elif packet_type == CAN_PACKET_STATUS_4 and len(data) >= 6: + s.temp_fet_c = struct.unpack('>h', data[0:2])[0] / 10.0 + s.temp_motor_c = struct.unpack('>h', data[2:4])[0] / 10.0 + s.current_in_a = struct.unpack('>h', data[4:6])[0] / 10.0 + + elif packet_type == CAN_PACKET_STATUS_5 and len(data) >= 2: + s.voltage_v = struct.unpack('>h', data[0:2])[0] / 10.0 + + # ------------------------------------------------------------------ + # CAN send helpers # ------------------------------------------------------------------ def _send_duty(self, vesc_id: int, duty: float): """Send CAN_PACKET_SET_DUTY to a VESC controller.""" - can_id = make_can_id(CAN_PACKET_SET_DUTY, vesc_id) + can_id = make_can_id(CAN_PACKET_SET_DUTY, vesc_id) payload = struct.pack('>i', int(duty * 100000)) msg = can.Message( arbitration_id=can_id, @@ -111,7 +249,9 @@ class VescCanDriver(Node): self.get_logger().error(f'CAN send error (id={vesc_id}): {e}') def destroy_node(self): - self._send_duty(self.left_id, 0.0) + self._running = False + self._rx_thread.join(timeout=1.0) + self._send_duty(self.left_id, 0.0) self._send_duty(self.right_id, 0.0) self.bus.shutdown() super().destroy_node() -- 2.47.2 From 9ed678ca35d1c03cdddeb1862561a90a10a2f99b Mon Sep 17 00:00:00 2001 From: sl-firmware Date: Tue, 17 Mar 2026 22:49:21 -0400 Subject: [PATCH 2/2] feat: IMU mount angle cal, CAN telemetry, LED override (Issues #680, #672, #685) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Issue #680 — IMU mount angle calibration: - imu_cal_flash.h/.c: store pitch/roll offsets in flash sector 7 (0x0807FF00, 64 bytes; preserves PID records across sector erase) - mpu6000_set_mount_offset(): subtracts offsets from pitch/roll output - mpu6000_has_mount_offset(): reports cal_status=2 to Orin - 'O' CDC command: capture current pitch/roll → save to flash → ACK JSON - Load offsets on boot; report in printf log CAN telemetry correction (Tee: production has no USB to Orin): - FC_IMU (0x402): pitch/roll/yaw/cal_status/balance_state at 50 Hz - orin_can_broadcast_imu() rate-limited to ORIN_IMU_TLM_HZ (50 Hz) - FC_BARO (0x403): pressure_pa/temp_x10/alt_cm at 1 Hz (Issue #672) - orin_can_broadcast_baro() rate-limited to ORIN_BARO_TLM_HZ (1 Hz) Issue #685 — LED CAN override: - ORIN_CAN_ID_LED_CMD (0x304): pattern/brightness/duration_ms from Orin - orin_can_led_override volatile state + orin_can_led_updated flag - main.c: apply pattern to LED state machine on each LED_CMD received Orin side: - saltybot_can_node.py: production SocketCAN bridge — reads 0x400-0x403, publishes /saltybot/imu, /saltybot/balance_state, /saltybot/barometer; subscribes /cmd_vel → 0x301 DRIVE; /saltybot/leds → 0x304 LED_CMD; sends 0x300 HEARTBEAT at 5 Hz; sends 0x303 ESTOP on shutdown - setup.py: register saltybot_can_node entry point + uart_bridge launch Fix: re-apply --defsym __stack_end=_estack-0x1000 linker fix to branch Co-Authored-By: Claude Sonnet 4.6 --- include/imu_cal_flash.h | 51 +++ include/mpu6000.h | 11 + include/orin_can.h | 55 ++- .../saltybot_bridge/saltybot_can_node.py | 402 ++++++++++++++++++ jetson/ros2_ws/src/saltybot_bridge/setup.py | 3 + lib/USB_CDC/src/usbd_cdc_if.c | 2 + platformio.ini | 1 + src/imu_cal_flash.c | 100 +++++ src/main.c | 71 ++++ src/mpu6000.c | 19 +- src/orin_can.c | 54 ++- 11 files changed, 760 insertions(+), 9 deletions(-) create mode 100644 include/imu_cal_flash.h create mode 100644 jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/saltybot_can_node.py create mode 100644 src/imu_cal_flash.c diff --git a/include/imu_cal_flash.h b/include/imu_cal_flash.h new file mode 100644 index 0000000..d0e6658 --- /dev/null +++ b/include/imu_cal_flash.h @@ -0,0 +1,51 @@ +#ifndef IMU_CAL_FLASH_H +#define IMU_CAL_FLASH_H + +#include +#include + +/* + * IMU mount angle calibration flash storage (Issue #680). + * + * Sector 7 (128 KB at 0x08060000) layout: + * 0x0807FF00 imu_cal_flash_t (64 bytes) ← this module + * 0x0807FF40 pid_sched_flash_t (128 bytes) ← pid_flash.c + * 0x0807FFC0 pid_flash_t (64 bytes) ← pid_flash.c + * + * Calibration flow: + * 1. Mount robot at its installed angle, power on, let IMU converge (~5s). + * 2. Send 'O' via USB CDC (dev-only path). + * 3. Firmware captures current pitch + roll as mount offsets, saves to flash. + * 4. mpu6000_read() subtracts offsets from output on every subsequent read. + * + * The sector erase preserves existing PID data by reading it first. + */ + +#define IMU_CAL_FLASH_ADDR 0x0807FF00UL +#define IMU_CAL_FLASH_MAGIC 0x534C5403UL /* 'SLT\x03' — version 3 */ + +typedef struct __attribute__((packed)) { + uint32_t magic; /* IMU_CAL_FLASH_MAGIC when valid */ + float pitch_offset; /* degrees subtracted from IMU pitch output */ + float roll_offset; /* degrees subtracted from IMU roll output */ + uint8_t _pad[52]; /* padding to 64 bytes */ +} imu_cal_flash_t; /* 64 bytes total */ + +/* + * imu_cal_flash_load() — read saved mount offsets from flash. + * Returns true and fills *pitch_offset / *roll_offset if magic is valid. + * Returns false if no valid calibration stored (caller keeps 0.0f defaults). + */ +bool imu_cal_flash_load(float *pitch_offset, float *roll_offset); + +/* + * imu_cal_flash_save() — erase sector 7 and write all three records atomically: + * imu_cal_flash_t at 0x0807FF00 + * pid_sched_flash_t at 0x0807FF40 (preserved from existing flash) + * pid_flash_t at 0x0807FFC0 (preserved from existing flash) + * Must be called while disarmed — sector erase stalls CPU ~1s. + * Returns true on success. + */ +bool imu_cal_flash_save(float pitch_offset, float roll_offset); + +#endif /* IMU_CAL_FLASH_H */ diff --git a/include/mpu6000.h b/include/mpu6000.h index d182037..43e7f78 100644 --- a/include/mpu6000.h +++ b/include/mpu6000.h @@ -29,4 +29,15 @@ bool mpu6000_is_calibrated(void); void mpu6000_read(IMUData *data); +/* + * mpu6000_set_mount_offset(pitch_deg, roll_deg) — set mount angle offsets. + * These are subtracted from the pitch and roll outputs in mpu6000_read(). + * Load via imu_cal_flash_load() on boot; update after 'O' CDC command. + * Issue #680. + */ +void mpu6000_set_mount_offset(float pitch_deg, float roll_deg); + +/* Returns true if non-zero mount offsets have been applied (Issue #680). */ +bool mpu6000_has_mount_offset(void); + #endif diff --git a/include/orin_can.h b/include/orin_can.h index 36c0c79..b3be600 100644 --- a/include/orin_can.h +++ b/include/orin_can.h @@ -31,14 +31,19 @@ #define ORIN_CAN_ID_DRIVE 0x301u #define ORIN_CAN_ID_MODE 0x302u #define ORIN_CAN_ID_ESTOP 0x303u +#define ORIN_CAN_ID_LED_CMD 0x304u /* LED pattern override (Issue #685) */ /* ---- FC → Orin telemetry IDs ---- */ -#define ORIN_CAN_ID_FC_STATUS 0x400u -#define ORIN_CAN_ID_FC_VESC 0x401u +#define ORIN_CAN_ID_FC_STATUS 0x400u /* balance state + pitch + vbat at 10 Hz */ +#define ORIN_CAN_ID_FC_VESC 0x401u /* VESC RPM + current at 10 Hz */ +#define ORIN_CAN_ID_FC_IMU 0x402u /* full IMU angles + cal status at 50 Hz (Issue #680) */ +#define ORIN_CAN_ID_FC_BARO 0x403u /* barometer pressure/temp/altitude at 1 Hz (Issue #672) */ /* ---- 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) */ +#define ORIN_TLM_HZ 10u /* FC_STATUS + FC_VESC broadcast rate (Hz) */ +#define ORIN_IMU_TLM_HZ 50u /* FC_IMU broadcast rate (Hz) */ +#define ORIN_BARO_TLM_HZ 1u /* FC_BARO broadcast rate (Hz) */ /* ---- Volatile state updated by orin_can_on_frame(), read by main loop ---- */ typedef struct { @@ -71,6 +76,34 @@ typedef struct __attribute__((packed)) { int16_t right_current_x10; /* right phase current × 10 (A) */ } orin_can_fc_vesc_t; /* 8 bytes */ +/* FC_IMU (0x402) — full IMU angles at 50 Hz (Issue #680) */ +typedef struct __attribute__((packed)) { + int16_t pitch_x10; /* pitch degrees × 10 (mount-offset corrected) */ + int16_t roll_x10; /* roll degrees × 10 (mount-offset corrected) */ + int16_t yaw_x10; /* yaw degrees × 10 (gyro-integrated, drifts) */ + uint8_t cal_status; /* 0=uncal, 1=gyro_cal, 2=gyro+mount_cal */ + uint8_t balance_state; /* BalanceState: 0=DISARMED, 1=ARMED, 2=TILT_FAULT */ +} orin_can_fc_imu_t; /* 8 bytes */ + +/* FC_BARO (0x403) — barometer at 1 Hz (Issue #672) */ +typedef struct __attribute__((packed)) { + int32_t pressure_pa; /* barometric pressure in Pa */ + int16_t temp_x10; /* temperature × 10 (°C) */ + int16_t alt_cm; /* altitude in cm (reference = pressure at boot) */ +} orin_can_fc_baro_t; /* 8 bytes */ + +/* LED_CMD (0x304) — Orin → FC LED pattern override (Issue #685) + * duration_ms = 0: hold until next state change; >0: revert after duration */ +typedef struct __attribute__((packed)) { + uint8_t pattern; /* 0=state_auto, 1=solid, 2=slow_blink, 3=fast_blink, 4=pulse */ + uint8_t brightness; /* 0-255 (0 = both LEDs off) */ + uint16_t duration_ms; /* override duration; 0 = permanent until state change */ +} orin_can_led_cmd_t; /* 4 bytes */ + +/* LED override state (updated by orin_can_on_frame, read by main loop) */ +extern volatile orin_can_led_cmd_t orin_can_led_override; +extern volatile uint8_t orin_can_led_updated; + /* ---- API ---- */ /* @@ -100,4 +133,20 @@ void orin_can_broadcast(uint32_t now_ms, const orin_can_fc_status_t *status, const orin_can_fc_vesc_t *vesc); +/* + * orin_can_broadcast_imu(now_ms, imu_tlm) — rate-limited broadcast of + * FC_IMU (0x402) at ORIN_IMU_TLM_HZ (50 Hz). + * Safe to call every ms; internally rate-limited. Issue #680. + */ +void orin_can_broadcast_imu(uint32_t now_ms, + const orin_can_fc_imu_t *imu_tlm); + +/* + * orin_can_broadcast_baro(now_ms, baro_tlm) — rate-limited broadcast of + * FC_BARO (0x403) at ORIN_BARO_TLM_HZ (1 Hz). + * Pass NULL to skip transmission. Issue #672. + */ +void orin_can_broadcast_baro(uint32_t now_ms, + const orin_can_fc_baro_t *baro_tlm); + #endif /* ORIN_CAN_H */ diff --git a/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/saltybot_can_node.py b/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/saltybot_can_node.py new file mode 100644 index 0000000..f0a4ee8 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/saltybot_can_node.py @@ -0,0 +1,402 @@ +""" +saltybot_can_node — production FC↔Orin bridge over CAN bus (Issues #680, #672, #685) + +In production the FC has NO USB connection to Orin — CAN bus only. +Reads IMU, balance state, barometer, and VESC telemetry from FC over CAN +and publishes them as ROS2 topics. Sends drive/heartbeat/estop commands +back to FC over CAN. + +CAN interface: SocketCAN (CANable USB adapter on vcan0 / can0) + +FC → Orin (telemetry): + 0x400 FC_STATUS int16 pitch_x10, int16 motor_cmd, uint16 vbat_mv, + uint8 balance_state, uint8 flags (10 Hz) + 0x401 FC_VESC int16 left_rpm_x10, int16 right_rpm_x10, + int16 left_cur_x10, int16 right_cur_x10 (10 Hz) + 0x402 FC_IMU int16 pitch_x10, int16 roll_x10, int16 yaw_x10, + uint8 cal_status, uint8 balance_state (50 Hz) + 0x403 FC_BARO int32 pressure_pa, int16 temp_x10, int16 alt_cm (1 Hz) + +Orin → FC (commands): + 0x300 HEARTBEAT uint32 sequence counter (5 Hz) + 0x301 DRIVE int16 speed BE, int16 steer BE (on /cmd_vel) + 0x303 ESTOP uint8 1=estop, 0=clear + 0x304 LED_CMD uint8 pattern, uint8 brightness, uint16 duration_ms LE + +Published topics: + /saltybot/imu (sensor_msgs/Imu) — from 0x402 + /saltybot/balance_state (std_msgs/String) — from 0x402 + 0x400 + /saltybot/barometer (sensor_msgs/FluidPressure) — from 0x403 + +Subscribed topics: + /cmd_vel (geometry_msgs/Twist) — sent as DRIVE + /saltybot/leds (std_msgs/String JSON) — sent as LED_CMD + +Parameters: + can_interface CAN socket interface name default: can0 + speed_scale m/s to ESC units multiplier default: 1000.0 + steer_scale rad/s to ESC units multiplier default: -500.0 + heartbeat_hz HEARTBEAT TX rate (Hz) default: 5.0 +""" + +import json +import math +import socket +import struct +import threading +import time + +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy + +from geometry_msgs.msg import Twist +from sensor_msgs.msg import Imu, FluidPressure +from std_msgs.msg import String +from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue + +# ---- CAN frame IDs ------------------------------------------------ + +CAN_FC_STATUS = 0x400 +CAN_FC_VESC = 0x401 +CAN_FC_IMU = 0x402 +CAN_FC_BARO = 0x403 + +CAN_HEARTBEAT = 0x300 +CAN_DRIVE = 0x301 +CAN_ESTOP = 0x303 +CAN_LED_CMD = 0x304 + +# ---- Constants ---------------------------------------------------- + +IMU_FRAME_ID = "imu_link" +_STATE_LABEL = {0: "DISARMED", 1: "ARMED", 2: "TILT_FAULT"} +_CAL_LABEL = {0: "UNCAL", 1: "GYRO_CAL", 2: "MOUNT_CAL"} + +# ---- Helpers ------------------------------------------------------ + +def _clamp(v: float, lo: float, hi: float) -> float: + return max(lo, min(hi, v)) + + +def _can_socket(iface: str) -> socket.socket: + """Open a raw SocketCAN socket on *iface*.""" + s = socket.socket(socket.AF_CAN, socket.SOCK_RAW, socket.CAN_RAW) + s.bind((iface,)) + s.settimeout(0.1) + return s + + +def _pack_frame(can_id: int, data: bytes) -> bytes: + """Pack a standard CAN frame for SocketCAN (16-byte struct).""" + dlc = len(data) + return struct.pack("=IB3x8s", can_id & 0x1FFFFFFF, dlc, + data.ljust(8, b"\x00")) + + +def _unpack_frame(raw: bytes): + """Unpack a raw SocketCAN frame; returns (can_id, data_bytes).""" + can_id, dlc = struct.unpack_from("=IB", raw, 0) + data = raw[8: 8 + (dlc & 0x0F)] + return can_id & 0x1FFFFFFF, data + + +# ---- Node --------------------------------------------------------- + +class SaltybotCanNode(Node): + def __init__(self): + super().__init__("saltybot_can") + + # ── Parameters ─────────────────────────────────────────────── + self.declare_parameter("can_interface", "can0") + self.declare_parameter("speed_scale", 1000.0) + self.declare_parameter("steer_scale", -500.0) + self.declare_parameter("heartbeat_hz", 5.0) + + iface = self.get_parameter("can_interface").value + self._speed_sc = self.get_parameter("speed_scale").value + self._steer_sc = self.get_parameter("steer_scale").value + hb_hz = self.get_parameter("heartbeat_hz").value + + # ── QoS ────────────────────────────────────────────────────── + sensor_qos = QoSProfile( + reliability=ReliabilityPolicy.BEST_EFFORT, + history=HistoryPolicy.KEEP_LAST, depth=10) + reliable_qos = QoSProfile( + reliability=ReliabilityPolicy.RELIABLE, + history=HistoryPolicy.KEEP_LAST, depth=10) + + # ── Publishers ─────────────────────────────────────────────── + self._imu_pub = self.create_publisher(Imu, "/saltybot/imu", sensor_qos) + self._bal_pub = self.create_publisher(String, "/saltybot/balance_state", reliable_qos) + self._baro_pub = self.create_publisher(FluidPressure,"/saltybot/barometer", reliable_qos) + self._diag_pub = self.create_publisher(DiagnosticArray, "/diagnostics", reliable_qos) + + # ── Subscribers ────────────────────────────────────────────── + self._cmd_sub = self.create_subscription( + Twist, "/cmd_vel", self._cmd_vel_cb, reliable_qos) + self._led_sub = self.create_subscription( + String, "/saltybot/leds", self._led_cb, reliable_qos) + + # ── State ──────────────────────────────────────────────────── + self._iface = iface + self._sock = None + self._sock_lock = threading.Lock() + self._hb_seq = 0 + self._last_speed = 0 + self._last_steer = 0 + self._last_state = -1 + self._last_cal = -1 + self._last_pitch = 0.0 + self._last_roll = 0.0 + self._last_yaw = 0.0 + self._last_motor = 0 + self._last_vbat = 0 + self._last_flags = 0 + + # ── Open CAN ───────────────────────────────────────────────── + self._open_can() + + # ── Timers ─────────────────────────────────────────────────── + self._rx_timer = self.create_timer(0.01, self._rx_cb) # 100 Hz poll + self._hb_timer = self.create_timer(1.0 / hb_hz, self._hb_cb) + + self.get_logger().info( + f"saltybot_can started — interface={iface} " + f"(speed_scale={self._speed_sc}, steer_scale={self._steer_sc})" + ) + + # ── CAN socket management ──────────────────────────────────────── + + def _open_can(self) -> bool: + with self._sock_lock: + try: + self._sock = _can_socket(self._iface) + self.get_logger().info(f"CAN socket open: {self._iface}") + return True + except OSError as exc: + self.get_logger().error(f"Cannot open CAN {self._iface}: {exc}") + self._sock = None + return False + + def _send(self, can_id: int, data: bytes) -> bool: + with self._sock_lock: + if self._sock is None: + return False + try: + self._sock.send(_pack_frame(can_id, data)) + return True + except OSError as exc: + self.get_logger().error( + f"CAN TX error: {exc}", throttle_duration_sec=2.0) + return False + + # ── RX poll ────────────────────────────────────────────────────── + + def _rx_cb(self): + with self._sock_lock: + if self._sock is None: + return + try: + while True: + raw = self._sock.recv(16) + can_id, data = _unpack_frame(raw) + self._dispatch(can_id, data) + except socket.timeout: + pass + except BlockingIOError: + pass + except OSError as exc: + self.get_logger().error( + f"CAN RX error: {exc}", throttle_duration_sec=5.0) + self._sock = None + + if self._sock is None: + self._open_can() + + def _dispatch(self, can_id: int, data: bytes): + now = self.get_clock().now().to_msg() + if can_id == CAN_FC_IMU and len(data) >= 8: + self._handle_fc_imu(data, now) + elif can_id == CAN_FC_STATUS and len(data) >= 8: + self._handle_fc_status(data) + elif can_id == CAN_FC_BARO and len(data) >= 8: + self._handle_fc_baro(data, now) + + # ── Frame handlers ─────────────────────────────────────────────── + + def _handle_fc_imu(self, data: bytes, stamp): + pitch_x10, roll_x10, yaw_x10, cal, state = struct.unpack_from("I", self._hb_seq & 0xFFFFFFFF) + self._hb_seq += 1 + self._send(CAN_HEARTBEAT, data) + + def _cmd_vel_cb(self, msg: Twist): + """Convert Twist → DRIVE (0x301): int16 speed BE, int16 steer BE.""" + speed = int(_clamp(msg.linear.x * self._speed_sc, -1000.0, 1000.0)) + steer = int(_clamp(msg.angular.z * self._steer_sc, -1000.0, 1000.0)) + self._last_speed = speed + self._last_steer = steer + data = struct.pack(">hh", speed, steer) + self._send(CAN_DRIVE, data) + + def _led_cb(self, msg: String): + """Parse /saltybot/leds JSON → LED_CMD (0x304). + Expected JSON: {"pattern": 1, "brightness": 200, "duration_ms": 5000} + pattern: 0=auto, 1=solid, 2=slow_blink, 3=fast_blink, 4=pulse + """ + try: + d = json.loads(msg.data) + except (ValueError, TypeError) as exc: + self.get_logger().debug(f"LED JSON parse error: {exc}") + return + pattern = int(d.get("pattern", 0)) & 0xFF + brightness = int(d.get("brightness", 255)) & 0xFF + duration_ms = int(d.get("duration_ms", 0)) & 0xFFFF + data = struct.pack(" + +bool imu_cal_flash_load(float *pitch_offset, float *roll_offset) +{ + const imu_cal_flash_t *p = (const imu_cal_flash_t *)IMU_CAL_FLASH_ADDR; + + if (p->magic != IMU_CAL_FLASH_MAGIC) return false; + + /* Sanity-check: mount offsets beyond ±90° indicate a corrupt record */ + if (p->pitch_offset < -90.0f || p->pitch_offset > 90.0f) return false; + if (p->roll_offset < -90.0f || p->roll_offset > 90.0f) return false; + + *pitch_offset = p->pitch_offset; + *roll_offset = p->roll_offset; + return true; +} + +/* Write 'len' bytes (multiple of 4) from 'src' to flash at 'addr'. + * Flash must be unlocked by caller. */ +static HAL_StatusTypeDef write_words(uint32_t addr, + const void *src, + uint32_t len) +{ + const uint32_t *p = (const uint32_t *)src; + for (uint32_t i = 0; i < len / 4u; i++) { + HAL_StatusTypeDef rc = HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, + addr, p[i]); + if (rc != HAL_OK) return rc; + addr += 4u; + } + return HAL_OK; +} + +bool imu_cal_flash_save(float pitch_offset, float roll_offset) +{ + /* Snapshot PID records BEFORE erasing so we can restore them */ + pid_flash_t pid_snap; + pid_sched_flash_t sched_snap; + memcpy(&pid_snap, (const void *)PID_FLASH_STORE_ADDR, sizeof(pid_snap)); + memcpy(&sched_snap, (const void *)PID_SCHED_FLASH_ADDR, sizeof(sched_snap)); + + HAL_StatusTypeDef rc; + + rc = HAL_FLASH_Unlock(); + if (rc != HAL_OK) return false; + + /* Erase sector 7 (covers all three records) */ + FLASH_EraseInitTypeDef erase = { + .TypeErase = FLASH_TYPEERASE_SECTORS, + .Sector = PID_FLASH_SECTOR, + .NbSectors = 1, + .VoltageRange = PID_FLASH_SECTOR_VOLTAGE, + }; + uint32_t sector_error = 0; + rc = HAL_FLASHEx_Erase(&erase, §or_error); + if (rc != HAL_OK || sector_error != 0xFFFFFFFFUL) { + HAL_FLASH_Lock(); + return false; + } + + /* Write new IMU calibration record at 0x0807FF00 */ + imu_cal_flash_t cal; + memset(&cal, 0xFF, sizeof(cal)); + cal.magic = IMU_CAL_FLASH_MAGIC; + cal.pitch_offset = pitch_offset; + cal.roll_offset = roll_offset; + + rc = write_words(IMU_CAL_FLASH_ADDR, &cal, sizeof(cal)); + if (rc != HAL_OK) { HAL_FLASH_Lock(); return false; } + + /* Restore PID gain schedule if it was valid */ + if (sched_snap.magic == PID_SCHED_MAGIC) { + rc = write_words(PID_SCHED_FLASH_ADDR, &sched_snap, sizeof(sched_snap)); + if (rc != HAL_OK) { HAL_FLASH_Lock(); return false; } + } + + /* Restore single-PID record if it was valid */ + if (pid_snap.magic == PID_FLASH_MAGIC) { + rc = write_words(PID_FLASH_STORE_ADDR, &pid_snap, sizeof(pid_snap)); + if (rc != HAL_OK) { HAL_FLASH_Lock(); return false; } + } + + HAL_FLASH_Lock(); + + /* Verify readback */ + const imu_cal_flash_t *v = (const imu_cal_flash_t *)IMU_CAL_FLASH_ADDR; + return (v->magic == IMU_CAL_FLASH_MAGIC && + v->pitch_offset == pitch_offset && + v->roll_offset == roll_offset); +} diff --git a/src/main.c b/src/main.c index 97e7c7d..8e3f6a3 100644 --- a/src/main.c +++ b/src/main.c @@ -35,6 +35,7 @@ #include "can_driver.h" #include "vesc_can.h" #include "orin_can.h" +#include "imu_cal_flash.h" #include "servo_bus.h" #include "gimbal.h" #include "lvc.h" @@ -50,6 +51,7 @@ extern volatile uint8_t cdc_recal_request; /* set by G command */ extern volatile uint32_t cdc_rx_count; /* total CDC packets received */ extern volatile uint8_t cdc_estop_request; extern volatile uint8_t cdc_estop_clear_request; +extern volatile uint8_t cdc_imu_cal_request; /* set by O command — mount cal (Issue #680) */ /* Direct motor test (set by W command in jetson_uart.c) */ volatile int16_t direct_test_speed = 0; @@ -168,6 +170,16 @@ int main(void) { */ if (imu_ret == 0) mpu6000_calibrate(); + /* Load IMU mount angle offsets from flash if previously calibrated (Issue #680) */ + { + float imu_pitch_off = 0.0f, imu_roll_off = 0.0f; + if (imu_cal_flash_load(&imu_pitch_off, &imu_roll_off)) { + mpu6000_set_mount_offset(imu_pitch_off, imu_roll_off); + printf("[IMU_CAL] Mount offsets loaded: pitch=%.1f° roll=%.1f°\n", + (double)imu_pitch_off, (double)imu_roll_off); + } + } + /* Init hoverboard ESC UART */ hoverboard_init(); @@ -323,6 +335,20 @@ int main(void) { /* Advance buzzer pattern sequencer (non-blocking, call every tick) */ buzzer_tick(now); + /* Orin LED override (Issue #685): apply pattern from CAN 0x304 if updated. + * Safety states (estop, tilt fault) are applied later and always win. */ + if (orin_can_led_updated) { + orin_can_led_updated = 0u; + /* pattern: 0=auto, 1=solid-green, 2=slow-blink, 3=fast-blink, 4=pulse */ + switch (orin_can_led_override.pattern) { + case 1u: led_set_state(LED_STATE_ARMED); break; + case 2u: led_set_state(LED_STATE_LOW_BATT); break; /* slow blink */ + case 3u: led_set_state(LED_STATE_ERROR); break; /* fast blink */ + case 4u: led_set_state(LED_STATE_CHARGING); break; /* pulse */ + default: break; /* 0=auto — let state machine below decide */ + } + } + /* Advance LED animation sequencer (non-blocking, call every tick) */ led_tick(now); @@ -625,6 +651,24 @@ int main(void) { if (imu_ret == 0) mpu6000_calibrate(); } + /* IMU mount angle calibration (Issue #680): capture current pitch/roll as + * mount offsets and save to flash. Disarmed only — flash erase stalls ~1s. */ + if (cdc_imu_cal_request && bal.state != BALANCE_ARMED) { + cdc_imu_cal_request = 0; + float off_p = bal.pitch_deg; + float off_r = imu.roll; + mpu6000_set_mount_offset(off_p, off_r); + bool cal_ok = imu_cal_flash_save(off_p, off_r); + char reply[64]; + snprintf(reply, sizeof(reply), + "{\"imu_cal\":%s,\"pitch_off\":%d,\"roll_off\":%d}\n", + cal_ok ? "ok" : "fail", + (int)(off_p * 10), (int)(off_r * 10)); + CDC_Transmit((uint8_t *)reply, strlen(reply)); + printf("[IMU_CAL] Mount offset saved: pitch=%.1f° roll=%.1f° (%s)\n", + (double)off_p, (double)off_r, cal_ok ? "OK" : "FAIL"); + } + /* Handle PID tuning commands from USB (P/I/D/T/M/?) */ if (cdc_cmd_ready) { cdc_cmd_ready = 0; @@ -759,6 +803,33 @@ int main(void) { orin_can_broadcast(now, &fc_st, &fc_vesc); } + /* FC_IMU (0x402): full IMU angles + calibration status at 50 Hz (Issue #680) */ + { + orin_can_fc_imu_t fc_imu; + fc_imu.pitch_x10 = (int16_t)(bal.pitch_deg * 10.0f); + fc_imu.roll_x10 = (int16_t)(imu.roll * 10.0f); + fc_imu.yaw_x10 = (int16_t)(imu.yaw * 10.0f); + /* cal_status: 0=uncal, 1=gyro_cal, 2=gyro+mount_cal */ + if (!mpu6000_is_calibrated()) fc_imu.cal_status = 0u; + else if (mpu6000_has_mount_offset()) fc_imu.cal_status = 2u; + else fc_imu.cal_status = 1u; + fc_imu.balance_state = (uint8_t)bal.state; + orin_can_broadcast_imu(now, &fc_imu); + } + + /* FC_BARO (0x403): barometer at 1 Hz (Issue #672) */ + if (baro_chip) { + int32_t _pres_pa; int16_t _temp_x10; + bmp280_read(&_pres_pa, &_temp_x10); + int32_t _alt_cm = bmp280_pressure_to_alt_cm(_pres_pa); + orin_can_fc_baro_t fc_baro; + fc_baro.pressure_pa = _pres_pa; + fc_baro.temp_x10 = _temp_x10; + fc_baro.alt_cm = (_alt_cm > 32767) ? 32767 : + (_alt_cm < -32768) ? -32768 : (int16_t)_alt_cm; + orin_can_broadcast_baro(now, &fc_baro); + } + /* CRSF telemetry uplink — battery voltage + arm state at 1 Hz. * Sends battery sensor frame (0x08) and flight mode frame (0x21) * back to ELRS TX module so the pilot's handset OSD shows Vbat + state. */ diff --git a/src/mpu6000.c b/src/mpu6000.c index 831907f..e89bf6c 100644 --- a/src/mpu6000.c +++ b/src/mpu6000.c @@ -39,6 +39,10 @@ static float s_bias_gy = 0.0f; static float s_bias_gz = 0.0f; static bool s_calibrated = false; +/* Mount angle offsets (degrees, Issue #680) — set via mpu6000_set_mount_offset() */ +static float s_pitch_offset = 0.0f; +static float s_roll_offset = 0.0f; + bool mpu6000_init(void) { int ret = icm42688_init(); if (ret == 0) { @@ -91,6 +95,17 @@ bool mpu6000_is_calibrated(void) { return s_calibrated; } +void mpu6000_set_mount_offset(float pitch_deg, float roll_deg) +{ + s_pitch_offset = pitch_deg; + s_roll_offset = roll_deg; +} + +bool mpu6000_has_mount_offset(void) +{ + return (s_pitch_offset != 0.0f || s_roll_offset != 0.0f); +} + void mpu6000_read(IMUData *data) { icm42688_data_t raw; icm42688_read(&raw); @@ -147,9 +162,9 @@ void mpu6000_read(IMUData *data) { /* Yaw: pure gyro integration — no accel correction, drifts over time */ s_yaw += gyro_yaw_rate * dt; - data->pitch = s_pitch; + data->pitch = s_pitch - s_pitch_offset; data->pitch_rate = gyro_pitch_rate; - data->roll = s_roll; + data->roll = s_roll - s_roll_offset; data->yaw = s_yaw; data->yaw_rate = gyro_yaw_rate; /* board_gz: raw bias-corrected gyro Z (Issue #616) */ data->accel_x = ax; diff --git a/src/orin_can.c b/src/orin_can.c index ced4f98..f84031e 100644 --- a/src/orin_can.c +++ b/src/orin_can.c @@ -16,14 +16,23 @@ #include "stm32f7xx_hal.h" #include -volatile OrinCanState orin_can_state; +volatile OrinCanState orin_can_state; +volatile orin_can_led_cmd_t orin_can_led_override; +volatile uint8_t orin_can_led_updated; + static uint32_t s_tlm_tick; +static uint32_t s_imu_tick; +static uint32_t s_baro_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)); + memset((void *)&orin_can_state, 0, sizeof(orin_can_state)); + memset((void *)&orin_can_led_override, 0, sizeof(orin_can_led_override)); + orin_can_led_updated = 0u; + /* Pre-wind so first broadcasts fire on the first eligible tick */ + s_tlm_tick = (uint32_t)(-(uint32_t)(1000u / ORIN_TLM_HZ)); + s_imu_tick = (uint32_t)(-(uint32_t)(1000u / ORIN_IMU_TLM_HZ)); + s_baro_tick = (uint32_t)(-(uint32_t)(1000u / ORIN_BARO_TLM_HZ)); can_driver_set_std_cb(orin_can_on_frame); } @@ -62,6 +71,16 @@ void orin_can_on_frame(uint16_t std_id, const uint8_t *data, uint8_t len) } break; + case ORIN_CAN_ID_LED_CMD: + /* pattern(u8), brightness(u8), duration_ms(u16 LE) — Issue #685 */ + if (len < 4u) { break; } + orin_can_led_override.pattern = data[0]; + orin_can_led_override.brightness = data[1]; + orin_can_led_override.duration_ms = (uint16_t)((uint16_t)data[2] | + ((uint16_t)data[3] << 8u)); + orin_can_led_updated = 1u; + break; + default: break; } @@ -94,3 +113,30 @@ void orin_can_broadcast(uint32_t now_ms, 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)); } + +void orin_can_broadcast_imu(uint32_t now_ms, + const orin_can_fc_imu_t *imu_tlm) +{ + if ((now_ms - s_imu_tick) < (1000u / ORIN_IMU_TLM_HZ)) { + return; + } + s_imu_tick = now_ms; + + uint8_t buf[8]; + memcpy(buf, imu_tlm, sizeof(orin_can_fc_imu_t)); + can_driver_send_std(ORIN_CAN_ID_FC_IMU, buf, (uint8_t)sizeof(orin_can_fc_imu_t)); +} + +void orin_can_broadcast_baro(uint32_t now_ms, + const orin_can_fc_baro_t *baro_tlm) +{ + if (baro_tlm == NULL) return; + if ((now_ms - s_baro_tick) < (1000u / ORIN_BARO_TLM_HZ)) { + return; + } + s_baro_tick = now_ms; + + uint8_t buf[8]; + memcpy(buf, baro_tlm, sizeof(orin_can_fc_baro_t)); + can_driver_send_std(ORIN_CAN_ID_FC_BARO, buf, (uint8_t)sizeof(orin_can_fc_baro_t)); +} -- 2.47.2