211 lines
7.6 KiB
C
211 lines
7.6 KiB
C
#include "stm32f4xx.h" // Device header
|
|
#include "stm32f4xx_conf.h"
|
|
#include "RefereeSystem_CRCTable.h"
|
|
#include "UART.h"
|
|
|
|
/****************************************************************************************************
|
|
|
|
此处裁判系统只接收0x0201命令,获取机器人性能体系数据,主要获取发射机构是否上电
|
|
帧格式:
|
|
0xA5 0x0D 0x00 包序号 帧头CRC8校验 0x01 0x02 13ByteData 整包CRC16校验
|
|
|___________________________________| |_______| |
|
|
帧头 命令码 数据
|
|
|
|
****************************************************************************************************/
|
|
|
|
/*接收数据缓冲区数组元素数=命令码对应数据段长度+9*/
|
|
uint8_t RefereeSystem_RxHEXPacket1[22]={0xA5,0x0D,0x00,0x00,0x00,0x01,0x02,0};//裁判系统0x0201命令码接收数据缓冲区
|
|
uint8_t RefereeSystem_RxHEXPacket2[25]={0xA5,0x10,0x00,0x00,0x00,0x02,0x02,0};//裁判系统0x0202命令码接收数据缓冲区
|
|
|
|
uint8_t RefereeSystem_ShooterStatus=1;//发射机构状态,0-发射机构未上电,1-发射机构上电
|
|
uint16_t RefereeSystem_Ref=120,RefereeSystem_Buffer=30.0f;//底盘功率上限,底盘功率缓冲能量(若无裁判系统,默认功率上限80W,缓冲能量30J)
|
|
float RefereeSystem_Power;//底盘实时功率
|
|
uint8_t RefereeSystem_RobotID=0;//机器人ID(无裁判系统时一直为0,4号步兵ID为4/104)
|
|
|
|
/*
|
|
*函数简介:裁判系统CRC8查表计算
|
|
*参数说明:校验数据
|
|
*参数说明:数据长度
|
|
*参数说明:CRC初始值(默认给参数0xFF)
|
|
*返回类型:无
|
|
*备注:表格位于Referee System_CRCTable.h文件中CRC8_Table数组
|
|
*/
|
|
uint8_t RefereeSystem_GetCRC8CheckSum(uint8_t *Data,uint16_t Length,uint8_t Initial)
|
|
{
|
|
uint8_t Minuend;
|
|
while(Length--)
|
|
{
|
|
Minuend=Initial^(*Data);
|
|
Data++;
|
|
Initial=CRC8_Table[Minuend];
|
|
}
|
|
return Initial;
|
|
}
|
|
|
|
/*
|
|
*函数简介:裁判系统CRC8校验
|
|
*参数说明:校验数据(含尾端CRC校验码)
|
|
*参数说明:数据长度
|
|
*返回类型:校验正确返回1,否则返回0
|
|
*备注:无
|
|
*/
|
|
uint8_t RefereeSystem_VerifyCRC8CheckSum(uint8_t *Data,uint16_t Length)
|
|
{
|
|
uint8_t CRC8CheckSum=0;
|
|
if((Data==0) || (Length<=2))return 0;//特殊情况处理
|
|
CRC8CheckSum=RefereeSystem_GetCRC8CheckSum(Data,Length-1,CRC8_Initial);//获取CRC8计算值
|
|
return CRC8CheckSum==Data[Length-1];//测量值与计算值相比较
|
|
}
|
|
|
|
/*
|
|
*函数简介:裁判系统CRC16查表计算
|
|
*参数说明:校验数据
|
|
*参数说明:数据长度
|
|
*参数说明:CRC初始值(默认给参数0xFFFF)
|
|
*返回类型:无
|
|
*备注:表格位于Referee System_CRCTable.h文件中CRC16_Table数组
|
|
*/
|
|
uint16_t RefereeSystem_GetCRC16CheckSum(uint8_t *Data,uint32_t Length,uint16_t Initial)
|
|
{
|
|
uint8_t Minuend;
|
|
while(Length--)
|
|
{
|
|
Minuend=*Data;
|
|
Data++;
|
|
Initial=((uint16_t)(Initial)>>8)^CRC16_Table[((uint16_t)(Initial)^(uint16_t)(Minuend))&0x00FF];
|
|
}
|
|
return Initial;
|
|
}
|
|
|
|
/*
|
|
*函数简介:裁判系统CRC16校验
|
|
*参数说明:校验数据(含尾端CRC校验码)
|
|
*参数说明:数据长度
|
|
*返回类型:校验正确返回1,否则返回0
|
|
*备注:无
|
|
*/
|
|
uint32_t RefereeSystem_VerifyCRC16CheckSum(uint8_t *Data, uint32_t Length)
|
|
{
|
|
uint16_t CRC16CheckSum=0;
|
|
if((Data==0)||(Length<=2))return 0;//特殊情况处理
|
|
CRC16CheckSum=RefereeSystem_GetCRC16CheckSum(Data,Length-2,CRC16_Initial);//获取CRC16计算值
|
|
return ((CRC16CheckSum&0xFF)==Data[Length-2]&&((CRC16CheckSum>>8)&0xff)==Data[Length-1]);//测量值与计算值相比较
|
|
}
|
|
|
|
/*
|
|
*函数简介:裁判系统接收初始化
|
|
*参数说明:无
|
|
*返回类型:无
|
|
*备注:默认使用UART1(USART6),默认Rx引脚PG9
|
|
*/
|
|
void RefereeSystem_Init(void)
|
|
{
|
|
UART1_Init();
|
|
//while(RefereeSystem_RobotID==0);
|
|
}
|
|
|
|
/*
|
|
*函数简介:UART1串口中断接收裁判系统数据
|
|
*参数说明:无
|
|
*返回类型:无
|
|
*备注:数据帧格式在最上方注释
|
|
*/
|
|
void USART6_IRQHandler(void)
|
|
{
|
|
/*有效数据位数=命令码对应数据段长度+2*/
|
|
#define DataLength1 15//裁判系统0x0201命令码有效数据位数
|
|
#define DataLength2 18//裁判系统0x0202命令码有效数据位数
|
|
|
|
static int RxHEXState=0;//定义静态变量用于接收模式的选择
|
|
static int pRxHEXState=0;//定义静态变量用于充当计数器
|
|
|
|
uint8_t RefereeSystem_RxData;//裁判系统接收数据
|
|
int RefereeSystem_PowerRow;//底盘实时功率原始数据
|
|
|
|
if(USART_GetITStatus(USART6,USART_IT_RXNE)==SET)//查询接收中断标志位
|
|
{
|
|
USART_ClearITPendingBit(USART6,USART_IT_RXNE);//清除接收中断标志位
|
|
|
|
RefereeSystem_RxData=USART_ReceiveData(USART6);//将数据存入缓存区
|
|
|
|
/*=====检查帧头=====*/
|
|
if(RxHEXState==0){if(RefereeSystem_RxData==0xA5)RxHEXState=1;}
|
|
|
|
/*=====检查数据段长度,区分命令码=====*/
|
|
else if(RxHEXState==1)
|
|
{
|
|
if(RefereeSystem_RxData==0x0D)RxHEXState=2;//0x0201命令码
|
|
else if(RefereeSystem_RxData==0x10)RxHEXState=8;//0x0202命令码
|
|
else RxHEXState=0;//其他命令码直接跳过
|
|
}
|
|
|
|
/*=====0x0201命令码=====*/
|
|
/*=====检查帧头其他部分=====*/
|
|
else if(RxHEXState==2){if(RefereeSystem_RxData==0x00)RxHEXState=3;else RxHEXState=0;}
|
|
else if(RxHEXState==3){RefereeSystem_RxHEXPacket1[3]=RefereeSystem_RxData;RxHEXState=4;}
|
|
else if(RxHEXState==4)
|
|
{
|
|
RefereeSystem_RxHEXPacket1[4]=RefereeSystem_RxData;
|
|
|
|
if(RefereeSystem_VerifyCRC8CheckSum(RefereeSystem_RxHEXPacket1,5)==1)RxHEXState=5;//检查CRC8
|
|
else RxHEXState=0;
|
|
}
|
|
|
|
/*=====检查命令码=====*/
|
|
else if(RxHEXState==5){if(RefereeSystem_RxData==0x01)RxHEXState=6;else RxHEXState=0;}
|
|
else if(RxHEXState==6){if(RefereeSystem_RxData==0x02){RxHEXState=7;pRxHEXState=0;}else RxHEXState=0;}
|
|
|
|
/*=====接收有效数据=====*/
|
|
else if(RxHEXState==7)
|
|
{
|
|
RefereeSystem_RxHEXPacket1[pRxHEXState+7]=RefereeSystem_RxData;//接收数据
|
|
pRxHEXState++;
|
|
|
|
if(pRxHEXState>=DataLength1)
|
|
{
|
|
if(RefereeSystem_VerifyCRC16CheckSum(RefereeSystem_RxHEXPacket1,22)==1)//CRC校验
|
|
{
|
|
RefereeSystem_RobotID=RefereeSystem_RxHEXPacket1[7];//获取机器人ID
|
|
RefereeSystem_ShooterStatus=(RefereeSystem_RxHEXPacket1[19]&0x04)>>2;//获取发射机构状态
|
|
RefereeSystem_Ref=RefereeSystem_RxHEXPacket1[17]|((uint16_t)RefereeSystem_RxHEXPacket1[18]<<8);//获取功率上限
|
|
}
|
|
RxHEXState=0;
|
|
}
|
|
}
|
|
|
|
|
|
/*=====0x0202命令码=====*/
|
|
/*=====检查帧头其他部分=====*/
|
|
else if(RxHEXState==8){if(RefereeSystem_RxData==0x00)RxHEXState=9;else RxHEXState=0;}
|
|
else if(RxHEXState==9){RefereeSystem_RxHEXPacket2[3]=RefereeSystem_RxData;RxHEXState=10;}
|
|
else if(RxHEXState==10)
|
|
{
|
|
RefereeSystem_RxHEXPacket2[4]=RefereeSystem_RxData;
|
|
if(RefereeSystem_VerifyCRC8CheckSum(RefereeSystem_RxHEXPacket2,5)==1)RxHEXState=11;
|
|
else RxHEXState=0;
|
|
}
|
|
|
|
/*=====检查命令码=====*/
|
|
else if(RxHEXState==11){if(RefereeSystem_RxData==0x02)RxHEXState=12;else RxHEXState=0;}
|
|
else if(RxHEXState==12){if(RefereeSystem_RxData==0x02){RxHEXState=13;pRxHEXState=0;}else RxHEXState=0;}
|
|
|
|
/*=====接收有效数据=====*/
|
|
else if(RxHEXState==13)
|
|
{
|
|
RefereeSystem_RxHEXPacket2[pRxHEXState+7]=RefereeSystem_RxData;//接收数据
|
|
pRxHEXState++;
|
|
|
|
if(pRxHEXState>=DataLength2)
|
|
{
|
|
if(RefereeSystem_VerifyCRC16CheckSum(RefereeSystem_RxHEXPacket2,25)==1)//CRC校验
|
|
{
|
|
RefereeSystem_Buffer=RefereeSystem_RxHEXPacket2[15]|((uint16_t)RefereeSystem_RxHEXPacket2[16]<<8);//获取缓冲能量
|
|
RefereeSystem_PowerRow=(int32_t)((uint32_t)RefereeSystem_RxHEXPacket2[11]|((uint32_t)RefereeSystem_RxHEXPacket2[12]<<8)|((uint32_t)RefereeSystem_RxHEXPacket2[13]<<16)|((uint32_t)RefereeSystem_RxHEXPacket2[14]<<24));
|
|
RefereeSystem_Power=*((float *)(&RefereeSystem_PowerRow));//获取底盘实时功率
|
|
}
|
|
RxHEXState=0;
|
|
}
|
|
}
|
|
}
|
|
}
|