File size: 4,022 Bytes
d8290eb | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 | #include "cpp_lowcmd_bridge/motor_crc_hg.hpp"
#include <algorithm>
#include <cstddef>
#include <cstring>
namespace {
static_assert(sizeof(MotorCmd) == 28, "unexpected H2 MotorCmd CRC layout");
static_assert(sizeof(LowCmd) == 1004, "unexpected H2 LowCmd CRC layout");
struct ImuStateCrcLayout {
std::array<float, 4> quaternion;
std::array<float, 3> gyroscope;
std::array<float, 3> accelerometer;
std::array<float, 3> rpy;
int16_t temperature;
};
struct MotorStateCrcLayout {
uint8_t mode;
float q;
float dq;
float ddq;
float tau_est;
std::array<int16_t, 2> temperature;
float vol;
std::array<uint32_t, 2> sensor;
uint32_t motorstate;
std::array<uint32_t, 4> reserve;
};
struct LowStateCrcLayout {
std::array<uint32_t, 2> version;
uint8_t mode_pr;
uint8_t mode_machine;
uint32_t tick;
ImuStateCrcLayout imu_state;
std::array<MotorStateCrcLayout, 35> motor_state;
std::array<uint8_t, 40> wireless_remote;
std::array<uint32_t, 4> reserve;
uint32_t crc;
};
static_assert(sizeof(ImuStateCrcLayout) == 56, "unexpected H2 IMU CRC layout");
static_assert(sizeof(MotorStateCrcLayout) == 56, "unexpected H2 motor-state CRC layout");
static_assert(sizeof(LowStateCrcLayout) == 2092, "unexpected H2 LowState CRC layout");
static_assert(offsetof(LowStateCrcLayout, crc) == 2088, "unexpected H2 LowState CRC offset");
} // namespace
void get_crc(unitree_hg::msg::LowCmd &msg) {
LowCmd raw{};
raw.modePr = msg.mode_pr;
raw.modeMachine = msg.mode_machine;
for (int i = 0; i < 35; i++) {
raw.motorCmd[i].mode = msg.motor_cmd[i].mode;
raw.motorCmd[i].q = msg.motor_cmd[i].q;
raw.motorCmd[i].dq = msg.motor_cmd[i].dq;
raw.motorCmd[i].tau = msg.motor_cmd[i].tau;
raw.motorCmd[i].Kp = msg.motor_cmd[i].kp;
raw.motorCmd[i].Kd = msg.motor_cmd[i].kd;
raw.motorCmd[i].reserve = msg.motor_cmd[i].reserve;
}
std::copy(msg.reserve.begin(), msg.reserve.end(), raw.reserve.begin());
std::array<uint32_t, sizeof(LowCmd) / sizeof(uint32_t)> words{};
std::memcpy(words.data(), &raw, sizeof(raw));
raw.crc = crc32_core(words.data(), static_cast<uint32_t>(words.size() - 1));
msg.crc = raw.crc;
}
bool low_state_crc_valid(const unitree_hg::msg::LowState &msg) {
LowStateCrcLayout raw{};
raw.version = msg.version;
raw.mode_pr = msg.mode_pr;
raw.mode_machine = msg.mode_machine;
raw.tick = msg.tick;
raw.imu_state.quaternion = msg.imu_state.quaternion;
raw.imu_state.gyroscope = msg.imu_state.gyroscope;
raw.imu_state.accelerometer = msg.imu_state.accelerometer;
raw.imu_state.rpy = msg.imu_state.rpy;
raw.imu_state.temperature = msg.imu_state.temperature;
for (size_t index = 0; index < raw.motor_state.size(); ++index) {
const auto &source = msg.motor_state[index];
auto &target = raw.motor_state[index];
target.mode = source.mode;
target.q = source.q;
target.dq = source.dq;
target.ddq = source.ddq;
target.tau_est = source.tau_est;
target.temperature = source.temperature;
target.vol = source.vol;
target.sensor = source.sensor;
target.motorstate = source.motorstate;
target.reserve = source.reserve;
}
raw.wireless_remote = msg.wireless_remote;
raw.reserve = msg.reserve;
raw.crc = msg.crc;
std::array<uint32_t, sizeof(LowStateCrcLayout) / sizeof(uint32_t)> words{};
std::memcpy(words.data(), &raw, sizeof(raw));
const uint32_t expected = crc32_core(
words.data(), static_cast<uint32_t>(words.size() - 1));
return msg.crc == expected;
}
uint32_t crc32_core(const uint32_t *ptr, uint32_t len) {
uint32_t xbit = 0;
uint32_t data = 0;
uint32_t CRC32 = 0xFFFFFFFF;
const uint32_t dwPolynomial = 0x04c11db7;
for (uint32_t i = 0; i < len; i++) {
xbit = 1 << 31;
data = ptr[i];
for (uint32_t bits = 0; bits < 32; bits++) {
if (CRC32 & 0x80000000) {
CRC32 <<= 1;
CRC32 ^= dwPolynomial;
} else
CRC32 <<= 1;
if (data & xbit) CRC32 ^= dwPolynomial;
xbit >>= 1;
}
}
return CRC32;
}
|