seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests 3 Actions Packages Projects Releases Wiki Activity

feat(bridge): binary STM32 command protocol (Issue #119) #128

Merged
sl-jetson merged 2 commits from sl-jetson/issue-119-cmd-protocol into main 2026-03-02 09:26:21 -05:00
Conversation 0 Commits 2 Files Changed 8 +1737 -3
sl-jetson commented 2026-03-02 09:17:42 -05:00
Collaborator
Copy Link

Summary

  • stm32_protocol.py — pure-Python binary frame codec (no ROS2 deps, fully unit-testable)
    • Frame: STX(0x02) + TYPE + LEN + PAYLOAD + CRC16-CCITT(BE) + ETX(0x03)
    • CRC covers TYPE+LEN+PAYLOAD; polynomial 0x1021, init 0xFFFF
    • Encoders: HEARTBEAT, SPEED_STEER (int16×2, ±1000), ARM, SET_MODE, PID_UPDATE (float32×3)
    • Telemetry decoders: ImuFrame, BatteryFrame, MotorRpmFrame, ArmStateFrame, ErrorFrame
    • FrameParser: streaming byte-by-byte state machine, resync after corrupt data
  • stm32_cmd_node.py — full-featured ROS2 bridge node
    • /cmd_vel → SPEED_STEER at up to 50 Hz with Twist scaling
    • HEARTBEAT every 200ms (STM32 watchdog fires at 500ms)
    • Jetson-side watchdog: no /cmd_vel for 500ms → send SPEED_STEER(0,0)
    • /saltybot/arm service (SetBool) + /saltybot/pid_update topic
    • Telemetry RX → /saltybot/imu, /saltybot/telemetry/battery, /saltybot/telemetry/motor_rpm, /saltybot/arm_state, /saltybot/error
    • Auto-reconnect on USB disconnect; zero-speed + disarm on shutdown
    • /diagnostics with serial health, frame/error counts
  • 90+ unit tests across test_stm32_protocol.py and test_stm32_cmd_node.py
    • MockSerial for byte-level TX/RX without hardware
    • CRC correctness, all frame types, bad CRC/ETX, resync, streaming, watchdog

Test plan

  • pytest test/test_stm32_protocol.py -v — all 60+ tests pass
  • pytest test/test_stm32_cmd_node.py -v — all 30+ tests pass
  • stm32_cmd_node launches and opens /dev/ttyACM0 on hardware
  • SPEED_STEER frames appear at 50 Hz on oscilloscope when /cmd_vel is published
  • Watchdog zero-speed fires 500ms after last /cmd_vel
  • Telemetry topics appear at expected rates when connected to STM32

Closes #119

## Summary - `stm32_protocol.py` — pure-Python binary frame codec (no ROS2 deps, fully unit-testable) - Frame: `STX(0x02) + TYPE + LEN + PAYLOAD + CRC16-CCITT(BE) + ETX(0x03)` - CRC covers TYPE+LEN+PAYLOAD; polynomial 0x1021, init 0xFFFF - Encoders: HEARTBEAT, SPEED_STEER (int16×2, ±1000), ARM, SET_MODE, PID_UPDATE (float32×3) - Telemetry decoders: `ImuFrame`, `BatteryFrame`, `MotorRpmFrame`, `ArmStateFrame`, `ErrorFrame` - `FrameParser`: streaming byte-by-byte state machine, resync after corrupt data - `stm32_cmd_node.py` — full-featured ROS2 bridge node - `/cmd_vel` → SPEED_STEER at up to 50 Hz with Twist scaling - HEARTBEAT every 200ms (STM32 watchdog fires at 500ms) - Jetson-side watchdog: no `/cmd_vel` for 500ms → send SPEED_STEER(0,0) - `/saltybot/arm` service (SetBool) + `/saltybot/pid_update` topic - Telemetry RX → `/saltybot/imu`, `/saltybot/telemetry/battery`, `/saltybot/telemetry/motor_rpm`, `/saltybot/arm_state`, `/saltybot/error` - Auto-reconnect on USB disconnect; zero-speed + disarm on shutdown - `/diagnostics` with serial health, frame/error counts - 90+ unit tests across `test_stm32_protocol.py` and `test_stm32_cmd_node.py` - `MockSerial` for byte-level TX/RX without hardware - CRC correctness, all frame types, bad CRC/ETX, resync, streaming, watchdog ## Test plan - [ ] `pytest test/test_stm32_protocol.py -v` — all 60+ tests pass - [ ] `pytest test/test_stm32_cmd_node.py -v` — all 30+ tests pass - [ ] `stm32_cmd_node` launches and opens `/dev/ttyACM0` on hardware - [ ] SPEED_STEER frames appear at 50 Hz on oscilloscope when `/cmd_vel` is published - [ ] Watchdog zero-speed fires 500ms after last `/cmd_vel` - [ ] Telemetry topics appear at expected rates when connected to STM32 Closes #119
sl-jetson added 2 commits 2026-03-02 09:17:43 -05:00 
Add stm32_protocol.py — pure-Python binary frame codec:
- Frame: STX(0x02)+TYPE+LEN+PAYLOAD+CRC16-CCITT(BE)+ETX(0x03)
- CRC covers TYPE+LEN+PAYLOAD; polynomial 0x1021, init 0xFFFF
- Encoders: HEARTBEAT, SPEED_STEER(-1000..+1000 int16), ARM, SET_MODE, PID_UPDATE
- Telemetry decoders: ImuFrame, BatteryFrame, MotorRpmFrame, ArmStateFrame, ErrorFrame
- FrameParser: streaming byte-by-byte state machine, resync on corrupt data

Add stm32_cmd_node.py — ROS2 bidirectional bridge node:
- /cmd_vel → SPEED_STEER at up to 50 Hz
- HEARTBEAT timer (default 200ms); STM32 watchdog fires at 500ms
- Jetson-side watchdog: no /cmd_vel for 500ms → send SPEED_STEER(0,0)
- /saltybot/arm service (SetBool) → ARM frame
- /saltybot/pid_update topic → PID_UPDATE frame
- Telemetry RX: IMU→/saltybot/imu, BATTERY→/saltybot/telemetry/battery,
  MOTOR_RPM→/saltybot/telemetry/motor_rpm, ARM_STATE→/saltybot/arm_state,
  ERROR→/saltybot/error
- Auto-reconnect on USB disconnect (serial.SerialException caught)
- Zero-speed + disarm sent on node shutdown
- /diagnostics with serial health, frame counts, last cmd age

Add test_stm32_protocol.py (60+ tests):
- CRC16-CCITT correctness, known test vectors
- All encoder output structures and payload values
- FrameParser: all telemetry types, bad CRC, bad ETX, resync, streaming,
  oversized payload, frame counters, reset

Add test_stm32_cmd_node.py (30+ tests):
- MockSerial: TX/RX byte-level testing without hardware
- Speed/steer clamping, scaling, frame structure
- Watchdog fires/doesn't fire relative to timeout
- CRC error counted, resync after garbage

Add stm32_cmd_params.yaml, stm32_cmd.launch.py.
Update package.xml (add std_srvs, geometry_msgs deps).
Update setup.py (add stm32_cmd_node entry point + new config/launch).

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
Add stm32_cmd_node and battery_node console_scripts, new launch/config files
to data_files list.

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
sl-jetson merged commit 5cfacdda3f into main 2026-03-02 09:26:21 -05:00
Sign in to join this conversation.
Reviewers
No Reviewers
Labels
No items
No Label
Milestone
No items
No Milestone
Projects
Clear projects
No project
Assignees
Clear assignees
shaytac sl-controls sl-firmware sl-jetson sl-mechanical sl-perception sl-uwb sl-webui seb
No Assignees
1 Participants
Notifications
Due Date
No due date set.
Dependencies

No dependencies set.

Reference: seb/saltylab-firmware#128
No description provided.
Delete Branch "sl-jetson/issue-119-cmd-protocol"

Deleting a branch is permanent. Although the deleted branch may continue to exist for a short time before it actually gets removed, it CANNOT be undone in most cases. Continue?