From 81f14e14ca759d52f2419a6f21bbc283e977f21e Mon Sep 17 00:00:00 2001 From: cyy_mac Date: Tue, 28 Jul 2026 16:01:13 +0800 Subject: [PATCH] =?UTF-8?q?=E8=A7=A3=E7=A0=81=E7=A7=BB=E5=8A=A8=E7=BD=AEcp?= =?UTF-8?q?p?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- fast_lowcmd_cpp/fast_lowcmd.py | 3 + fast_lowcmd_cpp/fast_mcu.py | 204 ++++++++++++++++++++++++ fast_lowcmd_cpp/go1_fast_lowcmd.cpp | 237 ++++++++++++++++++++++++++++ fast_lowcmd_cpp/test_fast_lowcmd.py | 24 +++ 4 files changed, 468 insertions(+) create mode 100644 fast_lowcmd_cpp/fast_mcu.py diff --git a/fast_lowcmd_cpp/fast_lowcmd.py b/fast_lowcmd_cpp/fast_lowcmd.py index 3087c6b..45ab122 100644 --- a/fast_lowcmd_cpp/fast_lowcmd.py +++ b/fast_lowcmd_cpp/fast_lowcmd.py @@ -65,3 +65,6 @@ class FastLowCmdBuilder: def encrypt(self, data): return self._native.encrypt(data) + + def decrypt_lowstate(self, data): + return self._native.decrypt_lowstate(data) diff --git a/fast_lowcmd_cpp/fast_mcu.py b/fast_lowcmd_cpp/fast_mcu.py new file mode 100644 index 0000000..b11f5b3 --- /dev/null +++ b/fast_lowcmd_cpp/fast_mcu.py @@ -0,0 +1,204 @@ +"""Fast MCU client using native LowState decrypt/parse. + +The public shape mirrors go1_pro_sdk.connection.MCUClient for deployment code, +while keeping the receive hot path out of pure Python. +""" +import socket +import time + +from fast_lowcmd import FastLowCmdBuilder, default_state_path +from go1_pro_sdk import MCU_IP, MCU_PORT +from go1_pro_sdk.utils.constants import RCVBUF_SIZE + + +class FastIMU: + __slots__ = ("quaternion", "gyroscope", "accelerometer", "rpy", "temperature") + + def __init__(self, fields): + self.quaternion = tuple(fields["imu_quaternion"]) + self.gyroscope = tuple(fields["imu_gyroscope"]) + self.accelerometer = (0.0, 0.0, 0.0) + self.rpy = tuple(fields["imu_rpy"]) + self.temperature = 0 + + +class FastMotorState: + __slots__ = ("mode", "q", "dq", "ddq", "tauEst", "q_raw", "dq_raw", "ddq_raw", "temperature", "reserve") + + def __init__(self, fields, i): + self.mode = int(fields["motor_mode"][i]) + self.q = float(fields["motor_q"][i]) + self.dq = float(fields["motor_dq"][i]) + self.ddq = float(fields["motor_ddq"][i]) + self.tauEst = float(fields["motor_tau"][i]) + self.q_raw = float(fields["motor_q_raw"][i]) + self.dq_raw = float(fields["motor_dq_raw"][i]) + self.ddq_raw = float(fields["motor_ddq_raw"][i]) + self.temperature = int(fields["motor_temperature"][i]) + self.reserve = [int(fields["motor_reserve0"][i]), int(fields["motor_reserve1"][i])] + + +class FastBMS: + __slots__ = ( + "version_h", "version_l", "bms_status", "SOC", "current", "cycle", + "BQ_NTC", "MCU_NTC", "cell_vol", + ) + + def __init__(self, fields): + self.version_h = int(fields["bms_version_h"]) + self.version_l = int(fields["bms_version_l"]) + self.bms_status = int(fields["bms_status"]) + self.SOC = int(fields["bms_soc"]) + self.current = int(fields["bms_current"]) + self.cycle = int(fields["bms_cycle"]) + self.BQ_NTC = [int(fields["bms_bq_ntc0"]), int(fields["bms_bq_ntc1"])] + self.MCU_NTC = [int(fields["bms_mcu_ntc0"]), int(fields["bms_mcu_ntc1"])] + self.cell_vol = [int(x) for x in fields["bms_cell_vol"]] + + @property + def voltage_mv(self): + return sum(self.cell_vol) + + @property + def voltage_v(self): + return self.voltage_mv / 1000.0 + + @property + def current_a(self): + return self.current / 1000.0 + + +class FastRemoteState: + __slots__ = ("head", "btn", "lx", "ly", "rx", "ry", "L2", "pressed") + + def __init__(self, fields): + axes = fields["remote_axes"] + self.head = b"\x55\xaa" + self.btn = int(fields["remote_btn"]) + self.lx = float(axes[0]) + self.ly = float(axes[1]) + self.rx = float(axes[2]) + self.ry = float(axes[3]) + self.L2 = float(axes[4]) + self.pressed = list(fields["remote_pressed"]) + + def is_pressed(self, name): + return name in self.pressed + + def any_button(self): + return self.btn != 0 + + +class FastLowState: + __slots__ = ( + "head", "levelFlag", "frameReserve", "SN", "version", "bandWidth", + "imu", "motorState", "footForce", "footForceEst", "bms", "tick", + "wirelessRemote", "reserve", "crc", "remote", + ) + + def __init__(self, fields): + self.head = int(fields["head"]) + self.levelFlag = int(fields["levelFlag"]) + self.frameReserve = int(fields["frameReserve"]) + self.SN = b"\x00" * 8 + self.version = b"\x00" * 8 + self.bandWidth = int(fields["bandWidth"]) + self.imu = FastIMU(fields) + self.motorState = [FastMotorState(fields, i) for i in range(20)] + self.footForce = (0, 0, 0, 0) + self.footForceEst = (0, 0, 0, 0) + self.bms = FastBMS(fields) + self.tick = 0 + self.wirelessRemote = b"\x00" * 40 + self.reserve = b"\x00" * 4 + self.crc = b"\x00" * 4 + self.remote = FastRemoteState(fields) + + +class FastMCUClient: + def __init__( + self, + state_path=None, + mcu_ip=MCU_IP, + mcu_port=MCU_PORT, + local_port=0, + endian="little"): + if endian != "little": + raise ValueError("FastMCUClient currently supports only little-endian Blowfish state") + self.mcu_ip = mcu_ip + self.mcu_port = mcu_port + self.builder = FastLowCmdBuilder(state_path or default_state_path()) + + sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + sock.setblocking(False) + sock.setsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF, RCVBUF_SIZE) + sock.bind(("", local_port)) + self.sock = sock + self.local_port = sock.getsockname()[1] + self._last_state = None + self.backend = "cpp_lowcmd_cpp_lowstate" + + def __enter__(self): + return self + + def __exit__(self, *args): + self.close() + + def close(self): + if self.sock: + self.sock.close() + self.sock = None + + def send_raw(self, raw_cipher): + self.sock.sendto(raw_cipher, (self.mcu_ip, self.mcu_port)) + return len(raw_cipher) + + def recv_latest(self): + last_data = None + while True: + try: + data, _ = self.sock.recvfrom(2048) + last_data = data + except (BlockingIOError, socket.timeout): + break + + if last_data is None: + return None + + fields = self.builder.decrypt_lowstate(last_data) + if fields is None: + return None + self._last_state = FastLowState(fields) + return self._last_state + + def recv_state(self, timeout=1.0): + deadline = time.time() + timeout + while time.time() < deadline: + state = self.recv_latest() + if state is not None: + return state + time.sleep(0.001) + return None + + @property + def last_state(self): + return self._last_state + + def wake_mcu(self, n_frames=50, dt=0.01): + damping = self.builder.build_encrypted_damping() + recv_count = 0 + for _ in range(n_frames): + self.send_raw(damping) + time.sleep(dt) + if self.recv_latest() is not None: + recv_count += 1 + return recv_count + + def safe_stop(self, n_frames=50, dt=0.002): + damping = self.builder.build_encrypted_damping() + for _ in range(n_frames): + try: + self.send_raw(damping) + except Exception: + pass + time.sleep(dt) diff --git a/fast_lowcmd_cpp/go1_fast_lowcmd.cpp b/fast_lowcmd_cpp/go1_fast_lowcmd.cpp index 8114ff1..e62ec5d 100644 --- a/fast_lowcmd_cpp/go1_fast_lowcmd.cpp +++ b/fast_lowcmd_cpp/go1_fast_lowcmd.cpp @@ -7,6 +7,7 @@ #include #include #include +#include namespace { @@ -483,6 +484,42 @@ void encrypt_block_le(FastLowCmdBuilder* self, const uint8_t* in, uint8_t* out) put_u32_le(out + 4, R); } +void decrypt_block_le(FastLowCmdBuilder* self, const uint8_t* in, uint8_t* out) { + uint32_t L = get_u32_le(in); + uint32_t R = get_u32_le(in + 4); + for (int i = 17; i > 1; --i) { + L ^= self->P[i]; + R ^= bf_f(self, L); + uint32_t tmp = L; + L = R; + R = tmp; + } + uint32_t tmp = L; + L = R; + R = tmp; + R ^= self->P[1]; + L ^= self->P[0]; + put_u32_le(out, L); + put_u32_le(out + 4, R); +} + +inline float get_f32_le(const uint8_t* p) { + uint32_t raw = get_u32_le(p); + float value; + static_assert(sizeof(raw) == sizeof(value), "float must be 32-bit"); + std::memcpy(&value, &raw, sizeof(value)); + return value; +} + +inline int16_t get_i16_le(const uint8_t* p) { + return static_cast( + static_cast(p[0]) | (static_cast(p[1]) << 8)); +} + +inline int32_t get_i32_le(const uint8_t* p) { + return static_cast(get_u32_le(p)); +} + PyObject* encrypt_bytes(FastLowCmdBuilder* self, const uint8_t* data, Py_ssize_t len) { if (len % 8 != 0) { PyErr_SetString(PyExc_ValueError, "data length must be a multiple of 8"); @@ -499,6 +536,86 @@ PyObject* encrypt_bytes(FastLowCmdBuilder* self, const uint8_t* data, Py_ssize_t return result; } +PyObject* make_float_list(const double* values, int count) { + PyObject* list = PyList_New(count); + if (!list) { + return nullptr; + } + for (int i = 0; i < count; ++i) { + PyObject* v = PyFloat_FromDouble(values[i]); + if (!v) { + Py_DECREF(list); + return nullptr; + } + PyList_SET_ITEM(list, i, v); + } + return list; +} + +PyObject* make_int_list(const int* values, int count) { + PyObject* list = PyList_New(count); + if (!list) { + return nullptr; + } + for (int i = 0; i < count; ++i) { + PyObject* v = PyLong_FromLong(values[i]); + if (!v) { + Py_DECREF(list); + return nullptr; + } + PyList_SET_ITEM(list, i, v); + } + return list; +} + +bool dict_set_steals(PyObject* dict, const char* key, PyObject* value) { + if (!value) { + return false; + } + int ok = PyDict_SetItemString(dict, key, value); + Py_DECREF(value); + return ok == 0; +} + +bool dict_set_long(PyObject* dict, const char* key, long value) { + PyObject* obj = PyLong_FromLong(value); + return dict_set_steals(dict, key, obj); +} + +PyObject* parse_remote_pressed(uint16_t btn) { + struct ButtonName { + uint16_t mask; + const char* name; + }; + static constexpr ButtonName buttons[] = { + {0x0001, "R1"}, {0x0002, "L1"}, {0x0004, "START"}, {0x0008, "SELECT"}, + {0x0010, "R2"}, {0x0020, "L2"}, {0x0040, "F1"}, {0x0080, "F2"}, + {0x0100, "A"}, {0x0200, "B"}, {0x0400, "X"}, {0x0800, "Y"}, + {0x1000, "UP"}, {0x2000, "RIGHT"}, {0x4000, "DOWN"}, {0x8000, "LEFT"}, + }; + PyObject* list = PyList_New(0); + if (!list) { + return nullptr; + } + for (const auto& b : buttons) { + if ((btn & b.mask) == 0) { + continue; + } + PyObject* s = PyUnicode_FromString(b.name); + if (!s) { + Py_DECREF(list); + return nullptr; + } + if (PyList_Append(list, s) < 0) { + Py_DECREF(s); + Py_DECREF(list); + return nullptr; + } + Py_DECREF(s); + } + return list; +} + int FastLowCmdBuilder_init(FastLowCmdBuilder* self, PyObject* args, PyObject* kwargs) { const char* state_path = nullptr; static const char* kwlist[] = {"state_path", nullptr}; @@ -748,6 +865,124 @@ PyObject* FastLowCmdBuilder_encrypt(FastLowCmdBuilder* self, PyObject* args) { return result; } +PyObject* FastLowCmdBuilder_decrypt_lowstate(FastLowCmdBuilder* self, PyObject* args) { + Py_buffer view; + if (!PyArg_ParseTuple(args, "y*", &view)) { + return nullptr; + } + if (view.len < 807) { + PyBuffer_Release(&view); + Py_RETURN_NONE; + } + Py_ssize_t aligned = (view.len / 8) * 8; + if (aligned < 807) { + PyBuffer_Release(&view); + Py_RETURN_NONE; + } + + const uint8_t* cipher = reinterpret_cast(view.buf); + std::vector data(static_cast(aligned)); + for (Py_ssize_t i = 0; i < aligned; i += 8) { + decrypt_block_le(self, cipher + i, data.data() + i); + } + PyBuffer_Release(&view); + + if (data[0] != 0xfe || data[1] != 0xef || data[2] != 0xff || data[3] != 0x00) { + Py_RETURN_NONE; + } + + double quat[4], gyro[3], rpy[3]; + for (int i = 0; i < 4; ++i) { + quat[i] = static_cast(get_f32_le(data.data() + 22 + i * 4)); + } + for (int i = 0; i < 3; ++i) { + gyro[i] = static_cast(get_f32_le(data.data() + 22 + 16 + i * 4)); + rpy[i] = static_cast(get_f32_le(data.data() + 22 + 40 + i * 4)); + } + + double q[MOTOR_COUNT], dq[MOTOR_COUNT], tau[MOTOR_COUNT], q_raw[MOTOR_COUNT], dq_raw[MOTOR_COUNT]; + int mode[MOTOR_COUNT], ddq[MOTOR_COUNT], ddq_raw[MOTOR_COUNT], temperature[MOTOR_COUNT]; + int reserve0[MOTOR_COUNT], reserve1[MOTOR_COUNT]; + for (int i = 0; i < MOTOR_COUNT; ++i) { + const uint8_t* m = data.data() + 75 + i * 32; + mode[i] = static_cast(m[0]); + q[i] = static_cast(get_f32_le(m + 1)); + dq[i] = static_cast(get_f32_le(m + 5)); + ddq[i] = static_cast(get_i16_le(m + 9)); + tau[i] = static_cast(get_i16_le(m + 11)) * 0.00390625; + q_raw[i] = static_cast(get_f32_le(m + 13)); + dq_raw[i] = static_cast(get_f32_le(m + 17)); + ddq_raw[i] = static_cast(get_i16_le(m + 21)); + temperature[i] = static_cast(m[24]); + reserve0[i] = static_cast(get_u32_le(m + 24)); + reserve1[i] = static_cast(get_u32_le(m + 28)); + } + + const uint8_t* bms = data.data() + 715; + int bms_current = static_cast(get_i32_le(bms + 4)); + int cell_vol[10]; + for (int i = 0; i < 10; ++i) { + cell_vol[i] = static_cast( + static_cast(bms[14 + i * 2]) + | (static_cast(bms[15 + i * 2]) << 8)); + } + + const uint8_t* remote = data.data() + 759; + uint16_t btn = static_cast(remote[2]) | (static_cast(remote[3]) << 8); + double remote_vals[5] = { + static_cast(get_f32_le(remote + 4)), // lx + static_cast(get_f32_le(remote + 20)), // ly + static_cast(get_f32_le(remote + 8)), // rx + static_cast(get_f32_le(remote + 12)), // ry + static_cast(get_f32_le(remote + 16)), // L2 + }; + + PyObject* out = PyDict_New(); + if (!out) { + return nullptr; + } + if (!dict_set_steals(out, "imu_quaternion", make_float_list(quat, 4)) + || !dict_set_steals(out, "imu_gyroscope", make_float_list(gyro, 3)) + || !dict_set_steals(out, "imu_rpy", make_float_list(rpy, 3)) + || !dict_set_steals(out, "motor_q", make_float_list(q, MOTOR_COUNT)) + || !dict_set_steals(out, "motor_dq", make_float_list(dq, MOTOR_COUNT)) + || !dict_set_steals(out, "motor_tau", make_float_list(tau, MOTOR_COUNT)) + || !dict_set_steals(out, "motor_q_raw", make_float_list(q_raw, MOTOR_COUNT)) + || !dict_set_steals(out, "motor_dq_raw", make_float_list(dq_raw, MOTOR_COUNT)) + || !dict_set_steals(out, "motor_mode", make_int_list(mode, MOTOR_COUNT)) + || !dict_set_steals(out, "motor_ddq", make_int_list(ddq, MOTOR_COUNT)) + || !dict_set_steals(out, "motor_ddq_raw", make_int_list(ddq_raw, MOTOR_COUNT)) + || !dict_set_steals(out, "motor_temperature", make_int_list(temperature, MOTOR_COUNT)) + || !dict_set_steals(out, "motor_reserve0", make_int_list(reserve0, MOTOR_COUNT)) + || !dict_set_steals(out, "motor_reserve1", make_int_list(reserve1, MOTOR_COUNT)) + || !dict_set_steals(out, "bms_cell_vol", make_int_list(cell_vol, 10)) + || !dict_set_steals(out, "remote_axes", make_float_list(remote_vals, 5)) + || !dict_set_steals(out, "remote_pressed", parse_remote_pressed(btn))) { + Py_DECREF(out); + return nullptr; + } + + if (!dict_set_long(out, "head", static_cast(data[0] | (data[1] << 8))) + || !dict_set_long(out, "levelFlag", data[2]) + || !dict_set_long(out, "frameReserve", data[3]) + || !dict_set_long(out, "bandWidth", static_cast(data[20] | (data[21] << 8))) + || !dict_set_long(out, "bms_version_h", bms[0]) + || !dict_set_long(out, "bms_version_l", bms[1]) + || !dict_set_long(out, "bms_status", bms[2]) + || !dict_set_long(out, "bms_soc", bms[3]) + || !dict_set_long(out, "bms_current", bms_current) + || !dict_set_long(out, "bms_cycle", static_cast(bms[8] | (bms[9] << 8))) + || !dict_set_long(out, "bms_bq_ntc0", bms[10]) + || !dict_set_long(out, "bms_bq_ntc1", bms[11]) + || !dict_set_long(out, "bms_mcu_ntc0", bms[12]) + || !dict_set_long(out, "bms_mcu_ntc1", bms[13]) + || !dict_set_long(out, "remote_btn", btn)) { + Py_DECREF(out); + return nullptr; + } + return out; +} + PyMethodDef FastLowCmdBuilder_methods[] = { {"build_plain_damping", reinterpret_cast(FastLowCmdBuilder_build_plain_damping), METH_NOARGS, "Build a plain all-damping LowCmd packet."}, @@ -771,6 +1006,8 @@ PyMethodDef FastLowCmdBuilder_methods[] = { "Build an encrypted LowCmd packet from a Python LowCmd object."}, {"encrypt", reinterpret_cast(FastLowCmdBuilder_encrypt), METH_VARARGS, "Encrypt a bytes-like object with the loaded Blowfish state."}, + {"decrypt_lowstate", reinterpret_cast(FastLowCmdBuilder_decrypt_lowstate), METH_VARARGS, + "Decrypt and parse a LowState UDP packet into primitive Python fields."}, {nullptr, nullptr, 0, nullptr}, }; diff --git a/fast_lowcmd_cpp/test_fast_lowcmd.py b/fast_lowcmd_cpp/test_fast_lowcmd.py index e4524fc..b8c1f3d 100644 --- a/fast_lowcmd_cpp/test_fast_lowcmd.py +++ b/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)