串口接收数据并解析
协议如下:
//payload(负载段)数据类型
typedef union{
struct{
uint32_t dataField:24;
uint32_t dataType:6;
uint32_t res:2;
}bit;
uint32_t data;
}measure_t;
//payload description数据类型
typedef struct
{
uint32_t timestamp;
union{
struct
{
uint16_t timemarksend:2;
uint16_t timemarkEdge:1;
uint16_t calibTagVaild:1;
uint16_t res:7;
uint16_t numMeas: 5;
}bit;
uint16_t data;
}flags;
uint16_t id;
}payload_desp_t;
//ublox message:UBX-ESF-MEAS数据类型
typedef struct
{
uint8_t Header[2];
uint8_t Class;
uint8_t Id;
uint8_t Len;
uint8_t Payload[128]; //max_meas = 31*4 +【】
uint8_t Crc[2];
}ublox_esf_meas_t;
//#pragma 预处理指令,作用是设定编译器的状态或者指示编译器完成一些特定动作
//#pragma pack():改变编译器内存对齐方式
#pragma pack()
#define HEADER1_NUM 0xB5
#define HEADER2_NUM 0x62
//枚举
typedef enum{
HEADER1 = 0,
HEADER2,
CLASS,
ID,
LENGTH,
PAYLOAD,
CRC1,
CRC2,
}parse_phase_t;
parse_phase_t parse_phase = HEADER1;
ublox_esf_meas_t ublox_esf_meas;
uint8_t payload_index = 0;
uint16_t crcsum = 0;
//receive函数 一次读一个字节数据,先找到header,然后向后读数据,
bool receive(){
while(1){
uint8_t ret = Serial.read(1);
if(ret == -1){
break;
}
switch (parse_phase)
{
case HEADER1:
if(ret == HEADER1_NUM){
parse_phase = HEADER2;
}
break;
case HEADER2:
if(ret == HEADER2_NUM){
parse_phase = CLASS;
}
else{
parse_phase = HEADER1;
}
break;
case CLASS:
ublox_esf_meas.Class = ret;
parse_phase = ID;
break;
case ID:
ublox_esf_meas.Id = ret;
parse_phase = LENGTH;
break;
case LENGTH:
ublox_esf_meas.Len = ret;
parse_phase = PAYLOAD;
case PAYLOAD:
ublox_esf_meas.Payload[payload_index++] = ret;
crcsum = crc16(ret);
if(payload_index == ublox_esf_meas.Len){
parse_phase = CRC1;
}
case CRC1:
if(ret == (crcsum>>8|0x00FF)){
switch (ublox_esf_meas.Class)
{
case 10:
handle_class10_msg(ublox_esf_meas.Id);
//break;
}
default:
//break;
}
//else{
parse_phase = HEADER1;
//}
default:
break;
}
}
typedef enum{
MEAS_ID =2,
}id_t;
id_t ids;
void handle_class10_msg(id_t id)
{
switch (id)
{
case MEAS_ID:
handle_meas_msg(ublox_esf_meas.payload,len);
break;
default:
break;
}
}
void handle_meas_msg(uint8_t *buf, uint8_t len){
payload_desp_t desp;
memcpy(&desp, buf, 6);
uint8_t numMeas = desp.flags.bit.numMeas;
measure_t *measure = new measure_t[numMeas];//开辟空间 存放负载数据
if(6+ 4*numMeas == len){
memcpy(measure, buf+6, numMeas*4);
for(int i=0; i< numMeas; i++){
uint8_t dataType = measure[i]->bit->dataType;
uint32_t value = measure[i]->bit->data;
if(dataType == 11){
msg.speed = value*1e-3;
}
}
//数据是对的
}else{
// 理论上进不来
ROS_INFO("解析错误数据");
return;
}
delete measure_t;
}