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>
98 lines
3.1 KiB
Python
98 lines
3.1 KiB
Python
"""
|
|
Unit tests for ESP32-S3 telemetry parsing logic.
|
|
Run with: pytest jetson/ros2_ws/src/saltybot_bridge/test/test_parse.py
|
|
"""
|
|
|
|
import json
|
|
import pytest
|
|
|
|
|
|
# ── Minimal stub so we can test parsing without a ROS2 runtime ───────────────
|
|
|
|
def parse_frame(raw: bytes):
|
|
"""Mirror of the parsing logic in serial_bridge_node._parse_and_publish."""
|
|
text = raw.decode("ascii", errors="ignore").strip()
|
|
if not text:
|
|
return None
|
|
data = json.loads(text)
|
|
|
|
if "err" in data:
|
|
return {"type": "fault", "errno": data["err"]}
|
|
|
|
required = ("p", "r", "e", "ig", "m", "s", "y")
|
|
if not all(k in data for k in required):
|
|
raise ValueError(f"Incomplete frame: {data}")
|
|
|
|
return {
|
|
"type": "telemetry",
|
|
"pitch_deg": data["p"] / 10.0,
|
|
"roll_deg": data["r"] / 10.0,
|
|
"yaw_deg": data["y"] / 10.0,
|
|
"pid_error_deg": data["e"] / 10.0,
|
|
"integral": data["ig"] / 10.0,
|
|
"motor_cmd": int(data["m"]),
|
|
"state": int(data["s"]),
|
|
}
|
|
|
|
|
|
# ── Tests ─────────────────────────────────────────────────────────────────────
|
|
|
|
def test_normal_frame():
|
|
raw = b'{"p":125,"r":-30,"e":50,"ig":20,"m":350,"s":1,"y":0}\n'
|
|
result = parse_frame(raw)
|
|
assert result["type"] == "telemetry"
|
|
assert result["pitch_deg"] == pytest.approx(12.5)
|
|
assert result["roll_deg"] == pytest.approx(-3.0)
|
|
assert result["yaw_deg"] == pytest.approx(0.0)
|
|
assert result["pid_error_deg"] == pytest.approx(5.0)
|
|
assert result["integral"] == pytest.approx(2.0)
|
|
assert result["motor_cmd"] == 350
|
|
assert result["state"] == 1 # ARMED
|
|
|
|
|
|
def test_fault_frame():
|
|
raw = b'{"err":-1}\n'
|
|
result = parse_frame(raw)
|
|
assert result["type"] == "fault"
|
|
assert result["errno"] == -1
|
|
|
|
|
|
def test_zero_frame():
|
|
raw = b'{"p":0,"r":0,"e":0,"ig":0,"m":0,"s":0,"y":0}\n'
|
|
result = parse_frame(raw)
|
|
assert result["type"] == "telemetry"
|
|
assert result["pitch_deg"] == pytest.approx(0.0)
|
|
assert result["state"] == 0 # DISARMED
|
|
|
|
|
|
def test_tilt_fault_state():
|
|
raw = b'{"p":450,"r":0,"e":400,"ig":999,"m":1000,"s":2,"y":0}\n'
|
|
result = parse_frame(raw)
|
|
assert result["state"] == 2 # TILT_FAULT
|
|
assert result["pitch_deg"] == pytest.approx(45.0)
|
|
assert result["motor_cmd"] == 1000
|
|
|
|
|
|
def test_negative_motor_cmd():
|
|
raw = b'{"p":-100,"r":0,"e":-100,"ig":-50,"m":-750,"s":1,"y":10}\n'
|
|
result = parse_frame(raw)
|
|
assert result["motor_cmd"] == -750
|
|
assert result["pitch_deg"] == pytest.approx(-10.0)
|
|
|
|
|
|
def test_incomplete_frame_raises():
|
|
raw = b'{"p":100,"r":0}\n'
|
|
with pytest.raises(ValueError, match="Incomplete frame"):
|
|
parse_frame(raw)
|
|
|
|
|
|
def test_empty_line_returns_none():
|
|
assert parse_frame(b"\n") is None
|
|
assert parse_frame(b" \n") is None
|
|
|
|
|
|
def test_corrupt_frame_raises_json_error():
|
|
raw = b'not_json\n'
|
|
with pytest.raises(json.JSONDecodeError):
|
|
parse_frame(raw)
|