| #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"); |
|
|
| } |
|
|
| 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; |
| } |
|
|
