75 lines
2.0 KiB
C++
75 lines
2.0 KiB
C++
#include "../2025_include_user/Imu.h"
|
|
|
|
uint8_t Imu::_buffer = 0;
|
|
Imu::Data Imu::_data = {};
|
|
static uint8_t frame_index = 0;
|
|
static uint8_t frame_buffer[11];
|
|
|
|
extern "C" void IMU_INST_IRQHandler(void) {
|
|
switch (DL_UART_getPendingInterrupt(IMU_INST)) {
|
|
case DL_UART_IIDX_RX:
|
|
Imu::_buffer = DL_UART_Main_receiveData(IMU_INST);
|
|
Imu::inject();
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
|
|
NVIC_ClearPendingIRQ(IMU_INST_INT_IRQN);
|
|
DL_UART_enableInterrupt(IMU_INST, DL_UART_INTERRUPT_RX); // 启用接收中断
|
|
}
|
|
|
|
void Imu::init() {
|
|
NVIC_ClearPendingIRQ(IMU_INST_INT_IRQN);
|
|
NVIC_EnableIRQ(IMU_INST_INT_IRQN);
|
|
DL_UART_enable(IMU_INST); // 启用 UART 模块
|
|
DL_UART_enableInterrupt(IMU_INST, DL_UART_INTERRUPT_RX); // 启用接收中断
|
|
|
|
}
|
|
|
|
void Imu::inject() {
|
|
if (frame_index == 0 && _buffer == 0x55) {
|
|
frame_buffer[frame_index++] = _buffer;
|
|
} else if (frame_index == 1 && _buffer == 0x55) {
|
|
frame_buffer[frame_index++] = _buffer;
|
|
} else if (frame_index >= 2) {
|
|
frame_buffer[frame_index++] = _buffer;
|
|
if (frame_index >= 11) {
|
|
uint8_t sum = 0;
|
|
for (int i = 0; i < 10; i++) {
|
|
sum += frame_buffer[i];
|
|
}
|
|
|
|
if (sum == frame_buffer[10] && frame_buffer[2] == 0x01 && frame_buffer[3] == 0x06) {
|
|
int16_t roll = (frame_buffer[5] << 8) | frame_buffer[4];
|
|
int16_t pitch = (frame_buffer[7] << 8) | frame_buffer[6];
|
|
int16_t yaw = (frame_buffer[9] << 8) | frame_buffer[8];
|
|
|
|
_data.roll = (float) roll / 32768.0f * 180.0f;
|
|
_data.pitch = (float) pitch / 32768.0f * 180.0f;
|
|
_data.yaw = (float) yaw / 32768.0f * 180.0f;
|
|
}
|
|
frame_index = 0;
|
|
}
|
|
} else {
|
|
frame_index = 0;
|
|
}
|
|
}
|
|
|
|
Imu::Data Imu::getData() {
|
|
return _data;
|
|
}
|
|
|
|
float Imu::getRoll() {
|
|
return _data.roll;
|
|
}
|
|
|
|
float Imu::getPitch() {
|
|
return _data.pitch;
|
|
}
|
|
|
|
float Imu::getYaw() {
|
|
return _data.yaw;
|
|
}
|
|
|