解码移动置cpp

fast_lowcmd_cpp/test_fast_lowcmd.py

@@ -6,6 +6,7 @@ from fast_lowcmd import FastLowCmdBuilder, default_state_path
 from go1_pro_sdk import Blowfish, LowCmd, MotorCmd, MotorMode, PowerProtectViolation
 from go1_pro_sdk import apply_safety
 from go1_pro_sdk.codec.lowcmd_builder import build_low_cmd_encrypted, build_low_cmd_plain
+from go1_pro_sdk.codec.lowstate_parser import parse_low_state
 
 
 @pytest.fixture
@@ -137,3 +138,26 @@ def test_lowcmd_object_matches_python(builder, bf):
 def test_state_file_is_required():
     with pytest.raises(FileNotFoundError):
         FastLowCmdBuilder(os.path.join(os.path.dirname(__file__), "missing_state.bin"))
+
+
+def test_lowstate_decrypt_matches_python(builder, bf):
+    root = os.path.dirname(os.path.dirname(__file__))
+    packet_path = os.path.join(root, "data", "captures", "mcu_response.bin")
+    with open(packet_path, "rb") as f:
+        packet = f.read()
+
+    fields = builder.decrypt_lowstate(packet)
+    decrypted = bf.decrypt_ecb(packet[: (len(packet) // 8) * 8])
+    state = parse_low_state(decrypted)
+
+    assert fields is not None
+    assert fields["bms_soc"] == state.bms.SOC
+    assert fields["remote_pressed"] == state.remote.pressed
+    for i in range(20):
+        assert fields["motor_mode"][i] == state.motorState[i].mode
+        assert fields["motor_q"][i] == pytest.approx(state.motorState[i].q)
+        assert fields["motor_dq"][i] == pytest.approx(state.motorState[i].dq)
+        assert fields["motor_tau"][i] == pytest.approx(state.motorState[i].tauEst)
+    assert fields["imu_quaternion"] == pytest.approx(state.imu.quaternion)
+    assert fields["imu_gyroscope"] == pytest.approx(state.imu.gyroscope)
+    assert fields["imu_rpy"] == pytest.approx(state.imu.rpy)