include/imu_cal_flash.h

@@ -0,0 +1,51 @@
+#ifndef IMU_CAL_FLASH_H
+#define IMU_CAL_FLASH_H
+
+#include <stdint.h>
+#include <stdbool.h>
+
+/*
+ * 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 */

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

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_STATUS   0x400u  /* balance state + pitch + vbat at 10 Hz */
-#define ORIN_CAN_ID_FC_VESC     0x401u
+#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 */

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("<hhhBB", data, 0)
+        pitch = pitch_x10 / 10.0
+        roll  = roll_x10  / 10.0
+        yaw   = yaw_x10   / 10.0
+
+        self._last_pitch = pitch
+        self._last_roll  = roll
+        self._last_yaw   = yaw
+        self._last_cal   = cal
+
+        # Publish IMU
+        imu = Imu()
+        imu.header.stamp    = stamp
+        imu.header.frame_id = IMU_FRAME_ID
+        imu.orientation_covariance[0] = -1.0   # orientation not provided
+
+        # Publish Euler angles via angular_velocity fields (convention matches
+        # saltybot_cmd_node — downstream nodes expect this mapping)
+        imu.angular_velocity.x = math.radians(pitch)
+        imu.angular_velocity.y = math.radians(roll)
+        imu.angular_velocity.z = math.radians(yaw)
+        cov = math.radians(0.5) ** 2
+        imu.angular_velocity_covariance[0] = cov
+        imu.angular_velocity_covariance[4] = cov
+        imu.angular_velocity_covariance[8] = cov
+        imu.linear_acceleration_covariance[0] = -1.0
+        self._imu_pub.publish(imu)
+
+        # Publish balance_state JSON
+        self._publish_balance_state(pitch, roll, yaw, state, cal, stamp)
+
+        # Log state/cal transitions
+        if state != self._last_state:
+            self.get_logger().info(
+                f"Balance state → {_STATE_LABEL.get(state, f'UNKNOWN({state})')}"
+            )
+            self._last_state = state
+        if cal != self._last_cal:
+            self.get_logger().info(
+                f"IMU cal status → {_CAL_LABEL.get(cal, f'CAL({cal})')}"
+            )
+
+    def _handle_fc_status(self, data: bytes):
+        pitch_x10, motor_cmd, vbat_mv, balance_state, flags = \
+            struct.unpack_from("<hhhBB", data, 0)
+        self._last_motor = motor_cmd
+        self._last_vbat  = vbat_mv
+        self._last_flags = flags
+
+    def _handle_fc_baro(self, data: bytes, stamp):
+        pressure_pa, temp_x10, alt_cm = struct.unpack_from("<ihh", data, 0)
+        msg = FluidPressure()
+        msg.header.stamp    = stamp
+        msg.header.frame_id = "barometer_link"
+        msg.fluid_pressure  = float(pressure_pa)      # Pa
+        msg.variance        = 0.0
+        self._baro_pub.publish(msg)
+
+        diag = DiagnosticArray()
+        diag.header.stamp = stamp
+        st = DiagnosticStatus()
+        st.level       = DiagnosticStatus.OK
+        st.name        = "saltybot/barometer"
+        st.hardware_id = "bmp280"
+        st.message     = "OK"
+        st.values = [
+            KeyValue(key="pressure_pa", value=str(pressure_pa)),
+            KeyValue(key="temp_c",      value=f"{temp_x10 / 10.0:.1f}"),
+            KeyValue(key="alt_cm",      value=str(alt_cm)),
+        ]
+        diag.status.append(st)
+        self._diag_pub.publish(diag)
+
+    def _publish_balance_state(self, pitch, roll, yaw, state, cal, stamp):
+        state_label = _STATE_LABEL.get(state, f"UNKNOWN({state})")
+        cal_label   = _CAL_LABEL.get(cal, f"CAL({cal})")
+        payload = {
+            "stamp":        f"{stamp.sec}.{stamp.nanosec:09d}",
+            "state":        state_label,
+            "cal_status":   cal_label,
+            "pitch_deg":    round(pitch, 1),
+            "roll_deg":     round(roll,  1),
+            "yaw_deg":      round(yaw,   1),
+            "motor_cmd":    self._last_motor,
+            "vbat_mv":      self._last_vbat,
+            "jetson_speed": self._last_speed,
+            "jetson_steer": self._last_steer,
+        }
+        str_msg = String()
+        str_msg.data = json.dumps(payload)
+        self._bal_pub.publish(str_msg)
+
+        diag = DiagnosticArray()
+        diag.header.stamp = stamp
+        st = DiagnosticStatus()
+        st.name        = "saltybot/balance_controller"
+        st.hardware_id = "stm32f722"
+        st.message     = state_label
+        st.level       = (DiagnosticStatus.OK    if state == 1 else
+                          DiagnosticStatus.WARN   if state == 0 else
+                          DiagnosticStatus.ERROR)
+        st.values = [
+            KeyValue(key="pitch_deg",  value=f"{pitch:.1f}"),
+            KeyValue(key="roll_deg",   value=f"{roll:.1f}"),
+            KeyValue(key="yaw_deg",    value=f"{yaw:.1f}"),
+            KeyValue(key="motor_cmd",  value=str(self._last_motor)),
+            KeyValue(key="state",      value=state_label),
+            KeyValue(key="cal_status", value=cal_label),
+        ]
+        diag.status.append(st)
+        self._diag_pub.publish(diag)
+
+    # ── TX callbacks ─────────────────────────────────────────────────
+
+    def _hb_cb(self):
+        """Send HEARTBEAT (0x300) with incrementing sequence counter."""
+        data = struct.pack(">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("<BBH", pattern, brightness, duration_ms)
+        self._send(CAN_LED_CMD, data)
+
+    # ── Lifecycle ────────────────────────────────────────────────────
+
+    def destroy_node(self):
+        # Send ESTOP on shutdown so FC doesn't keep rolling
+        self._send(CAN_ESTOP, bytes([1]))
+        with self._sock_lock:
+            if self._sock:
+                try:
+                    self._sock.close()
+                except OSError:
+                    pass
+                self._sock = None
+        super().destroy_node()
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = SaltybotCanNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

jetson/ros2_ws/src/saltybot_bridge/setup.py

@@ -15,6 +15,7 @@ setup(
             "launch/remote_estop.launch.py",
             "launch/stm32_cmd.launch.py",
             "launch/battery.launch.py",
+            "launch/uart_bridge.launch.py",
         ]),
         (f"share/{package_name}/config", [
             "config/bridge_params.yaml",
@@ -44,6 +45,8 @@ setup(
             "stm32_cmd_node    = saltybot_bridge.stm32_cmd_node:main",
             # Battery management node (Issue #125)
             "battery_node      = saltybot_bridge.battery_node:main",
+            # Production CAN bridge: FC telemetry RX + /cmd_vel TX over CAN (Issues #680, #672, #685)
+            "saltybot_can_node = saltybot_bridge.saltybot_can_node:main",
         ],
     },
 )

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
+# ── 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,6 +101,7 @@ 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
@@ -48,6 +110,7 @@ class VescCanDriver(Node):
         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,17 +125,36 @@ 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)
+        # Watchdog + telemetry publish timer
-        self.create_timer(0.1, self._watchdog_cb)
+        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'telemetry={tel_hz} Hz'
         )
 
     # ------------------------------------------------------------------
+    # Command velocity callback
+    # ------------------------------------------------------------------
 
     def _cmd_vel_cb(self, msg: Twist):
         self._last_cmd_time = time.monotonic()
@@ -88,12 +170,68 @@ class VescCanDriver(Node):
         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.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):
@@ -111,6 +249,8 @@ class VescCanDriver(Node):
             self.get_logger().error(f'CAN send error (id={vesc_id}): {e}')
 
     def destroy_node(self):
+        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()

lib/USB_CDC/src/usbd_cdc_if.c

@@ -8,6 +8,7 @@ static volatile uint8_t cdc_port_open = 0;  /* set when host asserts DTR */
 volatile uint8_t  cdc_arm_request = 0;    /* set by A command */
 volatile uint8_t  cdc_disarm_request = 0; /* set by D command */
 volatile uint8_t  cdc_recal_request = 0;  /* set by G command — gyro recalibration */
+volatile uint8_t  cdc_imu_cal_request = 0; /* set by O command — mount offset calibration (Issue #680) */
 volatile uint32_t cdc_rx_count = 0;       /* total CDC packets received from host */
 
 volatile uint8_t  cdc_estop_request = 0;
@@ -143,6 +144,7 @@ static int8_t CDC_Receive(uint8_t *buf, uint32_t *len) {
         case 'A': cdc_arm_request = 1;            break;
         case 'D': cdc_disarm_request = 1;         break;
         case 'G': cdc_recal_request = 1;          break; /* gyro recalibration */
+        case 'O': cdc_imu_cal_request = 1;        break; /* mount offset cal (Issue #680) */
         case 'R': request_bootloader();            break; /* never returns */
 
         case 'E': cdc_estop_request = 1;       break;

platformio.ini

@@ -16,3 +16,4 @@ build_flags =
     -Os
     -Wl,--defsym,_Min_Heap_Size=0x2000
     -Wl,--defsym,_Min_Stack_Size=0x1000
+    -Wl,--defsym,__stack_end=_estack-0x1000

src/imu_cal_flash.c

@@ -0,0 +1,100 @@
+/* imu_cal_flash.c — IMU mount angle calibration flash storage (Issue #680)
+ *
+ * Stores pitch/roll mount offsets in STM32F722 flash sector 7 at 0x0807FF00.
+ * Preserves existing PID records (pid_sched_flash_t + pid_flash_t) across
+ * the mandatory sector erase by reading them into RAM before erasing.
+ */
+
+#include "imu_cal_flash.h"
+#include "pid_flash.h"
+#include "stm32f7xx_hal.h"
+#include <string.h>
+
+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, &sector_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);
+}

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. */

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;

src/orin_can.c

@@ -17,13 +17,22 @@
 #include <string.h>
 
 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 */
+    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));
+}