21 bool init(
UART *uart, uint32_t timeout = 500) {
23 uint32_t start = osKernelGetTickCount();
24 while (osKernelGetTickCount() - start < timeout) {
26 if (!write_reg(0x3D, &data, 1)) {
30 if (!write_reg(0x3B, &data, 1)) {
34 if (!write_reg(0x3D, &data, 1)) {
46 std::array<int16_t, 3> data;
47 if (read_reg(0x1A,
reinterpret_cast<uint8_t *
>(data.data()), 6)) {
48 euler_x_ = data[0] / 900.0f;
49 euler_y_ = data[1] / 900.0f;
50 euler_z_ = data[2] / 900.0f;
75 bool write_reg(uint8_t addr, uint8_t *data, uint8_t size) {
76 std::array<uint8_t, 4> buf = {0xAA, 0x00, addr, size};
78 if (!uart_->
transmit(buf.data(), 4)) {
84 if (!uart_->
receive(buf.data(), 2, 5)) {
87 return buf[0] == 0xEE && buf[1] == 0x01;
90 bool read_reg(uint8_t addr, uint8_t *data, uint8_t size) {
91 std::array<uint8_t, 4> buf = {0xAA, 0x01, addr, size};
93 if (!uart_->
transmit(buf.data(), 4)) {
96 if (!uart_->
receive(buf.data(), 2, 5)) {
99 if (buf[0] != 0xBB || buf[1] != size) {
102 return uart_->
receive(data, size, 5);