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

解码移动置cpp

Browse Source
This commit is contained in:
cyy_mac
2026-07-28 16:01:13 +08:00
parent 8d8b171952
commit 81f14e14ca
4 changed files with 468 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

237
fast_lowcmd_cpp/go1_fast_lowcmd.cpp
View File

@@ -7,6 +7,7 @@
#include <cstring>
#include <fstream>
#include <string>
#include <vector>
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<int16_t>(
static_cast<uint16_t>(p[0]) | (static_cast<uint16_t>(p[1]) << 8));
}
inline int32_t get_i32_le(const uint8_t* p) {
return static_cast<int32_t>(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<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;
}
double quat[4], gyro[3], rpy[3];
for (int i = 0; i < 4; ++i) {
quat[i] = static_cast<double>(get_f32_le(data.data() + 22 + i * 4));
}
for (int i = 0; i < 3; ++i) {
gyro[i] = static_cast<double>(get_f32_le(data.data() + 22 + 16 + i * 4));
rpy[i] = static_cast<double>(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<int>(m[0]);
q[i] = static_cast<double>(get_f32_le(m + 1));
dq[i] = static_cast<double>(get_f32_le(m + 5));
ddq[i] = static_cast<int>(get_i16_le(m + 9));
tau[i] = static_cast<double>(get_i16_le(m + 11)) * 0.00390625;
q_raw[i] = static_cast<double>(get_f32_le(m + 13));
dq_raw[i] = static_cast<double>(get_f32_le(m + 17));
ddq_raw[i] = static_cast<int>(get_i16_le(m + 21));
temperature[i] = static_cast<int>(m[24]);
reserve0[i] = static_cast<int>(get_u32_le(m + 24));
reserve1[i] = static_cast<int>(get_u32_le(m + 28));
}
const uint8_t* bms = data.data() + 715;
int bms_current = static_cast<int>(get_i32_le(bms + 4));
int cell_vol[10];
for (int i = 0; i < 10; ++i) {
cell_vol[i] = static_cast<int>(
static_cast<uint16_t>(bms[14 + i * 2])
| (static_cast<uint16_t>(bms[15 + i * 2]) << 8));
}
const uint8_t* remote = data.data() + 759;
uint16_t btn = static_cast<uint16_t>(remote[2]) | (static_cast<uint16_t>(remote[3]) << 8);
double remote_vals[5] = {
static_cast<double>(get_f32_le(remote + 4)), // lx
static_cast<double>(get_f32_le(remote + 20)), // ly
static_cast<double>(get_f32_le(remote + 8)), // rx
static_cast<double>(get_f32_le(remote + 12)), // ry
static_cast<double>(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<long>(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<long>(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<long>(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<PyCFunction>(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<PyCFunction>(FastLowCmdBuilder_encrypt), METH_VARARGS,
"Encrypt a bytes-like object with the loaded Blowfish state."},
{"decrypt_lowstate", reinterpret_cast<PyCFunction>(FastLowCmdBuilder_decrypt_lowstate), METH_VARARGS,
"Decrypt and parse a LowState UDP packet into primitive Python fields."},
{nullptr, nullptr, 0, nullptr},
};