解码移动置cpp
This commit is contained in:
cyy_mac
@@ -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)
|
||||
|
||||
204
fast_lowcmd_cpp/fast_mcu.py
Normal file
204
fast_lowcmd_cpp/fast_mcu.py
Normal file
@@ -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)
|
||||
@@ -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},
|
||||
};
|
||||
|
||||
|
||||
@@ -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)
|
||||
|
||||
Reference in New Issue
Block a user