156 lines
4.7 KiB
C
156 lines
4.7 KiB
C
/*
|
||
N100 陀螺仪数据
|
||
*/
|
||
|
||
/* Includes ----------------------------------------------------------------- */
|
||
#include "n100.h"
|
||
|
||
#include <string.h>
|
||
#include "device/device.h"
|
||
|
||
#include "bsp/uart.h"
|
||
#include "component/user_math.h"
|
||
#include "component/crc8.h"
|
||
#include "component/crc16.h"
|
||
/* Private define ----------------------------------------------------------- */
|
||
#define RX_BUFF_SIZE 56
|
||
#define N100_BAG_HEAD 0xFC
|
||
#define N100_BAG_END 0xFD
|
||
// #define RX_BUFF_SIZE (sizeof(N100_AHRS_t))
|
||
/* Private macro ------------------------------------------------------------ */
|
||
/* Private typedef ---------------------------------------------------------- */
|
||
/* Private variables -------------------------------------------------------- */
|
||
static volatile uint32_t drop_message = 0;
|
||
|
||
static uint8_t rxbuf[RX_BUFF_SIZE] __attribute__ ((section(".AXI_SRAM")));
|
||
|
||
// static N100_AHRS_t rxbuf __attribute__((section(".AXI_SRAM")));
|
||
|
||
static bool inited = false;
|
||
|
||
static osThreadId_t thread_alert;
|
||
|
||
/* Private function -------------------------------------------------------- */
|
||
|
||
static void N100_RxCpltCallback(void) {
|
||
osThreadFlagsSet(thread_alert, SIGNAL_N100_NEW_DATA);
|
||
}
|
||
|
||
static void N100_ErrorCallback(void) {
|
||
N100_Restart();
|
||
}
|
||
|
||
static void N100_Decode(N100_t *n100, const uint8_t *rxbuf) {
|
||
if (n100 == NULL || rxbuf == NULL) return;
|
||
|
||
size_t offset = 0;
|
||
|
||
// header
|
||
n100->rx_raw.header.head = rxbuf[offset++];
|
||
n100->rx_raw.header.cmd = rxbuf[offset++];
|
||
n100->rx_raw.header.len = rxbuf[offset++];
|
||
n100->rx_raw.header.count = rxbuf[offset++];
|
||
n100->rx_raw.header.crc8 = rxbuf[offset++];
|
||
|
||
// data.crc16
|
||
n100->rx_raw.data.crc16 = (uint16_t)(rxbuf[offset] | (rxbuf[offset+1] << 8));
|
||
offset += 2;
|
||
|
||
//都是float32
|
||
memcpy(&n100->rx_raw.data.gyro.x, rxbuf + offset, 4);
|
||
offset += 4;
|
||
memcpy(&n100->rx_raw.data.gyro.y, rxbuf + offset, 4);
|
||
offset += 4;
|
||
memcpy(&n100->rx_raw.data.gyro.z, rxbuf + offset, 4);
|
||
offset += 4;
|
||
|
||
memcpy(&n100->rx_raw.data.eulr.rol, rxbuf + offset, 4);
|
||
offset += 4;
|
||
memcpy(&n100->rx_raw.data.eulr.pit, rxbuf + offset, 4);
|
||
offset += 4;
|
||
memcpy(&n100->rx_raw.data.eulr.yaw, rxbuf + offset, 4);
|
||
offset += 4;
|
||
// data.quat
|
||
memcpy(&n100->rx_raw.data.quat, rxbuf + offset, sizeof(AHRS_Quaternion_t));
|
||
offset += sizeof(AHRS_Quaternion_t);
|
||
|
||
// data.time
|
||
memcpy(&n100->rx_raw.data.time, rxbuf + offset, sizeof(int64_t));
|
||
offset += sizeof(int64_t);
|
||
|
||
// end
|
||
n100->rx_raw.end = rxbuf[offset];
|
||
}
|
||
|
||
// 直接对原始数据 rxbuf 校验
|
||
static bool N100_Verify(const uint8_t *rxbuf) {
|
||
if (rxbuf == NULL) return false;
|
||
// 检查包头包尾
|
||
if (rxbuf[0] != N100_BAG_HEAD || rxbuf[RX_BUFF_SIZE - 1] != N100_BAG_END) {
|
||
drop_message++;
|
||
return false;
|
||
}
|
||
// // CRC8 校验（包头4字节）
|
||
// if (rxbuf[4] != crc8_calc(rxbuf, 4)) {
|
||
// drop_message++;
|
||
// return false;
|
||
// }
|
||
// // CRC16 校验（数据段，从第7字节开始，长度为RX_BUFF_SIZE-8）
|
||
// uint16_t crc16 = (uint16_t)(rxbuf[5] | (rxbuf[6] << 8));
|
||
// if (crc16 != crc16_calc(rxbuf + 7, RX_BUFF_SIZE - 8)) {
|
||
// drop_message++;
|
||
// return false;
|
||
// }
|
||
return true;
|
||
}
|
||
|
||
/* Exported functions ------------------------------------------------------- */
|
||
int8_t N100_Init(N100_t *n100){
|
||
ASSERT(n100);
|
||
if (inited) return DEVICE_ERR_INITED;
|
||
if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
|
||
|
||
BSP_UART_RegisterCallback(BSP_UART_IMU_N100, BSP_UART_RX_CPLT_CB, N100_RxCpltCallback);
|
||
BSP_UART_RegisterCallback(BSP_UART_IMU_N100, BSP_UART_ERROR_CB, N100_ErrorCallback);
|
||
inited = true;
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
int8_t N100_Restart(void) {
|
||
__HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_IMU_N100));
|
||
__HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_IMU_N100));
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
int8_t N100_StartReceiving(N100_t *n100) {
|
||
if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_IMU_N100), rxbuf, RX_BUFF_SIZE) != HAL_OK)
|
||
return DEVICE_ERR;
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
bool N100_WaitDmaCplt(void) {
|
||
return (osThreadFlagsWait(SIGNAL_N100_NEW_DATA, osFlagsWaitAll, 200) ==
|
||
SIGNAL_N100_NEW_DATA);
|
||
}
|
||
|
||
int8_t N100_ParseData (N100_t *n100) {
|
||
if (n100 == NULL) return DEVICE_ERR_NULL;
|
||
// 先校验原始数据
|
||
if (!N100_Verify(rxbuf)) {
|
||
return DEVICE_ERR;
|
||
}
|
||
// 校验通过后再解码
|
||
N100_Decode(n100, rxbuf);
|
||
n100->eulr = n100->rx_raw.data.eulr;
|
||
n100->gyro = n100->rx_raw.data.gyro;
|
||
n100->quat = n100->rx_raw.data.quat;
|
||
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
int8_t N100_HandleOffline(N100_t *n100) {
|
||
if (n100 == NULL) return DEVICE_ERR_NULL;
|
||
/*清空n100结构体*/
|
||
memset(n100, 0, sizeof(*n100));
|
||
return DEVICE_OK;
|
||
} |