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

chore: remove Mamba F722S / STM32 / BlackPill legacy hw refs #716

Closed
sl-jetson wants to merge 2 commits from sl-android/cleanup-legacy-hw into main
pull from: sl-android/cleanup-legacy-hw
merge into: seb:main
Conversation 0 Commits 2 Files Changed 16 +115 -153
16 changed files with 115 additions and 153 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

200
docs/AGENTS.md
View File

@ -5,17 +5,18 @@ You're working on **SaltyLab**, a self-balancing two-wheeled indoor robot. Read
## Project Overview ## Project Overview
A hoverboard-based balancing robot with two compute layers: A hoverboard-based balancing robot with two compute layers:
1. **FC (Flight Controller)** — MAMBA F722S (STM32F722RET6 + MPU6000 IMU). Runs a lean C balance loop at up to 8kHz. Talks UART to the hoverboard ESC. This is the safety-critical layer. 1. **ESP32-S3 BALANCE** — runs the PID balance loop. Safety-critical, operates independently of the Orin.
2. **Jetson Nano** — AI brain. ROS2, SLAM, person tracking. Sends velocity commands to FC via UART. Not safety-critical — FC operates independently. 2. **ESP32-S3 IO** — handles I/O: motor commands to VESCs, sensor polling, CAN/UART comms.
3. **Orin Nano Super** — AI brain. ROS2, SLAM, person tracking. Sends velocity commands to ESP32-S3 BALANCE via UART. Not safety-critical.
``` ```
Jetson (speed+steer via UART1) ←→ ELRS RC (UART3, kill switch) Orin Nano Super (speed+steer via UART) ←→ ELRS RC (kill switch)
│ │
▼ ▼
MAMBA F722S (MPU6000 IMU, PID balance) ESP32-S3 BALANCE (IMU, PID balance loop)
│ │
▼ UART2 ▼ CAN / UART
Hoverboard ESC (FOC) → 2× 8" hub motors ESP32-S3 IO → VESC 68 (left) + VESC 56 (right)
``` ```
## ⚠️ SAFETY — READ THIS OR PEOPLE GET HURT ## ⚠️ SAFETY — READ THIS OR PEOPLE GET HURT
@ -26,7 +27,7 @@ This is not a toy. 8" hub motors + 36V battery can crush fingers, break toes, an
2. **Tilt cutoff at ±25°** — motors to zero, require manual re-arm. No retry, no recovery. 2. **Tilt cutoff at ±25°** — motors to zero, require manual re-arm. No retry, no recovery.
3. **Hardware watchdog (50ms)** — if firmware hangs, motors cut. 3. **Hardware watchdog (50ms)** — if firmware hangs, motors cut.
4. **RC kill switch** — dedicated ELRS channel, checked every loop iteration. Always overrides. 4. **RC kill switch** — dedicated ELRS channel, checked every loop iteration. Always overrides.
5. **Jetson UART timeout (200ms)** — if Jetson disconnects, motors cut. 5. **Orin UART timeout (200ms)** — if Orin disconnects, motors cut.
6. **Speed hard cap** — firmware limit, start at 10%. Increase only after proven stable. 6. **Speed hard cap** — firmware limit, start at 10%. Increase only after proven stable.
7. **Never test untethered** until PID is stable for 5+ minutes on a tether. 7. **Never test untethered** until PID is stable for 5+ minutes on a tether.
@ -35,31 +36,16 @@ This is not a toy. 8" hub motors + 36V battery can crush fingers, break toes, an
## Repository Layout ## Repository Layout
``` ```
firmware/ # STM32 HAL firmware (PlatformIO) esp32/ # ESP32-S3 firmware (ESP-IDF)
├── src/ ├── balance/ # ESP32-S3 BALANCE — PID loop, IMU, safety
│ ├── main.c # Entry point, clock config, main loop └── io/ # ESP32-S3 IO — VESC CAN, sensors, comms
│ ├── icm42688.c # ICM-42688-P SPI driver (backup IMU — currently broken)
│ ├── bmp280.c # Barometer driver (disabled)
│ └── status.c # LED + buzzer status patterns
├── include/
│ ├── config.h # Pin definitions, constants
│ ├── icm42688.h
│ ├── mpu6000.h # MPU6000 driver header (primary IMU)
│ ├── hoverboard.h # Hoverboard ESC UART protocol
│ ├── crsf.h # ELRS CRSF protocol
│ ├── bmp280.h
│ └── status.h
├── lib/USB_CDC/ # USB CDC stack (serial over USB)
│ ├── src/ # CDC implementation, USB descriptors, PCD config
│ └── include/
└── platformio.ini # Build config
cad/ # OpenSCAD parametric parts (16 files) cad/ # OpenSCAD parametric parts (16 files)
├── dimensions.scad # ALL measurements live here — single source of truth ├── dimensions.scad # ALL measurements live here — single source of truth
├── assembly.scad # Full robot assembly visualization ├── assembly.scad # Full robot assembly visualization
├── motor_mount_plate.scad ├── motor_mount_plate.scad
├── battery_shelf.scad ├── battery_shelf.scad
├── fc_mount.scad # Vibration-isolated FC mount ├── esp32_balance_mount.scad # Vibration-isolated ESP32-S3 BALANCE mount
├── jetson_shelf.scad ├── jetson_shelf.scad
├── esc_mount.scad ├── esc_mount.scad
├── sensor_tower_top.scad ├── sensor_tower_top.scad
@ -82,55 +68,55 @@ PLATFORM.md # Hardware platform reference
## Hardware Quick Reference ## Hardware Quick Reference
### MAMBA F722S Flight Controller ### ESP32-S3 BALANCE
| Spec | Value | | Spec | Value |
|------|-------| |------|-------|
| MCU | STM32F722RET6 (Cortex-M7, 216MHz, 512KB flash, 256KB RAM) | | MCU | ESP32-S3 (dual-core Xtensa LX7, 240MHz, 512KB SRAM, 8MB flash) |
| Primary IMU | MPU6000 (WHO_AM_I = 0x68) | | Primary IMU | MPU6000 (SPI) |
| IMU Bus | SPI1: PA5=SCK, PA6=MISO, PA7=MOSI, CS=PA4 | | Role | PID balance loop, tilt cutoff, arming |
| IMU EXTI | PC4 (data ready interrupt) | | Comms to Orin | UART (velocity commands in, telemetry out) |
| IMU Orientation | CW270 (Betaflight convention) | | Flash | `idf.py -p /dev/ttyUSB0 flash` |
| Secondary IMU | ICM-42688-P (on same SPI1, CS unknown — currently non-functional) |
| Betaflight Target | DIAT-MAMBAF722_2022B |
| USB | OTG FS (PA11/PA12), enumerates as /dev/cu.usbmodemSALTY0011 |
| VID/PID | 0x0483/0x5740 |
| LEDs | PC15 (LED1), PC14 (LED2), active low |
| Buzzer | PB2 (inverted push-pull) |
| Battery ADC | PC1=VBAT, PC3=CURR (ADC3) |
| DFU | Hold yellow BOOT button + plug USB (or send 'R' over CDC) |
### UART Assignments ### ESP32-S3 IO
| UART | Pins | Connected To | Baud | | Spec | Value |
|------|------|-------------|------| |------|-------|
| USART1 | PA9/PA10 | Jetson Nano | 115200 | | MCU | ESP32-S3 |
| USART2 | PA2/PA3 | Hoverboard ESC | 115200 | | Role | VESC CAN driver, sensor polling, peripheral I/O |
| USART3 | PB10/PB11 | ELRS Receiver | 420000 (CRSF) | | VESC IDs | 68 (left), 56 (right) |
| UART4 | — | Spare | — | | Motor bus | CAN 1Mbit/s |
| UART5 | — | Spare | — | | Flash | `idf.py -p /dev/ttyUSB1 flash` |
### UART Assignments (ESP32-S3 BALANCE)
| UART | Connected To | Baud |
|------|-------------|------|
| UART0 | Orin Nano Super | 115200 |
| UART1 | ESP32-S3 IO | 115200 |
| UART2 | ELRS Receiver | 420000 (CRSF) |
### Motor/ESC ### Motor/ESC
- 2× 8" pneumatic hub motors (36V, hoverboard type) - 2× 8" pneumatic hub motors (36V, hoverboard type)
- Hoverboard ESC with FOC firmware - 2× VESC motor controllers (CAN IDs 68, 56)
- UART protocol: `{0xABCD, int16 speed, int16 steer, uint16 checksum}` at 115200 - VESC CAN protocol: standard SET_DUTY / SET_CURRENT / SET_RPM
- Speed range: -1000 to +1000 - Speed range: -1.0 to +1.0 (duty cycle)
### Physical Dimensions (from `cad/dimensions.scad`) ### Physical Dimensions (from `cad/dimensions.scad`)
| Part | Key Measurement | | Part | Key Measurement |
|------|----------------| |------|----------------|
| FC mounting holes | 25.5mm spacing (NOT standard 30.5mm!) | | ESP32-S3 BALANCE board | ~55×28mm (DevKit form factor) |
| FC board size | ~36mm square | | ESP32-S3 IO board | ~55×28mm (DevKit form factor) |
| Hub motor body | Ø200mm (~8") | | Hub motor body | Ø200mm (~8") |
| Motor axle | Ø12mm, 45mm long | | Motor axle | Ø12mm, 45mm long |
| Jetson Nano | 100×80×29mm, M2.5 holes at 86×58mm | | Orin Nano Super | 100×79mm, M2.5 holes at 86×58mm |
| RealSense D435i | 90×25×25mm, 1/4-20 tripod mount | | RealSense D435i | 90×25×25mm, 1/4-20 tripod mount |
| RPLIDAR A1 | Ø70×41mm, 4× M2.5 on Ø67mm circle | | RPLIDAR A1 | Ø70×41mm, 4× M2.5 on Ø67mm circle |
| Kill switch hole | Ø22mm panel mount | | Kill switch hole | Ø22mm panel mount |
| Battery pack | ~180×80×40mm | | Battery pack | ~180×80×40mm |
| Hoverboard ESC | ~80×50×15mm | | VESC (each) | ~70×50×15mm |
| 2020 extrusion | 20mm square, M5 center bore | | 2020 extrusion | 20mm square, M5 center bore |
| Frame width | ~350mm (axle to axle) | | Frame width | ~350mm (axle to axle) |
| Frame height | ~500-550mm total | | Frame height | ~500-550mm total |
@ -147,7 +133,7 @@ PLATFORM.md # Hardware platform reference
| sensor_tower_top | ASA | 80% | | sensor_tower_top | ASA | 80% |
| lidar_standoff (Ø80×80mm) | ASA | 40% | | lidar_standoff (Ø80×80mm) | ASA | 40% |
| realsense_bracket | PETG | 60% | | realsense_bracket | PETG | 60% |
| fc_mount (vibration isolated) | TPU+PETG | — | | esp32_balance_mount (vibration isolated) | TPU+PETG | — |
| bumper front + rear (350×50×30mm) | TPU | 30% | | bumper front + rear (350×50×30mm) | TPU | 30% |
| handle | PETG | 80% | | handle | PETG | 80% |
| kill_switch_mount | PETG | 80% | | kill_switch_mount | PETG | 80% |
@ -159,99 +145,75 @@ PLATFORM.md # Hardware platform reference
### Critical Lessons Learned (DON'T REPEAT THESE) ### Critical Lessons Learned (DON'T REPEAT THESE)
1. **SysTick_Handler with HAL_IncTick() is MANDATORY** — without it, HAL_Delay() and every HAL timeout hangs forever. This bricked us multiple times. 1. **NEVER auto-run untested code on_boot** — we bricked the NSPanel 3x doing this. Test manually first.
2. **DCache breaks SPI on STM32F7** — disable DCache or use cache-aligned DMA buffers with clean/invalidate. We disable it. 2. **`-(int)0 == 0`** — checking `if (-result)` to detect errors doesn't work when result is 0. Always use explicit error codes.
3. **`-(int)0 == 0`** — checking `if (-result)` to detect errors doesn't work when result is 0 (success and failure look the same). Always use explicit error codes. 3. **USB CDC needs RX primed in init** — without it, the OUT endpoint never starts listening.
4. **NEVER auto-run untested code on_boot** — we bricked the NSPanel 3x doing this. Test manually first. 4. **Watchdog must be fed every loop iteration** — if balance loop stalls, motors must cut within 50ms.
5. **USB CDC needs ReceivePacket() primed in CDC_Init** — without it, the OUT endpoint never starts listening. No data reception. 5. **Never change PID and speed limit in the same test** — one variable at a time.
### DFU Reboot (Betaflight Method) ### Build & Flash (ESP32-S3)
The firmware supports reboot-to-DFU via USB command:
1. Send `R` byte over USB CDC
2. Firmware writes `0xDEADBEEF` to RTC backup register 0
3. `NVIC_SystemReset()` — clean hardware reset
4. On boot, `checkForBootloader()` (called after `HAL_Init()`) reads the magic
5. If magic found: clears it, remaps system memory, jumps to STM32 bootloader at `0x1FF00000`
6. Board appears as DFU device, ready for `dfu-util` flash
### Build & Flash
```bash ```bash
cd firmware/ # Balance board
python3 -m platformio run # Build cd esp32/balance/
dfu-util -a 0 -s 0x08000000:leave -D .pio/build/f722/firmware.bin # Flash idf.py build && idf.py -p /dev/ttyUSB0 flash monitor
# IO board
cd esp32/io/
idf.py build && idf.py -p /dev/ttyUSB1 flash monitor
``` ```
Dev machine: mbpm4 (seb@192.168.87.40), PlatformIO project at `~/Projects/saltylab-firmware/` Dev machine: mbpm4 (seb@192.168.87.40)
### Clock Configuration
```
HSE 8MHz → PLL (M=8, N=432, P=2, Q=9) → SYSCLK 216MHz
PLLSAI (N=384, P=8) → CLK48 48MHz (USB)
APB1 = HCLK/4 = 54MHz
APB2 = HCLK/2 = 108MHz
Fallback: HSI 16MHz if HSE fails (PLL M=16)
```
## Current Status & Known Issues ## Current Status & Known Issues
### Working ### Working
- USB CDC serial streaming (50Hz JSON: `{"ax":...,"ay":...,"az":...,"gx":...,"gy":...,"gz":...}`) - IMU streaming (50Hz JSON: `{"ax":...,"ay":...,"az":...,"gx":...,"gy":...,"gz":...}`)
- Clock config with HSE + HSI fallback - VESC CAN communication (IDs 68, 56)
- Reboot-to-DFU via USB 'R' command - LED status patterns
- LED status patterns (status.c)
- Web UI with WebSerial + Three.js 3D visualization - Web UI with WebSerial + Three.js 3D visualization
### Broken / In Progress ### In Progress
- **ICM-42688-P SPI reads return all zeros** — was the original IMU target, but SPI communication completely non-functional despite correct pin config. May be dead silicon. Switched to MPU6000 as primary. - PID balance loop tuning
- **MPU6000 driver** — header exists but implementation needs completion - ELRS CRSF receiver integration
- **PID balance loop** — not yet implemented - Orin UART integration
- **Hoverboard ESC UART** — protocol defined, driver not written
- **ELRS CRSF receiver** — protocol defined, driver not written
- **Barometer (BMP280)** — I2C init hangs, disabled
### TODO (Priority Order) ### TODO (Priority Order)
1. Get MPU6000 streaming accel+gyro data 1. Tune PID balance loop on ESP32-S3 BALANCE
2. Implement complementary filter (pitch angle) 2. Implement complementary filter (pitch angle)
3. Write hoverboard ESC UART driver 3. Wire ELRS receiver, implement CRSF parser
4. Write PID balance loop with safety checks 4. Bench test (VESCs disconnected, verify PID output)
5. Wire ELRS receiver, implement CRSF parser 5. First tethered balance test at 10% speed
6. Bench test (ESC disconnected, verify PID output) 6. Orin UART integration
7. First tethered balance test at 10% speed 7. LED subsystem (ESP32-S3 IO)
8. Jetson UART integration
9. LED subsystem (ESP32-C3)
## Communication Protocols ## Communication Protocols
### Jetson → FC (UART1, 50Hz) ### Orin → ESP32-S3 BALANCE (UART0, 50Hz)
```c ```c
struct { uint8_t header=0xAA; int16_t speed; int16_t steer; uint8_t mode; uint8_t checksum; }; struct { uint8_t header=0xAA; int16_t speed; int16_t steer; uint8_t mode; uint8_t checksum; };
// mode: 0=idle, 1=balance, 2=follow, 3=RC // mode: 0=idle, 1=balance, 2=follow, 3=RC
``` ```
### FC → Hoverboard ESC (UART2, loop rate) ### ESP32-S3 IO → VESC (CAN, loop rate)
```c - Standard VESC CAN protocol (SET_DUTY / SET_CURRENT / SET_RPM)
struct { uint16_t start=0xABCD; int16_t speed; int16_t steer; uint16_t checksum; }; - Node IDs: VESC 68 (left), VESC 56 (right)
// speed/steer: -1000 to +1000
```
### FC → Jetson Telemetry (UART1 TX, 50Hz) ### ESP32-S3 BALANCE → Orin Telemetry (UART0 TX, 50Hz)
``` ```
T:12.3,P:45,L:100,R:-80,S:3\n T:12.3,P:45,L:100,R:-80,S:3\n
// T=tilt°, P=PID output, L/R=motor commands, S=state (0-3) // T=tilt°, P=PID output, L/R=motor commands, S=state (0-3)
``` ```
### FC → USB CDC (50Hz JSON) ### ESP32-S3 → USB (50Hz JSON, debug/tuning)
```json ```json
{"ax":123,"ay":-456,"az":16384,"gx":10,"gy":-5,"gz":3,"t":250,"p":0,"bt":0} {"ax":123,"ay":-456,"az":16384,"gx":10,"gy":-5,"gz":3,"t":250,"p":0}
// Raw IMU values (int16), t=temp×10, p=pressure, bt=baro temp // Raw IMU values (int16), t=temp×10, p=PID output
``` ```
## LED Subsystem (ESP32-C3) ## LED Subsystem (ESP32-S3 IO)
ESP32-C3 eavesdrops on FC→Jetson telemetry (listen-only tap on UART1 TX). No extra FC UART needed. ESP32-S3 IO eavesdrops on BALANCE→Orin telemetry (listen-only). No extra UART needed.
| State | Pattern | Color | | State | Pattern | Color |
|-------|---------|-------| |-------|---------|-------|
@ -275,7 +237,7 @@ ESP32-C3 eavesdrops on FC→Jetson telemetry (listen-only tap on UART1 TX). No e
1. **Read SALTYLAB.md fully** before making any design decisions 1. **Read SALTYLAB.md fully** before making any design decisions
2. **Never remove safety checks** from firmware — add more if needed 2. **Never remove safety checks** from firmware — add more if needed
3. **All measurements go in `cad/dimensions.scad`** — single source of truth 3. **All measurements go in `cad/dimensions.scad`** — single source of truth
4. **Test firmware on bench before any motor test** — ESC disconnected, verify outputs on serial 4. **Test firmware on bench before any motor test** — VESCs disconnected, verify outputs on serial
5. **One variable at a time** — don't change PID and speed limit in the same test 5. **One variable at a time** — don't change PID and speed limit in the same test
6. **Document what you change** — update this file if you add pins, change protocols, or discover hardware quirks 6. **Document what you change** — update this file if you add pins, change protocols, or discover hardware quirks
7. **Ask before wiring changes** — wrong connections can fry the FC ($50+ board) 7. **Ask before wiring changes** — wrong connections can fry an ESP32 or VESC

2
jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_protocol.py → jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/esp32_protocol.py
View File

@ -1,4 +1,4 @@
"""stm32_protocol.py — Binary frame codec for Jetson↔STM32 communication. """esp32_protocol.py — Binary frame codec for Jetson↔STM32 communication.
Issue #119: defines the binary serial protocol between the Jetson Nano and the Issue #119: defines the binary serial protocol between the Jetson Nano and the
STM32F722 flight controller over USB CDC @ 921600 baud. STM32F722 flight controller over USB CDC @ 921600 baud.

2
jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py
View File

@ -55,7 +55,7 @@ from sensor_msgs.msg import Imu
from std_msgs.msg import String from std_msgs.msg import String
from std_srvs.srv import SetBool, Trigger from std_srvs.srv import SetBool, Trigger
from .stm32_protocol import ( from .esp32_protocol import (
FrameParser, FrameParser,
ImuFrame, BatteryFrame, MotorRpmFrame, ArmStateFrame, ErrorFrame, ImuFrame, BatteryFrame, MotorRpmFrame, ArmStateFrame, ErrorFrame,
encode_heartbeat, encode_speed_steer, encode_arm, encode_set_mode, encode_heartbeat, encode_speed_steer, encode_arm, encode_set_mode,

14
jetson/ros2_ws/src/saltybot_bridge/test/test_stm32_cmd_node.py
View File

@ -29,7 +29,7 @@ import pytest
sys.path.insert(0, os.path.join(os.path.dirname(__file__), "..")) sys.path.insert(0, os.path.join(os.path.dirname(__file__), ".."))
from saltybot_bridge.stm32_protocol import ( from saltybot_bridge.esp32_protocol import (
STX, ETX, CmdType, TelType, STX, ETX, CmdType, TelType,
encode_speed_steer, encode_heartbeat, encode_arm, encode_pid_update, encode_speed_steer, encode_heartbeat, encode_arm, encode_pid_update,
_build_frame, _crc16_ccitt, _build_frame, _crc16_ccitt,
@ -219,10 +219,10 @@ class TestMockSerialTX:
class TestMockSerialRX: class TestMockSerialRX:
"""Test RX parsing path using MockSerial with pre-loaded telemetry data.""" """Test RX parsing path using MockSerial with pre-loaded telemetry data."""
from saltybot_bridge.stm32_protocol import FrameParser from saltybot_bridge.esp32_protocol import FrameParser
def test_rx_imu_frame(self): def test_rx_imu_frame(self):
from saltybot_bridge.stm32_protocol import FrameParser, ImuFrame from saltybot_bridge.esp32_protocol import FrameParser, ImuFrame
raw = _imu_frame_bytes(pitch=500, roll=-200, yaw=100, ax=0, ay=0, az=981) raw = _imu_frame_bytes(pitch=500, roll=-200, yaw=100, ax=0, ay=0, az=981)
ms = MockSerial(rx_data=raw) ms = MockSerial(rx_data=raw)
parser = FrameParser() parser = FrameParser()
@ -241,7 +241,7 @@ class TestMockSerialRX:
assert f.accel_z == pytest.approx(9.81) assert f.accel_z == pytest.approx(9.81)
def test_rx_battery_frame(self): def test_rx_battery_frame(self):
from saltybot_bridge.stm32_protocol import FrameParser, BatteryFrame from saltybot_bridge.esp32_protocol import FrameParser, BatteryFrame
raw = _battery_frame_bytes(v_mv=10500, i_ma=1200, soc=45) raw = _battery_frame_bytes(v_mv=10500, i_ma=1200, soc=45)
ms = MockSerial(rx_data=raw) ms = MockSerial(rx_data=raw)
parser = FrameParser() parser = FrameParser()
@ -257,7 +257,7 @@ class TestMockSerialRX:
assert f.soc_pct == 45 assert f.soc_pct == 45
def test_rx_multiple_frames_in_one_read(self): def test_rx_multiple_frames_in_one_read(self):
from saltybot_bridge.stm32_protocol import FrameParser from saltybot_bridge.esp32_protocol import FrameParser
raw = (_imu_frame_bytes() + _arm_state_frame_bytes() + _battery_frame_bytes()) raw = (_imu_frame_bytes() + _arm_state_frame_bytes() + _battery_frame_bytes())
ms = MockSerial(rx_data=raw) ms = MockSerial(rx_data=raw)
parser = FrameParser() parser = FrameParser()
@ -271,7 +271,7 @@ class TestMockSerialRX:
assert parser.frames_error == 0 assert parser.frames_error == 0
def test_rx_bad_crc_counted_as_error(self): def test_rx_bad_crc_counted_as_error(self):
from saltybot_bridge.stm32_protocol import FrameParser from saltybot_bridge.esp32_protocol import FrameParser
raw = bytearray(_arm_state_frame_bytes(state=1)) raw = bytearray(_arm_state_frame_bytes(state=1))
raw[-3] ^= 0xFF # corrupt CRC raw[-3] ^= 0xFF # corrupt CRC
ms = MockSerial(rx_data=bytes(raw)) ms = MockSerial(rx_data=bytes(raw))
@ -282,7 +282,7 @@ class TestMockSerialRX:
assert parser.frames_error == 1 assert parser.frames_error == 1
def test_rx_resync_after_corrupt_byte(self): def test_rx_resync_after_corrupt_byte(self):
from saltybot_bridge.stm32_protocol import FrameParser, ArmStateFrame from saltybot_bridge.esp32_protocol import FrameParser, ArmStateFrame
garbage = b"\xDE\xAD\x00\x00" garbage = b"\xDE\xAD\x00\x00"
valid = _arm_state_frame_bytes(state=1) valid = _arm_state_frame_bytes(state=1)
ms = MockSerial(rx_data=garbage + valid) ms = MockSerial(rx_data=garbage + valid)

6
jetson/ros2_ws/src/saltybot_bridge/test/test_stm32_protocol.py
View File

@ -1,4 +1,4 @@
"""test_stm32_protocol.py — Unit tests for binary STM32 frame codec. """test_esp32_protocol.py — Unit tests for binary STM32 frame codec.
Tests: Tests:
- CRC16-CCITT correctness - CRC16-CCITT correctness
@ -12,7 +12,7 @@ Tests:
- Speed/steer clamping in encode_speed_steer - Speed/steer clamping in encode_speed_steer
- Round-trip encode → decode for all known telemetry types - Round-trip encode → decode for all known telemetry types
Run with: pytest test/test_stm32_protocol.py -v Run with: pytest test/test_esp32_protocol.py -v
""" """
from __future__ import annotations from __future__ import annotations
@ -25,7 +25,7 @@ import os
# ── Path setup (no ROS2 install needed) ────────────────────────────────────── # ── Path setup (no ROS2 install needed) ──────────────────────────────────────
sys.path.insert(0, os.path.join(os.path.dirname(__file__), "..")) sys.path.insert(0, os.path.join(os.path.dirname(__file__), ".."))
from saltybot_bridge.stm32_protocol import ( from saltybot_bridge.esp32_protocol import (
STX, ETX, STX, ETX,
CmdType, TelType, CmdType, TelType,
ImuFrame, BatteryFrame, MotorRpmFrame, ArmStateFrame, ErrorFrame, ImuFrame, BatteryFrame, MotorRpmFrame, ArmStateFrame, ErrorFrame,

2
jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/mamba_protocol.py → jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/balance_protocol.py
View File

@ -1,6 +1,6 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
""" """
mamba_protocol.py — CAN message encoding/decoding for the Mamba motor controller balance_protocol.py — CAN message encoding/decoding for the Mamba motor controller
and VESC telemetry. and VESC telemetry.
CAN message layout CAN message layout

4
jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/can_bridge_node.py
View File

@ -1,6 +1,6 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
""" """
can_bridge_node.py — ROS2 node bridging the SaltyBot Orin to the Mamba motor can_bridge_node.py — ROS2 node bridging the SaltyBot Orin to the ESP32-S3 BALANCE
controller and VESC motor controllers over CAN bus. controller and VESC motor controllers over CAN bus.
The node opens the SocketCAN interface (slcan0 by default), spawns a background The node opens the SocketCAN interface (slcan0 by default), spawns a background
@ -34,7 +34,7 @@ from rcl_interfaces.msg import SetParametersResult
from sensor_msgs.msg import BatteryState, Imu from sensor_msgs.msg import BatteryState, Imu
from std_msgs.msg import Bool, Float32MultiArray, String from std_msgs.msg import Bool, Float32MultiArray, String
from saltybot_can_bridge.mamba_protocol import ( from saltybot_can_bridge.balance_protocol import (
MAMBA_CMD_ESTOP, MAMBA_CMD_ESTOP,
MAMBA_CMD_MODE, MAMBA_CMD_MODE,
MAMBA_CMD_VELOCITY, MAMBA_CMD_VELOCITY,

4
jetson/ros2_ws/src/saltybot_can_bridge/test/test_can_bridge.py
View File

@ -1,6 +1,6 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
""" """
Unit tests for saltybot_can_bridge.mamba_protocol. Unit tests for saltybot_can_bridge.balance_protocol.
No ROS2 or CAN hardware required — tests exercise encode/decode round-trips No ROS2 or CAN hardware required — tests exercise encode/decode round-trips
and boundary conditions entirely in Python. and boundary conditions entirely in Python.
@ -11,7 +11,7 @@ Run with: pytest test/test_can_bridge.py -v
import struct import struct
import unittest import unittest
from saltybot_can_bridge.mamba_protocol import ( from saltybot_can_bridge.balance_protocol import (
MAMBA_CMD_ESTOP, MAMBA_CMD_ESTOP,
MAMBA_CMD_MODE, MAMBA_CMD_MODE,
MAMBA_CMD_VELOCITY, MAMBA_CMD_VELOCITY,

10
jetson/ros2_ws/src/saltybot_can_e2e_test/saltybot_can_e2e_test/protocol_defs.py
View File

@ -6,7 +6,7 @@ Orin↔Mamba↔VESC integration test suite.
All IDs and payload formats are derived from: All IDs and payload formats are derived from:
include/orin_can.h — Orin↔FC (Mamba) protocol include/orin_can.h — Orin↔FC (Mamba) protocol
include/vesc_can.h — VESC CAN protocol include/vesc_can.h — VESC CAN protocol
saltybot_can_bridge/mamba_protocol.py — existing bridge constants saltybot_can_bridge/balance_protocol.py — existing bridge constants
CAN IDs used in tests CAN IDs used in tests
--------------------- ---------------------
@ -22,7 +22,7 @@ FC (Mamba) → Orin telemetry (standard 11-bit, matching orin_can.h):
FC_IMU 0x402 8 bytes FC_IMU 0x402 8 bytes
FC_BARO 0x403 8 bytes FC_BARO 0x403 8 bytes
Mamba ↔ VESC internal commands (matching mamba_protocol.py): Mamba ↔ VESC internal commands (matching balance_protocol.py):
MAMBA_CMD_VELOCITY 0x100 8 bytes left_mps (f32) | right_mps (f32) big-endian MAMBA_CMD_VELOCITY 0x100 8 bytes left_mps (f32) | right_mps (f32) big-endian
MAMBA_CMD_MODE 0x101 1 byte mode (0=idle,1=drive,2=estop) MAMBA_CMD_MODE 0x101 1 byte mode (0=idle,1=drive,2=estop)
MAMBA_CMD_ESTOP 0x102 1 byte 0x01=stop MAMBA_CMD_ESTOP 0x102 1 byte 0x01=stop
@ -54,7 +54,7 @@ FC_IMU: int = 0x402
FC_BARO: int = 0x403 FC_BARO: int = 0x403
# --------------------------------------------------------------------------- # ---------------------------------------------------------------------------
# Mamba → VESC internal command IDs (from mamba_protocol.py) # Mamba → VESC internal command IDs (from balance_protocol.py)
# --------------------------------------------------------------------------- # ---------------------------------------------------------------------------
MAMBA_CMD_VELOCITY: int = 0x100 MAMBA_CMD_VELOCITY: int = 0x100
@ -136,14 +136,14 @@ def build_estop_cmd(action: int = 1) -> bytes:
# --------------------------------------------------------------------------- # ---------------------------------------------------------------------------
# Frame builders — Mamba velocity commands (mamba_protocol.py encoding) # Frame builders — Mamba velocity commands (balance_protocol.py encoding)
# --------------------------------------------------------------------------- # ---------------------------------------------------------------------------
def build_velocity_cmd(left_mps: float, right_mps: float) -> bytes: def build_velocity_cmd(left_mps: float, right_mps: float) -> bytes:
""" """
Build a MAMBA_CMD_VELOCITY payload (8 bytes, 2 × float32 big-endian). Build a MAMBA_CMD_VELOCITY payload (8 bytes, 2 × float32 big-endian).
Matches encode_velocity_cmd() in mamba_protocol.py. Matches encode_velocity_cmd() in balance_protocol.py.
""" """
return struct.pack(">ff", float(left_mps), float(right_mps)) return struct.pack(">ff", float(left_mps), float(right_mps))

6
jetson/ros2_ws/src/saltybot_can_e2e_test/test/conftest.py
View File

@ -14,7 +14,7 @@ _pkg_root = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
if _pkg_root not in sys.path: if _pkg_root not in sys.path:
sys.path.insert(0, _pkg_root) sys.path.insert(0, _pkg_root)
# Also add the saltybot_can_bridge package so we can import mamba_protocol. # Also add the saltybot_can_bridge package so we can import balance_protocol.
_bridge_pkg = os.path.join( _bridge_pkg = os.path.join(
os.path.dirname(_pkg_root), "saltybot_can_bridge" os.path.dirname(_pkg_root), "saltybot_can_bridge"
) )
@ -60,7 +60,7 @@ def loopback_can_bus():
@pytest.fixture(scope="function") @pytest.fixture(scope="function")
def bridge_components(): def bridge_components():
""" """
Return the mamba_protocol encode/decode callables and a fresh mock bus. Return the balance_protocol encode/decode callables and a fresh mock bus.
Yields a dict with keys: Yields a dict with keys:
bus — MockCANBus instance bus — MockCANBus instance
@ -69,7 +69,7 @@ def bridge_components():
encode_estop — encode_estop_cmd(stop) → bytes encode_estop — encode_estop_cmd(stop) → bytes
decode_vesc — decode_vesc_state(data) → VescStateTelemetry decode_vesc — decode_vesc_state(data) → VescStateTelemetry
""" """
from saltybot_can_bridge.mamba_protocol import ( from saltybot_can_bridge.balance_protocol import (
encode_velocity_cmd, encode_velocity_cmd,
encode_mode_cmd, encode_mode_cmd,
encode_estop_cmd, encode_estop_cmd,

2
jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_drive_command.py
View File

@ -28,7 +28,7 @@ from saltybot_can_e2e_test.protocol_defs import (
parse_velocity_cmd, parse_velocity_cmd,
parse_fc_vesc, parse_fc_vesc,
) )
from saltybot_can_bridge.mamba_protocol import ( from saltybot_can_bridge.balance_protocol import (
encode_velocity_cmd, encode_velocity_cmd,
encode_mode_cmd, encode_mode_cmd,
) )

2
jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_estop.py
View File

@ -32,7 +32,7 @@ from saltybot_can_e2e_test.protocol_defs import (
parse_velocity_cmd, parse_velocity_cmd,
parse_fc_status, parse_fc_status,
) )
from saltybot_can_bridge.mamba_protocol import ( from saltybot_can_bridge.balance_protocol import (
encode_velocity_cmd, encode_velocity_cmd,
encode_mode_cmd, encode_mode_cmd,
encode_estop_cmd, encode_estop_cmd,

2
jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_fc_vesc_broadcast.py
View File

@ -30,7 +30,7 @@ from saltybot_can_e2e_test.protocol_defs import (
parse_fc_vesc, parse_fc_vesc,
parse_vesc_status, parse_vesc_status,
) )
from saltybot_can_bridge.mamba_protocol import ( from saltybot_can_bridge.balance_protocol import (
VESC_TELEM_STATE as BRIDGE_VESC_TELEM_STATE, VESC_TELEM_STATE as BRIDGE_VESC_TELEM_STATE,
decode_vesc_state, decode_vesc_state,
) )

2
jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_heartbeat_timeout.py
View File

@ -33,7 +33,7 @@ from saltybot_can_e2e_test.protocol_defs import (
build_velocity_cmd, build_velocity_cmd,
parse_velocity_cmd, parse_velocity_cmd,
) )
from saltybot_can_bridge.mamba_protocol import ( from saltybot_can_bridge.balance_protocol import (
encode_velocity_cmd, encode_velocity_cmd,
encode_mode_cmd, encode_mode_cmd,
encode_estop_cmd, encode_estop_cmd,

8
jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_mode_switching.py
View File

@ -27,7 +27,7 @@ from saltybot_can_e2e_test.protocol_defs import (
build_velocity_cmd, build_velocity_cmd,
parse_velocity_cmd, parse_velocity_cmd,
) )
from saltybot_can_bridge.mamba_protocol import ( from saltybot_can_bridge.balance_protocol import (
encode_velocity_cmd, encode_velocity_cmd,
encode_mode_cmd, encode_mode_cmd,
encode_estop_cmd, encode_estop_cmd,
@ -189,7 +189,7 @@ class TestModeCommandEncoding:
"""build_mode_cmd in protocol_defs must produce identical bytes.""" """build_mode_cmd in protocol_defs must produce identical bytes."""
for mode in (MODE_IDLE, MODE_DRIVE, MODE_ESTOP): for mode in (MODE_IDLE, MODE_DRIVE, MODE_ESTOP):
assert build_mode_cmd(mode) == encode_mode_cmd(mode), \ assert build_mode_cmd(mode) == encode_mode_cmd(mode), \
f"protocol_defs.build_mode_cmd({mode}) != mamba_protocol.encode_mode_cmd({mode})" f"protocol_defs.build_mode_cmd({mode}) != balance_protocol.encode_mode_cmd({mode})"
class TestInvalidMode: class TestInvalidMode:
@ -218,8 +218,8 @@ class TestInvalidMode:
accepted = sm.set_mode(-1) accepted = sm.set_mode(-1)
assert accepted is False assert accepted is False
def test_mamba_protocol_invalid_mode_raises(self): def test_balance_protocol_invalid_mode_raises(self):
"""mamba_protocol.encode_mode_cmd must raise on invalid mode.""" """balance_protocol.encode_mode_cmd must raise on invalid mode."""
with pytest.raises(ValueError): with pytest.raises(ValueError):
encode_mode_cmd(99) encode_mode_cmd(99)
with pytest.raises(ValueError): with pytest.raises(ValueError):

2
jetson/ros2_ws/src/saltybot_gimbal/saltybot_gimbal/jlink_gimbal.py
View File

@ -13,7 +13,7 @@ Telemetry type (STM32 → Jetson):
uint16 pan_speed_raw + uint16 tilt_speed_raw + uint16 pan_speed_raw + uint16 tilt_speed_raw +
uint8 torque_en + uint8 rx_err_pct (10 bytes) uint8 torque_en + uint8 rx_err_pct (10 bytes)
Frame format (shared with stm32_protocol.py): Frame format (shared with esp32_protocol.py):
[STX=0x02][CMD][LEN][PAYLOAD...][CRC16_hi][CRC16_lo][ETX=0x03] [STX=0x02][CMD][LEN][PAYLOAD...][CRC16_hi][CRC16_lo][ETX=0x03]
CRC16-CCITT: poly=0x1021, init=0xFFFF, covers CMD+LEN+PAYLOAD bytes. CRC16-CCITT: poly=0x1021, init=0xFFFF, covers CMD+LEN+PAYLOAD bytes.
""" """