From 7e4f632268c3144c9e254ed7260548600c094e14 Mon Sep 17 00:00:00 2001 From: cyy_mac Date: Thu, 30 Jul 2026 19:23:47 +0800 Subject: [PATCH] =?UTF-8?q?cpp=20sdk=E6=9B=B4=E6=96=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- fast_lowcmd_cpp/fast_mcu.py | 48 ++--- fast_lowcmd_cpp/go1_fast_lowcmd.cpp | 293 ++++++++++++++++++++++++---- fast_lowcmd_cpp/test_fast_lowcmd.py | 23 +++ 3 files changed, 303 insertions(+), 61 deletions(-) diff --git a/fast_lowcmd_cpp/fast_mcu.py b/fast_lowcmd_cpp/fast_mcu.py index b11f5b3..a166f91 100644 --- a/fast_lowcmd_cpp/fast_mcu.py +++ b/fast_lowcmd_cpp/fast_mcu.py @@ -1,12 +1,12 @@ -"""Fast MCU client using native LowState decrypt/parse. +"""Fast MCU client using native LowState decrypt/parse and UDP drain. 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_fast_lowcmd import FastNativeMCUClient from go1_pro_sdk import MCU_IP, MCU_PORT from go1_pro_sdk.utils.constants import RCVBUF_SIZE @@ -127,16 +127,19 @@ class FastMCUClient: 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] + resolved_state_path = state_path or default_state_path() + self.builder = FastLowCmdBuilder(resolved_state_path) + self._native = FastNativeMCUClient( + resolved_state_path, + mcu_ip, + int(mcu_port), + int(local_port), + int(RCVBUF_SIZE), + endian, + ) + self.local_port = self._native.local_port() self._last_state = None - self.backend = "cpp_lowcmd_cpp_lowstate" + self.backend = "cpp_lowcmd_cpp_lowstate_cpp_udp" def __enter__(self): return self @@ -145,27 +148,16 @@ class FastMCUClient: self.close() def close(self): - if self.sock: - self.sock.close() - self.sock = None + native = getattr(self, "_native", None) + if native is not None: + native.close() + self._native = None def send_raw(self, raw_cipher): - self.sock.sendto(raw_cipher, (self.mcu_ip, self.mcu_port)) - return len(raw_cipher) + return self._native.send_raw(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) + fields = self._native.recv_latest_fields() if fields is None: return None self._last_state = FastLowState(fields) diff --git a/fast_lowcmd_cpp/go1_fast_lowcmd.cpp b/fast_lowcmd_cpp/go1_fast_lowcmd.cpp index 64b1084..079e403 100644 --- a/fast_lowcmd_cpp/go1_fast_lowcmd.cpp +++ b/fast_lowcmd_cpp/go1_fast_lowcmd.cpp @@ -1,6 +1,7 @@ #define PY_SSIZE_T_CLEAN #include #include +#include #include #include #include @@ -9,6 +10,12 @@ #include #include +#include +#include +#include +#include +#include + namespace { constexpr Py_ssize_t LOWCMD_SIZE = 616; @@ -460,6 +467,31 @@ struct FastLowCmdBuilder { uint32_t S[4][256]; }; +bool load_blowfish_state(const char* state_path, uint32_t P[18], uint32_t S[4][256]) { + std::ifstream f(state_path, std::ios::binary); + if (!f) { + PyErr_Format(PyExc_FileNotFoundError, "cannot open Blowfish state file: %s", state_path); + return false; + } + std::array buf{}; + f.read(reinterpret_cast(buf.data()), static_cast(buf.size())); + if (f.gcount() < static_cast(buf.size())) { + PyErr_Format(PyExc_ValueError, "Blowfish state file must contain at least 4168 bytes: %s", state_path); + return false; + } + const uint8_t* p = buf.data(); + for (int i = 0; i < 18; ++i) { + P[i] = get_u32_le(p + i * 4); + } + p += 18 * 4; + for (int s = 0; s < 4; ++s) { + for (int i = 0; i < 256; ++i) { + S[s][i] = get_u32_le(p + (s * 256 + i) * 4); + } + } + return true; +} + uint32_t bf_f(FastLowCmdBuilder* self, uint32_t x) { return (((self->S[0][(x >> 24) & 0xffu] + self->S[1][(x >> 16) & 0xffu]) & 0xffffffffu) ^ self->S[2][(x >> 8) & 0xffu]) + self->S[3][x & 0xffu]; @@ -623,28 +655,7 @@ int FastLowCmdBuilder_init(FastLowCmdBuilder* self, PyObject* args, PyObject* kw return -1; } - std::ifstream f(state_path, std::ios::binary); - if (!f) { - PyErr_Format(PyExc_FileNotFoundError, "cannot open Blowfish state file: %s", state_path); - return -1; - } - std::array buf{}; - f.read(reinterpret_cast(buf.data()), static_cast(buf.size())); - if (f.gcount() < static_cast(buf.size())) { - PyErr_Format(PyExc_ValueError, "Blowfish state file must contain at least 4168 bytes: %s", state_path); - return -1; - } - const uint8_t* p = buf.data(); - for (int i = 0; i < 18; ++i) { - self->P[i] = get_u32_le(p + i * 4); - } - p += 18 * 4; - for (int s = 0; s < 4; ++s) { - for (int i = 0; i < 256; ++i) { - self->S[s][i] = get_u32_le(p + (s * 256 + i) * 4); - } - } - return 0; + return load_blowfish_state(state_path, self->P, self->S) ? 0 : -1; } PyObject* FastLowCmdBuilder_build_plain_damping(FastLowCmdBuilder*, PyObject*) { @@ -865,27 +876,19 @@ 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); +PyObject* parse_lowstate_packet(FastLowCmdBuilder* self, const uint8_t* cipher, Py_ssize_t len) { + if (len < 807) { Py_RETURN_NONE; } - Py_ssize_t aligned = (view.len / 8) * 8; + Py_ssize_t aligned = (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; @@ -983,6 +986,207 @@ PyObject* FastLowCmdBuilder_decrypt_lowstate(FastLowCmdBuilder* self, PyObject* return out; } +PyObject* FastLowCmdBuilder_decrypt_lowstate(FastLowCmdBuilder* self, PyObject* args) { + Py_buffer view; + if (!PyArg_ParseTuple(args, "y*", &view)) { + return nullptr; + } + PyObject* result = parse_lowstate_packet( + self, reinterpret_cast(view.buf), view.len); + PyBuffer_Release(&view); + return result; +} + +struct FastNativeMCUClient { + PyObject_HEAD + int sock_fd; + int local_port; + FastLowCmdBuilder codec; +}; + +void close_native_socket(FastNativeMCUClient* self) { + if (self->sock_fd >= 0) { + close(self->sock_fd); + self->sock_fd = -1; + } +} + +PyObject* FastNativeMCUClient_new(PyTypeObject* type, PyObject*, PyObject*) { + auto* self = reinterpret_cast(type->tp_alloc(type, 0)); + if (self) { + self->sock_fd = -1; + self->local_port = 0; + std::memset(&self->codec, 0, sizeof(self->codec)); + } + return reinterpret_cast(self); +} + +void FastNativeMCUClient_dealloc(FastNativeMCUClient* self) { + close_native_socket(self); + Py_TYPE(self)->tp_free(reinterpret_cast(self)); +} + +int set_nonblocking(int fd) { + int flags = fcntl(fd, F_GETFL, 0); + if (flags < 0) { + return -1; + } + return fcntl(fd, F_SETFL, flags | O_NONBLOCK); +} + +int FastNativeMCUClient_init(FastNativeMCUClient* self, PyObject* args, PyObject* kwargs) { + const char* state_path = nullptr; + const char* mcu_ip = "192.168.123.10"; + const char* endian = "little"; + int mcu_port = 8007; + int local_port = 0; + int rcvbuf_size = 4096; + static const char* kwlist[] = { + "state_path", "mcu_ip", "mcu_port", "local_port", "rcvbuf_size", "endian", nullptr}; + if (!PyArg_ParseTupleAndKeywords( + args, kwargs, "s|siiis", const_cast(kwlist), + &state_path, &mcu_ip, &mcu_port, &local_port, &rcvbuf_size, &endian)) { + return -1; + } + if (std::strcmp(endian, "little") != 0) { + PyErr_SetString(PyExc_ValueError, "FastNativeMCUClient currently supports only little-endian Blowfish state"); + return -1; + } + close_native_socket(self); + if (!load_blowfish_state(state_path, self->codec.P, self->codec.S)) { + return -1; + } + + self->sock_fd = socket(AF_INET, SOCK_DGRAM, 0); + if (self->sock_fd < 0) { + PyErr_SetFromErrno(PyExc_OSError); + return -1; + } + int reuse = 1; + setsockopt(self->sock_fd, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse)); + if (rcvbuf_size > 0) { + setsockopt(self->sock_fd, SOL_SOCKET, SO_RCVBUF, &rcvbuf_size, sizeof(rcvbuf_size)); + } + if (set_nonblocking(self->sock_fd) < 0) { + PyErr_SetFromErrno(PyExc_OSError); + close_native_socket(self); + return -1; + } + + sockaddr_in local{}; + local.sin_family = AF_INET; + local.sin_addr.s_addr = htonl(INADDR_ANY); + local.sin_port = htons(static_cast(local_port)); + if (bind(self->sock_fd, reinterpret_cast(&local), sizeof(local)) < 0) { + PyErr_SetFromErrno(PyExc_OSError); + close_native_socket(self); + return -1; + } + + sockaddr_in target{}; + target.sin_family = AF_INET; + target.sin_port = htons(static_cast(mcu_port)); + if (inet_pton(AF_INET, mcu_ip, &target.sin_addr) != 1) { + PyErr_Format(PyExc_ValueError, "invalid IPv4 target: %s", mcu_ip); + close_native_socket(self); + return -1; + } + if (connect(self->sock_fd, reinterpret_cast(&target), sizeof(target)) < 0) { + PyErr_SetFromErrno(PyExc_OSError); + close_native_socket(self); + return -1; + } + + sockaddr_in actual{}; + socklen_t actual_len = sizeof(actual); + if (getsockname(self->sock_fd, reinterpret_cast(&actual), &actual_len) == 0) { + self->local_port = ntohs(actual.sin_port); + } else { + self->local_port = local_port; + } + return 0; +} + +PyObject* FastNativeMCUClient_close(FastNativeMCUClient* self, PyObject*) { + close_native_socket(self); + Py_RETURN_NONE; +} + +PyObject* FastNativeMCUClient_local_port(FastNativeMCUClient* self, PyObject*) { + return PyLong_FromLong(self->local_port); +} + +PyObject* FastNativeMCUClient_send_raw(FastNativeMCUClient* self, PyObject* args) { + if (self->sock_fd < 0) { + PyErr_SetString(PyExc_RuntimeError, "FastNativeMCUClient is closed"); + return nullptr; + } + Py_buffer view; + if (!PyArg_ParseTuple(args, "y*", &view)) { + return nullptr; + } + ssize_t sent = 0; + Py_BEGIN_ALLOW_THREADS + sent = send(self->sock_fd, view.buf, static_cast(view.len), 0); + Py_END_ALLOW_THREADS + PyBuffer_Release(&view); + if (sent < 0) { + PyErr_SetFromErrno(PyExc_OSError); + return nullptr; + } + return PyLong_FromSsize_t(sent); +} + +PyObject* FastNativeMCUClient_recv_latest_fields(FastNativeMCUClient* self, PyObject*) { + if (self->sock_fd < 0) { + PyErr_SetString(PyExc_RuntimeError, "FastNativeMCUClient is closed"); + return nullptr; + } + + std::array buffer{}; + std::array latest{}; + ssize_t latest_len = -1; + while (true) { + ssize_t received = 0; + Py_BEGIN_ALLOW_THREADS + received = recv(self->sock_fd, buffer.data(), buffer.size(), 0); + Py_END_ALLOW_THREADS + if (received > 0) { + latest_len = received; + std::memcpy(latest.data(), buffer.data(), static_cast(received)); + continue; + } + if (received == 0) { + break; + } + if (errno == EINTR) { + continue; + } + if (errno == EAGAIN || errno == EWOULDBLOCK) { + break; + } + PyErr_SetFromErrno(PyExc_OSError); + return nullptr; + } + + if (latest_len < 0) { + Py_RETURN_NONE; + } + return parse_lowstate_packet(&self->codec, latest.data(), latest_len); +} + +PyMethodDef FastNativeMCUClient_methods[] = { + {"close", reinterpret_cast(FastNativeMCUClient_close), METH_NOARGS, + "Close the native UDP socket."}, + {"local_port", reinterpret_cast(FastNativeMCUClient_local_port), METH_NOARGS, + "Return the bound local UDP port."}, + {"send_raw", reinterpret_cast(FastNativeMCUClient_send_raw), METH_VARARGS, + "Send an already encrypted LowCmd packet through the native UDP socket."}, + {"recv_latest_fields", reinterpret_cast(FastNativeMCUClient_recv_latest_fields), METH_NOARGS, + "Drain pending UDP packets and return parsed fields for the newest LowState packet."}, + {nullptr, nullptr, 0, nullptr}, +}; + PyMethodDef FastLowCmdBuilder_methods[] = { {"build_plain_damping", reinterpret_cast(FastLowCmdBuilder_build_plain_damping), METH_NOARGS, "Build a plain all-damping LowCmd packet."}, @@ -1015,6 +1219,10 @@ PyTypeObject FastLowCmdBuilderType = { PyVarObject_HEAD_INIT(nullptr, 0) }; +PyTypeObject FastNativeMCUClientType = { + PyVarObject_HEAD_INIT(nullptr, 0) +}; + PyModuleDef module = { PyModuleDef_HEAD_INIT, "go1_fast_lowcmd", @@ -1038,6 +1246,19 @@ PyMODINIT_FUNC PyInit_go1_fast_lowcmd(void) { return nullptr; } + FastNativeMCUClientType.tp_name = "go1_fast_lowcmd.FastNativeMCUClient"; + FastNativeMCUClientType.tp_basicsize = sizeof(FastNativeMCUClient); + FastNativeMCUClientType.tp_flags = Py_TPFLAGS_DEFAULT; + FastNativeMCUClientType.tp_doc = "Native UDP MCU client with C++ LowState drain/decrypt/parse."; + FastNativeMCUClientType.tp_new = FastNativeMCUClient_new; + FastNativeMCUClientType.tp_init = reinterpret_cast(FastNativeMCUClient_init); + FastNativeMCUClientType.tp_dealloc = reinterpret_cast(FastNativeMCUClient_dealloc); + FastNativeMCUClientType.tp_methods = FastNativeMCUClient_methods; + + if (PyType_Ready(&FastNativeMCUClientType) < 0) { + return nullptr; + } + PyObject* m = PyModule_Create(&module); if (!m) { return nullptr; @@ -1048,6 +1269,12 @@ PyMODINIT_FUNC PyInit_go1_fast_lowcmd(void) { Py_DECREF(m); return nullptr; } + Py_INCREF(&FastNativeMCUClientType); + if (PyModule_AddObject(m, "FastNativeMCUClient", reinterpret_cast(&FastNativeMCUClientType)) < 0) { + Py_DECREF(&FastNativeMCUClientType); + Py_DECREF(m); + return nullptr; + } PyModule_AddIntConstant(m, "LOWCMD_SIZE", LOWCMD_SIZE); return m; } diff --git a/fast_lowcmd_cpp/test_fast_lowcmd.py b/fast_lowcmd_cpp/test_fast_lowcmd.py index b8c1f3d..70a4fbe 100644 --- a/fast_lowcmd_cpp/test_fast_lowcmd.py +++ b/fast_lowcmd_cpp/test_fast_lowcmd.py @@ -3,6 +3,8 @@ import os import pytest from fast_lowcmd import FastLowCmdBuilder, default_state_path +from fast_mcu import FastMCUClient +from go1_fast_lowcmd import FastNativeMCUClient 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 @@ -140,9 +142,30 @@ def test_state_file_is_required(): FastLowCmdBuilder(os.path.join(os.path.dirname(__file__), "missing_state.bin")) +def test_native_mcu_client_empty_recv(): + client = FastNativeMCUClient(default_state_path(), "127.0.0.1", 8007, 0, 4096, "little") + try: + assert client.local_port() > 0 + assert client.recv_latest_fields() is None + finally: + client.close() + + +def test_fast_mcu_uses_native_udp_client(): + client = FastMCUClient(mcu_ip="127.0.0.1", mcu_port=8007, local_port=0) + try: + assert client.backend == "cpp_lowcmd_cpp_lowstate_cpp_udp" + assert client.local_port > 0 + assert client.recv_latest() is None + finally: + client.close() + + 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") + if not os.path.exists(packet_path): + pytest.skip("private LowState capture is not available") with open(packet_path, "rb") as f: packet = f.read()