cyy/go1_pro_sdk
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

cpp sdk更新

Browse Source
This commit is contained in:
cyy_mac
2026-07-30 19:23:47 +08:00
parent bd07331734
commit 7e4f632268
3 changed files with 303 additions and 61 deletions
Download Patch File Download Diff File Expand all files Collapse all files

48
fast_lowcmd_cpp/fast_mcu.py
View File

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

293
fast_lowcmd_cpp/go1_fast_lowcmd.cpp
View File

@@ -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);
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;
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<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;
}

23
fast_lowcmd_cpp/test_fast_lowcmd.py
View File

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