feat: PID tuning interface via CAN/ROS2 (Issue #693)

- Mamba (STM32): add ORIN_CAN_ID_PID_SET (0x305) handler in orin_can.c.
  Receives kp/ki/kd as uint16*100 (BE), applies to running balance loop,
  replies with FC_PID_ACK (0x405) echoing clamped gains. Gains persist in
  RAM until reboot; not saved to flash.
- Jetson: expose pid/kp, pid/ki, pid/kd as ROS2 parameters in
  can_bridge_node. Parameter changes trigger encode_pid_set_cmd() and
  send CAN frame 0x305 immediately. ACK frame 0x405 logged at DEBUG.
- mamba_protocol.py: add ORIN_CAN_ID_PID_SET / FC_PID_ACK IDs,
  PidGains dataclass, encode_pid_set_cmd(), decode_pid_ack().

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

include/orin_can.h

@@ -32,6 +32,7 @@
 #define ORIN_CAN_ID_MODE        0x302u
 #define ORIN_CAN_ID_ESTOP       0x303u
 #define ORIN_CAN_ID_LED_CMD     0x304u  /* LED pattern override (Issue #685) */
+#define ORIN_CAN_ID_PID_SET     0x305u  /* PID gain update: kp/ki/kd (Issue #693) */
 
 /* ---- FC → Orin telemetry IDs ---- */
 #define ORIN_CAN_ID_FC_STATUS   0x400u  /* balance state + pitch + vbat at 10 Hz */
@@ -39,6 +40,7 @@
 #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) */
 #define ORIN_CAN_ID_FC_BTN      0x404u  /* button event on-demand (Issue #682) */
+#define ORIN_CAN_ID_FC_PID_ACK  0x405u  /* PID gain ACK: echoes applied kp/ki/kd (Issue #693) */
 
 /* ---- Timing ---- */
 #define ORIN_HB_TIMEOUT_MS      500u   /* Orin offline after 500 ms without any frame */
@@ -56,6 +58,11 @@ typedef struct {
     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 */
+    /* PID_SET (Issue #693) -- set by orin_can_on_frame(), consumed by main */
+    volatile uint8_t  pid_updated;     /* set on PID_SET, cleared by main */
+    volatile uint16_t pid_kp_x100;     /* Kp * 100 (0..50000) */
+    volatile uint16_t pid_ki_x100;     /* Ki * 100 (0..5000) */
+    volatile uint16_t pid_kd_x100;     /* Kd * 100 (0..5000) */
 } OrinCanState;
 
 extern volatile OrinCanState orin_can_state;
@@ -164,4 +171,21 @@ void orin_can_broadcast_baro(uint32_t now_ms,
  */
 void orin_can_send_btn_event(uint8_t event_id, uint8_t balance_state);
 
+/* orin_can_send_pid_ack() -- send FC_PID_ACK (0x405). Issue #693. */
+void orin_can_send_pid_ack(float kp, float ki, float kd);
+
+/* PID_SET (0x305) -- 6-byte payload: kp*100, ki*100, kd*100 (uint16 BE each) */
+typedef struct __attribute__((packed)) {
+    uint16_t kp_x100;
+    uint16_t ki_x100;
+    uint16_t kd_x100;
+} orin_can_pid_set_t;
+
+/* FC_PID_ACK (0x405) -- FC -> Orin echo of applied gains */
+typedef struct __attribute__((packed)) {
+    uint16_t kp_x100;
+    uint16_t ki_x100;
+    uint16_t kd_x100;
+} orin_can_fc_pid_ack_t;
+
 #endif /* ORIN_CAN_H */

jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/can_bridge_node.py

@@ -30,6 +30,7 @@ import can
 import rclpy
 from geometry_msgs.msg import Twist
 from rclpy.node import Node
+from rcl_interfaces.msg import SetParametersResult
 from sensor_msgs.msg import BatteryState, Imu
 from std_msgs.msg import Bool, Float32MultiArray, String
 
@@ -40,14 +41,18 @@ from saltybot_can_bridge.mamba_protocol import (
     MAMBA_TELEM_BATTERY,
     MAMBA_TELEM_IMU,
     VESC_TELEM_STATE,
+    ORIN_CAN_ID_FC_PID_ACK,
+    ORIN_CAN_ID_PID_SET,
     MODE_DRIVE,
     MODE_ESTOP,
     MODE_IDLE,
     encode_estop_cmd,
     encode_mode_cmd,
     encode_velocity_cmd,
+    encode_pid_set_cmd,
     decode_battery_telem,
     decode_imu_telem,
+    decode_pid_ack,
     decode_vesc_state,
 )
 
@@ -70,12 +75,18 @@ class CanBridgeNode(Node):
         self.declare_parameter("right_vesc_can_id", 68)
         self.declare_parameter("mamba_can_id", 1)
         self.declare_parameter("command_timeout_s", 0.5)
+        self.declare_parameter("pid/kp", 0.0)
+        self.declare_parameter("pid/ki", 0.0)
+        self.declare_parameter("pid/kd", 0.0)
 
         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
+        self._pid_kp: float = self.get_parameter("pid/kp").value
+        self._pid_ki: float = self.get_parameter("pid/ki").value
+        self._pid_kd: float = self.get_parameter("pid/kd").value
 
         # ── State ─────────────────────────────────────────────────────────
         self._bus: Optional[can.BusABC] = None
@@ -99,6 +110,7 @@ class CanBridgeNode(Node):
         # ── Subscriptions ─────────────────────────────────────────────────
         self.create_subscription(Twist, "/cmd_vel", self._cmd_vel_cb, 10)
         self.create_subscription(Bool, "/estop", self._estop_cb, 10)
+        self.add_on_set_parameters_callback(self._on_set_parameters)
 
         # ── Timers ────────────────────────────────────────────────────────
         self.create_timer(1.0 / _WATCHDOG_HZ, self._watchdog_cb)
@@ -119,6 +131,30 @@ class CanBridgeNode(Node):
             f"mamba={self._mamba_id}"
         )
 
+    # -- PID parameter callback (Issue #693) --
+
+    def _on_set_parameters(self, params) -> SetParametersResult:
+        """Send new PID gains over CAN when pid/* params change."""
+        for p in params:
+            if p.name == "pid/kp":
+                self._pid_kp = float(p.value)
+            elif p.name == "pid/ki":
+                self._pid_ki = float(p.value)
+            elif p.name == "pid/kd":
+                self._pid_kd = float(p.value)
+            else:
+                continue
+            try:
+                payload = encode_pid_set_cmd(self._pid_kp, self._pid_ki, self._pid_kd)
+                self._send_can(ORIN_CAN_ID_PID_SET, payload, "pid_set")
+                self.get_logger().info(
+                    f"PID gains sent: Kp={self._pid_kp:.2f} "
+                    f"Ki={self._pid_ki:.2f} Kd={self._pid_kd:.2f}"
+                )
+            except ValueError as exc:
+                return SetParametersResult(successful=False, reason=str(exc))
+        return SetParametersResult(successful=True)
+
     # ── Connection management ──────────────────────────────────────────────
 
     def _try_connect(self) -> None:
@@ -282,6 +318,12 @@ class CanBridgeNode(Node):
             elif arb_id == VESC_TELEM_STATE + self._right_vesc_id:
                 self._handle_vesc_state(data, frame.timestamp, side="right")
 
+            elif arb_id == ORIN_CAN_ID_FC_PID_ACK:
+                gains = decode_pid_ack(data)
+                self.get_logger().debug(
+                    f"FC PID ACK: Kp={gains.kp:.2f} Ki={gains.ki:.2f} Kd={gains.kd:.2f}"
+                )
+
         except Exception as exc:
             self.get_logger().warning(
                 f"Error parsing CAN frame 0x{arb_id:03X}: {exc}"

jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/mamba_protocol.py

@@ -38,6 +38,8 @@ MAMBA_TELEM_IMU: int = 0x200
 MAMBA_TELEM_BATTERY: int = 0x201
 
 VESC_TELEM_STATE: int = 0x300
+ORIN_CAN_ID_PID_SET: int = 0x305
+ORIN_CAN_ID_FC_PID_ACK: int = 0x405
 
 # ---------------------------------------------------------------------------
 # Mode constants
@@ -82,6 +84,14 @@ class VescStateTelemetry:
     current: float = 0.0   # phase current, A
 
 
+@dataclass
+class PidGains:
+    """Balance PID gains (Issue #693)."""
+    kp: float = 0.0
+    ki: float = 0.0
+    kd: float = 0.0
+
+
 # ---------------------------------------------------------------------------
 # Encode helpers
 # ---------------------------------------------------------------------------
@@ -142,6 +152,13 @@ def encode_estop_cmd(stop: bool = True) -> bytes:
     return struct.pack(_FMT_ESTOP, 0x01 if stop else 0x00)
 
 
+def encode_pid_set_cmd(kp: float, ki: float, kd: float) -> bytes:
+    """Encode ORIN_CAN_ID_PID_SET (6 bytes, uint16 BE x3). Issue #693."""
+    if kp < 0.0 or ki < 0.0 or kd < 0.0:
+        raise ValueError("PID gains must be non-negative")
+    return struct.pack(_FMT_PID, round(min(kp,_PID_KP_MAX)*100), round(min(ki,_PID_KI_MAX)*100), round(min(kd,_PID_KD_MAX)*100))
+
+
 # ---------------------------------------------------------------------------
 # Decode helpers
 # ---------------------------------------------------------------------------
@@ -199,3 +216,9 @@ def decode_vesc_state(data: bytes) -> VescStateTelemetry:
     """
     erpm, duty, voltage, current = struct.unpack(_FMT_VESC, data)
     return VescStateTelemetry(erpm=erpm, duty=duty, voltage=voltage, current=current)
+
+
+def decode_pid_ack(data: bytes) -> PidGains:
+    """Decode ORIN_CAN_ID_FC_PID_ACK (6 bytes). Issue #693."""
+    kp_x100, ki_x100, kd_x100 = struct.unpack(_FMT_PID, data)
+    return PidGains(kp=kp_x100/100.0, ki=ki_x100/100.0, kd=kd_x100/100.0)

src/main.c

@@ -528,6 +528,23 @@ int main(void) {
             if (safety_remote_estop_active() && bal.state == BALANCE_DISARMED)
                 safety_remote_estop_clear();
         }
+        /* PID_SET (Issue #693): apply new gains from Orin immediately.
+         * Gains persist in RAM until reboot; not saved to flash. */
+        if (orin_can_state.pid_updated) {
+            orin_can_state.pid_updated = 0u;
+            float new_kp = (float)orin_can_state.pid_kp_x100 / 100.0f;
+            float new_ki = (float)orin_can_state.pid_ki_x100 / 100.0f;
+            float new_kd = (float)orin_can_state.pid_kd_x100 / 100.0f;
+            if (new_kp > 500.0f) new_kp = 500.0f;
+            if (new_ki > 50.0f)  new_ki = 50.0f;
+            if (new_kd > 50.0f)  new_kd = 50.0f;
+            bal.kp = new_kp;
+            bal.ki = new_ki;
+            bal.kd = new_kd;
+            orin_can_send_pid_ack(bal.kp, bal.ki, bal.kd);
+            printf("[PID] Orin set Kp=%.3f Ki=%.3f Kd=%.3f\n",
+                   (double)bal.kp, (double)bal.ki, (double)bal.kd);
+        }
 
         /* Hardware button park/re-arm (Issue #682).
          * Short press -> park (ARMED only): freeze PID, stop motors, amber LED.

src/orin_can.c

@@ -81,11 +81,34 @@ void orin_can_on_frame(uint16_t std_id, const uint8_t *data, uint8_t len)
         orin_can_led_updated = 1u;
         break;
 
+    case ORIN_CAN_ID_PID_SET:
+        /* kp_x100(u16 BE), ki_x100(u16 BE), kd_x100(u16 BE) -- Issue #693 */
+        if (len < 6u) { break; }
+        orin_can_state.pid_kp_x100 = (uint16_t)(((uint16_t)data[0] << 8u) | (uint16_t)data[1]);
+        orin_can_state.pid_ki_x100 = (uint16_t)(((uint16_t)data[2] << 8u) | (uint16_t)data[3]);
+        orin_can_state.pid_kd_x100 = (uint16_t)(((uint16_t)data[4] << 8u) | (uint16_t)data[5]);
+        orin_can_state.pid_updated = 1u;
+        break;
+
     default:
         break;
     }
 }
 
+void orin_can_send_pid_ack(float kp, float ki, float kd)
+{
+    orin_can_fc_pid_ack_t ack;
+    if (kp < 0.0f) kp = 0.0f;
+    if (ki < 0.0f) ki = 0.0f;
+    if (kd < 0.0f) kd = 0.0f;
+    ack.kp_x100 = (uint16_t)(kp * 100.0f + 0.5f);
+    ack.ki_x100 = (uint16_t)(ki * 100.0f + 0.5f);
+    ack.kd_x100 = (uint16_t)(kd * 100.0f + 0.5f);
+    uint8_t buf[6];
+    memcpy(buf, &ack, sizeof(ack));
+    can_driver_send_std(ORIN_CAN_ID_FC_PID_ACK, buf, (uint8_t)sizeof(ack));
+}
+
 bool orin_can_is_alive(uint32_t now_ms)
 {
     if (orin_can_state.last_rx_ms == 0u) {