cpp sdk更新

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())
+        resolved_state_path = state_path or default_state_path()
-
+        self.builder = FastLowCmdBuilder(resolved_state_path)
-        sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+        self._native = FastNativeMCUClient(
-        sock.setblocking(False)
+            resolved_state_path,
-        sock.setsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF, RCVBUF_SIZE)
+            mcu_ip,
-        sock.bind(("", local_port))
+            int(mcu_port),
-        self.sock = sock
+            int(local_port),
-        self.local_port = sock.getsockname()[1]
+            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:
+        native = getattr(self, "_native", None)
-            self.sock.close()
+        if native is not None:
-            self.sock = None
+            native.close()
+            self._native = None
 
     def send_raw(self, raw_cipher):
-        self.sock.sendto(raw_cipher, (self.mcu_ip, self.mcu_port))
+        return self._native.send_raw(raw_cipher)
-        return len(raw_cipher)
 
     def recv_latest(self):
-        last_data = None
+        fields = self._native.recv_latest_fields()
-        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)

fast_lowcmd_cpp/go1_fast_lowcmd.cpp

@@ -1,6 +1,7 @@
 #define PY_SSIZE_T_CLEAN
 #include <Python.h>
 #include <array>
+#include <cerrno>
 #include <cmath>
 #include <cstdio>
 #include <cstdint>
@@ -9,6 +10,12 @@
 #include <string>
 #include <vector>
 
+#include <arpa/inet.h>
+#include <fcntl.h>
+#include <netinet/in.h>
+#include <sys/socket.h>
+#include <unistd.h>
+
 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<uint8_t, 4168> buf{};
+    f.read(reinterpret_cast<char*>(buf.data()), static_cast<std::streamsize>(buf.size()));
+    if (f.gcount() < static_cast<std::streamsize>(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);
+    return load_blowfish_state(state_path, self->P, self->S) ? 0 : -1;
-    if (!f) {
-        PyErr_Format(PyExc_FileNotFoundError, "cannot open Blowfish state file: %s", state_path);
-        return -1;
-    }
-    std::array<uint8_t, 4168> buf{};
-    f.read(reinterpret_cast<char*>(buf.data()), static_cast<std::streamsize>(buf.size()));
-    if (f.gcount() < static_cast<std::streamsize>(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;
 }
 
 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) {
+PyObject* parse_lowstate_packet(FastLowCmdBuilder* self, const uint8_t* cipher, Py_ssize_t len) {
-    Py_buffer view;
+    if (len < 807) {
-    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;
+    Py_ssize_t aligned = (len / 8) * 8;
     if (aligned < 807) {
-        PyBuffer_Release(&view);
         Py_RETURN_NONE;
     }
 
-    const uint8_t* cipher = reinterpret_cast<const uint8_t*>(view.buf);
     std::vector<uint8_t> data(static_cast<size_t>(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<const uint8_t*>(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<FastNativeMCUClient*>(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<PyObject*>(self);
+}
+
+void FastNativeMCUClient_dealloc(FastNativeMCUClient* self) {
+    close_native_socket(self);
+    Py_TYPE(self)->tp_free(reinterpret_cast<PyObject*>(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<char**>(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<uint16_t>(local_port));
+    if (bind(self->sock_fd, reinterpret_cast<sockaddr*>(&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<uint16_t>(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<sockaddr*>(&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<sockaddr*>(&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<size_t>(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<uint8_t, 2048> buffer{};
+    std::array<uint8_t, 2048> 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<size_t>(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<PyCFunction>(FastNativeMCUClient_close), METH_NOARGS,
+     "Close the native UDP socket."},
+    {"local_port", reinterpret_cast<PyCFunction>(FastNativeMCUClient_local_port), METH_NOARGS,
+     "Return the bound local UDP port."},
+    {"send_raw", reinterpret_cast<PyCFunction>(FastNativeMCUClient_send_raw), METH_VARARGS,
+     "Send an already encrypted LowCmd packet through the native UDP socket."},
+    {"recv_latest_fields", reinterpret_cast<PyCFunction>(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<PyCFunction>(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<initproc>(FastNativeMCUClient_init);
+    FastNativeMCUClientType.tp_dealloc = reinterpret_cast<destructor>(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<PyObject*>(&FastNativeMCUClientType)) < 0) {
+        Py_DECREF(&FastNativeMCUClientType);
+        Py_DECREF(m);
+        return nullptr;
+    }
     PyModule_AddIntConstant(m, "LOWCMD_SIZE", LOWCMD_SIZE);
     return m;
 }

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()