bb35ddd56d
- Resolve 73 committed conflict markers (bulk resolution taking theirs/ESP32 side) - Rename all MAMBA_CMD_* → BALANCE_CMD_*, MAMBA_TELEM_* → BALANCE_TELEM_* - Rename FC_STATUS/VESC/IMU/BARO → BALANCE_STATUS/VESC/IMU/BARO in protocol_defs.py - Update can_bridge_node.py: fix imports, replace legacy encode/decode calls with balance_protocol equivalents (encode_velocity_cmd, encode_mode_cmd, decode_imu_telem, decode_battery_telem, decode_vesc_state); fix watchdog and destroy_node - Rename stm32_protocol.py/stm32_cmd_node.py → esp32_protocol.py/esp32_cmd_node.py - Delete mamba_protocol.py; stm32_cmd.launch.py/stm32_cmd_params.yaml archived - Update can_bridge_params.yaml: mamba_can_id → balance_can_id - Update docs/AGENTS.md, SALTYLAB.md, wiring-diagram.md for ESP32-S3 architecture - Update test/test_ota.py sys.path to legacy/stm32/scripts/flash_firmware.py - No legacy STM32/Mamba refs remain outside legacy/ and SAUL-TEE-SYSTEM-REFERENCE.md Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
97 lines
3.1 KiB
Python
97 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)
|