使用时,解析一个丢一个数据,通讯协议定义最短接收指令个数为9,所以当接收一个完成指令在进行解析,接收数据格式CC 33 length order data1 ...... datan check1 check2
void UsartQueueDataProcess(void)
{
if(usart1_send.size > 9)
{
int i = 0;
UsartCheckInform check_inform;
if (UsartQueuePop(&usart1_send, &check_inform.Head1) == USART_QUEUE_OK)
{
//printf("-----------------head1--------------------------");
if (check_inform.Head1 == 0xcc) // head1;
{
check_inform.data[0] = 0xcc;
//printf("-----------------head1--------------------------");
if (UsartQueuePop(&usart1_send, &check_inform.Head2) == USART_QUEUE_OK)
{
if (check_inform.Head2 == 0x33) // head2
{
check_inform.data[1] = 0x33;
//printf("-----------------head2--------------------------");
if (UsartQueuePop(&usart1_send, &check_inform.Lenghth) == USART_QUEUE_OK)
{
check_inform.data[2] = check_inform.Lenghth;
// printf("-----------------Lenghth--------------------------");
for (i = 0; i < check_inform.Lenghth; i++)
{
UsartQueuePop(&usart1_send, &check_inform.data[i + 3]);
}
if (Check_Sum(check_inform.data, check_inform.Lenghth + 3) == 1) //校验正确
{
//
switch (check_inform.data[3])
{
case 0x01:// 运动控制指令
if(state_yksp!=1)
{
if (check_inform.data[8] == 1)
{
speedlinedata = (int16_t)(((uint16_t)check_inform.data[5] << 8) + (uint16_t)check_inform.data[4]);
speedangledata = (int16_t)(((uint16_t)check_inform.data[7] << 8) + (uint16_t)check_inform.data[6]);
state_pcsp=1;
carsport_state=0;
speed11=const_speed*(speedlinedata-wheel_interval/2*speedangledata/1000);
speed22=const_speed*(speedlinedata-wheel_interval/2*speedangledata/1000);
speed33=-const_speed*(speedlinedata+wheel_interval/2*speedangledata/1000);
speed44=-const_speed*(speedlinedata+wheel_interval/2*speedangledata/1000);
}
else
{
speed11=0;
speed22=0;
speed33=0;
speed44=0;
}
Motor_Velocity(Motor1_Drive_ID,(uint32_t)(speed11));
Motor_Velocity(Motor2_Drive_ID,(uint32_t)(speed22));
delay_us(1000);
Motor_Velocity(Motor3_Drive_ID,(uint32_t)(speed33));
Motor_Velocity(Motor4_Drive_ID,(uint32_t)(speed44));
}
break ;
case 0x11: // 信号处理单元 融入数据查询 即融合信息上传频率设定
DataFrequency = check_inform.data[4]; //信息频率上传确定 0为单次
break;
case 0x12:// 里程计清零指令
X = 0;
Y = 0;
Yaw_Angle = 0;
break;
case 0x81: // 当前发送机状态为0 且第一次解析发送机指令
if ((check_inform.data[4] == 0x01) && (enginestartstopflag == 0x00))
{
//printf("-----------------ENGING---START------------------------");
emginestarstopstate = 0X0E;
timedurcnt=0;
}
if ((check_inform.data[4] == 0x02) && (enginestartstopflag == 0x01))
{
//printf("-----------------ENGING---STOP------------------------");
emginestarstopstate=0X10;
timedurcnt=0;
}
break;
case 0x82: // 底盘升降
switch (check_inform.data[4])
{
case 0x01:
// 当前不在01 挡位则进行解析
if (UnderPlantFour != 0x01)
{
UnderPlantUpdownConState = 0xFF;
UpdownTimeflag = 0x01;
}
break;
case 0x02:
if (UnderPlantFour != 0x02)
{
UnderPlantUpdownConState = 0xFF;
UpdownTimeflag = 0x02;
}
break;
case 0x03:
if (UnderPlantFour != 0x03)
{
UnderPlantUpdownConState = 0xFF;
UpdownTimeflag = 0x03;
}
break;
case 0x04:
if (UnderPlantFour != 0x04)
{
UnderPlantUpdownConState = 0xFF;
UpdownTimeflag = 0x04;
}
break;
}
break;
case 0x90: // 目前只有设置油量选项 根据协议其余信息自动上传
switch (check_inform.data[4])
{
// 设置油量
case 0x03:
oil = check_inform.data[5];
break;
}
break;
case 0x92:
// 电机复位请求标志
Motor_ErrorReset_Request(Motor1_Drive_ID);
Motor_ErrorReset_Request(Motor2_Drive_ID);
Motor_ErrorReset_Request(Motor3_Drive_ID);
Motor_ErrorReset_Request(Motor4_Drive_ID);
MOTOR_init();
break;
default :
break;
}
}
}
}
}
}
}
}
}