205 lines
5.5 KiB
C
205 lines
5.5 KiB
C
#include "nuc.h"
|
||
#include "crc16.h"
|
||
#include <string.h>
|
||
#include "define.h"
|
||
#include "error_detect.h"
|
||
|
||
static volatile uint32_t drop_message = 0;
|
||
|
||
static osThreadId_t thread_alert;
|
||
|
||
uint8_t nucbuf[32]; //接收缓存区
|
||
uint8_t SendBuffer[11]; //发送缓存区
|
||
int b=0;
|
||
int c =0;
|
||
int e=0;
|
||
static void NUC_IdleCallback(void) {
|
||
osThreadFlagsSet(thread_alert,SIGNAL_NUC_RAW_REDY);
|
||
detect_hook(NUC_TOE);
|
||
}
|
||
int8_t NUC_Init(NUC_t *nuc){
|
||
if(nuc == NULL) return DEVICE_ERR_NULL;
|
||
if((thread_alert = osThreadGetId()) == NULL ) return DEVICE_ERR_NULL;
|
||
BSP_UART_RegisterCallback(BSP_UART_NUC,BSP_UART_IDLE_LINE_CB,
|
||
NUC_IdleCallback);
|
||
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
//static void NUC_CBLTCallback(void)
|
||
//{
|
||
// osThreadFlagsSet(thread_alert, SIGNAL_NUC_RAW_REDY);
|
||
// // detect_hook(NUC_TOE);
|
||
//}
|
||
//static void NUC_ERRORCALLBACK(void)
|
||
//{
|
||
// NUC_Restart();
|
||
// uint32_t error_code = HAL_UART_GetError(BSP_UART_GetHandle(BSP_UART_NUC));
|
||
// // osThreadFlagsSet(thread_alert, SIGNAL_NUC_ERROR);
|
||
//}
|
||
//int8_t NUC_Init(NUC_t *nuc)
|
||
//{
|
||
// if (nuc == NULL)
|
||
// return DEVICE_ERR_NULL;
|
||
// if ((thread_alert = osThreadGetId()) == NULL)
|
||
// return DEVICE_ERR_NULL;
|
||
// BSP_UART_RegisterCallback(BSP_UART_NUC, BSP_UART_RX_CPLT_CB,
|
||
// NUC_CBLTCallback);
|
||
// BSP_UART_RegisterCallback(BSP_UART_NUC, BSP_UART_ERROR_CB,
|
||
// NUC_ERRORCALLBACK);
|
||
// return DEVICE_OK;
|
||
//}
|
||
|
||
int8_t NUC_StartReceiving() {
|
||
if (HAL_UARTEx_ReceiveToIdle_DMA(BSP_UART_GetHandle(BSP_UART_NUC),
|
||
(uint8_t *)nucbuf,
|
||
sizeof(nucbuf)) == HAL_OK)
|
||
return DEVICE_OK;
|
||
return DEVICE_ERR;
|
||
}
|
||
int8_t NUC_Restart(void) {
|
||
__HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_NUC));
|
||
__HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_NUC));
|
||
return DEVICE_OK;
|
||
}
|
||
bool_t NUC_WaitDmaCplt(void) {
|
||
return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll,500) ==
|
||
SIGNAL_NUC_RAW_REDY);
|
||
}
|
||
|
||
//发送函数
|
||
int8_t NUC_Send(fp32 *data) {
|
||
union {
|
||
float x[2];
|
||
uint8_t data[8];
|
||
}instance;
|
||
for (int i = 0; i < 2; i++) {
|
||
instance.x[i] = data[i];
|
||
}
|
||
SendBuffer[0] = 0xFC; //发送ID
|
||
SendBuffer[1] = 0x01; //控制帧
|
||
|
||
for(int i = 2; i < 10; i++){
|
||
SendBuffer[i] = instance.data[i-2];
|
||
}
|
||
SendBuffer[10] = 0xFD; //结束符
|
||
|
||
if (HAL_UART_Transmit(BSP_UART_GetHandle(BSP_UART_NUC),
|
||
(uint8_t *)SendBuffer,sizeof(SendBuffer),1000) == HAL_OK){
|
||
b++;
|
||
return DEVICE_OK;
|
||
}
|
||
return DEVICE_ERR;
|
||
}
|
||
|
||
//发送控制函数
|
||
void NUC_Send_control(CMD_NUC_t *n,CMD_t *cmd,Chassis_t *chassis) {
|
||
static fp32 send_data[2];
|
||
|
||
//send_data[0]：校准标志位 1：sick校准成功
|
||
//send_data[1]：锁框锁球标志位 0：锁框 1：锁车
|
||
|
||
//启动校准
|
||
if (chassis->radar_reset_flag == 1) {
|
||
send_data[0] = 1;
|
||
send_data[1] = 0;
|
||
NUC_Send(send_data);
|
||
}
|
||
//锁框模式
|
||
else if(cmd->C_cmd.communicate == NO){
|
||
send_data[0] = 0;
|
||
send_data[1] = 0;
|
||
NUC_Send(send_data);
|
||
}
|
||
//传球模式
|
||
else if(cmd->C_cmd.communicate == YES){
|
||
send_data[0] = 0;
|
||
send_data[1] = 1;
|
||
NUC_Send(send_data);
|
||
}
|
||
}
|
||
|
||
//接收函数
|
||
/*车偏左需右旋，角度为正 车偏右需左旋，角度为负*/
|
||
int8_t NUC_RawParse(CMD_NUC_t *n) {
|
||
if (n == NULL) return DEVICE_ERR_NULL;
|
||
union {
|
||
float x[3];
|
||
char data[12];
|
||
} instance; // 方便在浮点数和字符数组之间进行数据转换
|
||
|
||
if(nucbuf[0]!=HEAD) goto error; //发送ID不是底盘
|
||
else{
|
||
#ifdef nuc_1
|
||
/* 协议格式
|
||
0xFF HEAD
|
||
0x09 控制帧
|
||
0x01 相机帧
|
||
flag fp32
|
||
angle fp32
|
||
yaw fp32
|
||
0xFE TAIL
|
||
*/
|
||
if (nucbuf[15] != TAIL) goto error;
|
||
instance.data[0] = nucbuf[3];
|
||
instance.data[1] = nucbuf[4];
|
||
instance.data[2] = nucbuf[5];
|
||
instance.data[3] = nucbuf[6];
|
||
n->nuc.flag = instance.x[0];
|
||
|
||
instance.data[4] = nucbuf[7];
|
||
instance.data[5] = nucbuf[8];
|
||
instance.data[6] = nucbuf[9];
|
||
instance.data[7] = nucbuf[10];
|
||
n->nuc.angle = instance.x[1]/3.1415926535f*360;
|
||
|
||
instance.data[8] = nucbuf[11];
|
||
instance.data[9] = nucbuf[12];
|
||
instance.data[10] = nucbuf[13];
|
||
instance.data[11] = nucbuf[14];
|
||
n->nuc.yaw = instance.x[2];
|
||
//将 n->nuc.yaw映射到（0，360）
|
||
n->nuc.yaw = instance.x[2]/3.1415926535f*180;
|
||
if(n->nuc.yaw<0) n->nuc.yaw +=360;
|
||
#elif defined(nuc_2)
|
||
/* 协议格式
|
||
0xFF HEAD
|
||
angle fp32 //框的角度
|
||
yaw fp32 //r2的角度
|
||
0xFE TAIL
|
||
*/
|
||
if (nucbuf[17] != TAIL) goto error;
|
||
instance.data[0] = nucbuf[1];
|
||
instance.data[1] = nucbuf[2];
|
||
instance.data[2] = nucbuf[3];
|
||
instance.data[3] = nucbuf[4];
|
||
n->nuc.angle = (instance.x[0] != 0.0f) ? (instance.x[0]/3.1415926535f*360) : 0.0f;
|
||
|
||
instance.data[4] = nucbuf[9];
|
||
instance.data[5] = nucbuf[10];
|
||
instance.data[6] = nucbuf[11];
|
||
instance.data[7] = nucbuf[12];
|
||
n->nuc.yaw = (instance.x[2] != 0.0f) ? (instance.x[2]/3.1415926535f*360) : 0.0f;
|
||
|
||
#endif
|
||
//接收正常时绿灯翻转
|
||
HAL_GPIO_TogglePin(GPIOH,LED_G_Pin);
|
||
e++;
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
error:
|
||
c++;
|
||
//接收不正常时红灯翻转
|
||
HAL_GPIO_TogglePin(GPIOH,LED_R_Pin);
|
||
return DEVICE_ERR;
|
||
}
|
||
|
||
int8_t NUC_HandleOffline(CMD_NUC_t *cmd)
|
||
{
|
||
if(cmd == NULL) return DEVICE_ERR_NULL;
|
||
memset(cmd, 0, sizeof(*cmd));
|
||
return 0;
|
||
}
|
||
|