From de4d1bbe3ab4a8168ee2fb815c0f97c3ae6c3aa5 Mon Sep 17 00:00:00 2001 From: sl-controls Date: Fri, 20 Mar 2026 16:38:09 -0400 Subject: [PATCH] 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 --- include/orin_can.h | 24 +++++++++++ .../saltybot_can_bridge/can_bridge_node.py | 42 +++++++++++++++++++ .../saltybot_can_bridge/mamba_protocol.py | 23 ++++++++++ src/main.c | 17 ++++++++ src/orin_can.c | 23 ++++++++++ 5 files changed, 129 insertions(+) diff --git a/include/orin_can.h b/include/orin_can.h index 7009bd2..aea84b0 100644 --- a/include/orin_can.h +++ b/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 */ diff --git a/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/can_bridge_node.py b/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/can_bridge_node.py index c267938..fe62740 100644 --- a/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/can_bridge_node.py +++ b/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/can_bridge_node.py @@ -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}" diff --git a/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/mamba_protocol.py b/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/mamba_protocol.py index 10ca43b..804ae29 100644 --- a/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/mamba_protocol.py +++ b/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/mamba_protocol.py @@ -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) diff --git a/src/main.c b/src/main.c index b522b6d..f5d040b 100644 --- a/src/main.c +++ b/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. diff --git a/src/orin_can.c b/src/orin_can.c index dc59792..37d1e63 100644 --- a/src/orin_can.c +++ b/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) { -- 2.47.2