sl-firmware
291dd689f8
Archive STM32 firmware to legacy/stm32/: - src/, include/, lib/USB_CDC/, platformio.ini, test stubs, flash_firmware.py - test/test_battery_adc.c, test_hw_button.c, test_pid_schedule.c, test_vesc_can.c, test_can_watchdog.c - USB_CDC_BUG.md Rename: stm32_protocol → esp32_protocol, mamba_protocol → balance_protocol, stm32_cmd_node → esp32_cmd_node, stm32_cmd_params → esp32_cmd_params, stm32_cmd.launch.py → esp32_cmd.launch.py, test_stm32_protocol → test_esp32_protocol, test_stm32_cmd_node → test_esp32_cmd_node Content cleanup across all files: - Mamba F722S → ESP32-S3 BALANCE - BlackPill → ESP32-S3 IO - STM32F722/F7xx → ESP32-S3 - stm32Mode/Version/Port → esp32Mode/Version/Port - STM32 State/Mode labels → ESP32 State/Mode - Jetson Nano → Jetson Orin Nano Super - /dev/stm32 → /dev/esp32 - stm32_bridge → esp32_bridge - STM32 HAL → ESP-IDF docs/SALTYLAB.md: - Update "Drone FC Details" to describe ESP32-S3 BALANCE board (Waveshare ESP32-S3 Touch LCD 1.28) - Replace verbose "Self-Balancing Control" STM32 section with brief note pointing to SAUL-TEE-SYSTEM-REFERENCE.md TEAM.md: Update Embedded Firmware Engineer role to ESP32-S3 / ESP-IDF No new functionality — cleanup only. Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
100 lines
2.9 KiB
Python
100 lines
2.9 KiB
Python
"""
|
|
Unit tests for Jetson→ESP32-S3 command serialization logic.
|
|
Tests Twist→speed/steer conversion and frame formatting.
|
|
Run with: pytest jetson/ros2_ws/src/saltybot_bridge/test/test_cmd.py
|
|
"""
|
|
|
|
import pytest
|
|
|
|
|
|
# ── Minimal stubs (no ROS2 runtime needed) ───────────────────────────────────
|
|
|
|
def _clamp(v, lo, hi):
|
|
return max(lo, min(hi, v))
|
|
|
|
|
|
def twist_to_frame(linear_x, angular_z, speed_scale=1000.0, steer_scale=-500.0):
|
|
"""Mirror of SaltybotCmdNode._cmd_vel_cb frame building."""
|
|
speed = int(_clamp(linear_x * speed_scale, -1000.0, 1000.0))
|
|
steer = int(_clamp(angular_z * steer_scale, -1000.0, 1000.0))
|
|
return f"C{speed},{steer}\n".encode("ascii"), speed, steer
|
|
|
|
|
|
# ── Frame format tests ────────────────────────────────────────────────────────
|
|
|
|
def test_zero_twist_produces_zero_cmd():
|
|
frame, speed, steer = twist_to_frame(0.0, 0.0)
|
|
assert frame == b"C0,0\n"
|
|
assert speed == 0
|
|
assert steer == 0
|
|
|
|
|
|
def test_full_forward():
|
|
frame, speed, steer = twist_to_frame(1.0, 0.0)
|
|
assert frame == b"C1000,0\n"
|
|
assert speed == 1000
|
|
|
|
|
|
def test_full_reverse():
|
|
frame, speed, steer = twist_to_frame(-1.0, 0.0)
|
|
assert frame == b"C-1000,0\n"
|
|
assert speed == -1000
|
|
|
|
|
|
def test_left_turn_positive_angular_z():
|
|
# Default steer_scale=-500: +angular.z → negative steer
|
|
frame, speed, steer = twist_to_frame(0.0, 1.0)
|
|
assert steer == -500
|
|
assert b"C0,-500\n" == frame
|
|
|
|
|
|
def test_right_turn_negative_angular_z():
|
|
frame, speed, steer = twist_to_frame(0.0, -1.0)
|
|
assert steer == 500
|
|
assert b"C0,500\n" == frame
|
|
|
|
|
|
def test_speed_clamped_at_max():
|
|
_, speed, _ = twist_to_frame(5.0, 0.0) # 5 m/s >> 1 m/s max
|
|
assert speed == 1000
|
|
|
|
|
|
def test_speed_clamped_at_min():
|
|
_, speed, _ = twist_to_frame(-5.0, 0.0)
|
|
assert speed == -1000
|
|
|
|
|
|
def test_steer_clamped_at_max():
|
|
# angular.z=-5 rad/s with steer_scale=-500 → +2500 → clamped to +1000
|
|
_, _, steer = twist_to_frame(0.0, -5.0)
|
|
assert steer == 1000
|
|
|
|
|
|
def test_steer_clamped_at_min():
|
|
_, _, steer = twist_to_frame(0.0, 5.0)
|
|
assert steer == -1000
|
|
|
|
|
|
def test_combined_motion():
|
|
frame, speed, steer = twist_to_frame(0.5, -0.4)
|
|
assert speed == 500
|
|
assert steer == int(_clamp(-0.4 * -500.0, -1000.0, 1000.0)) # +200
|
|
assert frame == b"C500,200\n"
|
|
|
|
|
|
def test_custom_scales():
|
|
# speed_scale=500 → 1 m/s = 500 ESC units
|
|
frame, speed, steer = twist_to_frame(1.0, 0.0, speed_scale=500.0, steer_scale=-250.0)
|
|
assert speed == 500
|
|
assert frame == b"C500,0\n"
|
|
|
|
|
|
def test_heartbeat_frame():
|
|
assert b"H\n" == b"H\n" # constant — just verifies expected bytes
|
|
|
|
|
|
def test_zero_cmd_frame():
|
|
"""C0,0\\n must be sent on shutdown."""
|
|
frame, _, _ = twist_to_frame(0.0, 0.0)
|
|
assert frame == b"C0,0\n"
|