辅助理解：数据结构（C语言版）严蔚敏 清华大学出版社(环形队列)
一、C++实现
Status InitQueue(SqQueue &Q)
{
//构造一个空队列Q
Q.base=(QElemType*)malloc( MAXQSIZE*sizeof(QElemType));
if(!Q.base)
{
return false;
printf("***************Queue fail");//存储分配失败
}
Q.front=Q.rear=0;
return true;
}
int QueueLength(SqQueue &Q)
{
//返回Q的长度
return ((Q.rear-Q.front+MAXQSIZE)%MAXQSIZE);
}
Status EnQueue(SqQueue &Q,QElemType *e,int num)
{
//插入元素e为Q的新的队尾元素
if((Q.rear+num)%MAXQSIZE==Q.front) return ERROR;//队列满
if(Q.rear+num>MAXQSIZE)//到数据末尾需要注意内存拷贝的问题
{
memcpy(&Q.base[Q.rear],&e[0],MAXQSIZE-Q.rear);
memcpy(&Q.base[0],&e[MAXQSIZE-Q.rear],num+Q.rear-MAXQSIZE);
}
else
memcpy(&Q.base[Q.rear],&e[0],num);
Q.rear=(Q.rear+num)%MAXQSIZE;
return OK;
}
//出栈一个
Status DeQueue(SqQueue &Q,QElemType &e)
{
//若队列不为空，则删除Q的对头文件，用e返回其值，并返回ok
//否则返回ERROR
if(Q.front==Q.rear) return ERROR;
e=Q.base[Q.front];
Q.front=(Q.front+1)%MAXQSIZE;
return OK;
}
//出栈多个
int DeQueue(SqQueue &Q,QElemType *e,int num)
{
//若队列不为空，则删除Q的对头文件，用e返回其值，并返回ok
//否则返回ERROR
int length=(Q.rear-Q.front+MAXQSIZE)%MAXQSIZE;
if(length==0) return 0;
if(num>0)
{
if(length>num-1){
if(Q.front+num<MAXQSIZE)
memcpy(&e[0],&Q.base[Q.front],num);
else
{
memcpy(&e[0],&Q.base[Q.front],MAXQSIZE-Q.front);
memcpy(&e[MAXQSIZE-Q.front],&Q.base[0],Q.front+num-MAXQSIZE);
}
Q.front=(Q.front+num)%MAXQSIZE;
return num;
}
else{
if(Q.front+length<MAXQSIZE)
{
memcpy(&e[0],&Q.base[Q.front],length);
}
else
{
memcpy(&e[0],&Q.base[Q.front],MAXQSIZE-Q.front);
memcpy(&e[MAXQSIZE-Q.front],&Q.base[0],Q.front+length-MAXQSIZE);
}
Q.front=(Q.front+length)%MAXQSIZE;
return length;
}
}
else
return 0;
}多数据入栈
void Serial_Receive(SerialPub *Ser_)
{
QElemType rcv_buf[10240];//10k
Ser_->mlog->logInfo("Serial Program:USB data reception");
Ser_->serial_tbefore=(ros::Time::now()).toSec();
while(ros::ok()){
double serial_tnow=(ros::Time::now()).toSec();
if(serial_tnow-Ser_->serial_tbefore>4||!Ser_->serial_ttyACM1)
{
close(Ser_->device);
Ser_->Serial_open(Ser_->c_s);
Ser_->serial_tbefore=(ros::Time::now()).toSec();
}
int rcv_ser=read(Ser_->device,rcv_buf, sizeof(rcv_buf));
Ser_->time_remeber=0;
Ser_->serial_tbefore=(ros::Time::now()).toSec();
if(rcv_ser>0)
{
if(!EnQueue(Q_Save,&rcv_buf[0],rcv_ser))
{
printf("(%d,%d)\n",Q_Save.front,Q_Save.rear);
}
}
}
}多数据出栈
void SerialPub::del_ser(void)
{
uint8_t update_buff_[204800];
ssize_t rtn=0;
while(ros::ok())
{
Rec_Begin:
while(QueueLength(Q_Save)){
while(ros::ok()){
DeQueue(Q_Save,&buf_save[0],1);
if(buf_save[0]==0xA4)
{
frame_buff_[0]=0xA4;
DeQueue(Q_Save,&buf_save[0],1);
if(buf_save[00]==0x59){
frame_buff_[1]=0x59;
DeQueue(Q_Save,&buf_save[0],1);
if(buf_save[00]==0x43){
frame_buff_[2]=0x43;
break;
}
}
}
}
rtn = DeQueue(Q_Save,&buf_save[0],7);
if(rtn!=7){
mlog->logError("Serial Program:Rec 7 Error");
myprintf(&buf_save[0],7);
goto Rec_Begin;
}
memcpy(&frame_buff_[3],&buf_save[0],7);
n_size=frame_buff_[9]+(frame_buff_[8]<<8)+(frame_buff_[7]<<16)+(frame_buff_[6]<<24);
if(printf_terminal_)
std::cout<<n_size <<std::endl;
while(QueueLength(Q_Save)<n_size+1){usleep(100);}
rtn = DeQueue(Q_Save,&buf_save[0],n_size+2);
if(rtn!=n_size+2){
mlog->logError("Serial Program:Rec Lgt Error");
myprintf(&buf_save[0],n_size+2);
goto Rec_Begin;
}
memcpy(&frame_buff_[10],&buf_save[0],n_size+2);
if(frame_buff_[n_size+12-2]!=0x0D||frame_buff_[n_size+12-1]!=0x0A){
mlog->logError("Serial Program:Rec End Error");
myprintf(&frame_buff_[0],n_size+12);
goto Rec_Begin;
}
switch(frame_buff_[4])
{
case Tx2_upd_sof://升级是0x15，回复是0x14
{
switch (frame_buff_[5])
{
case 01:
{
send_data_update[4]=0x14;
if(1)
{
std::string temp_up;
for (uint8_t i=4;i<n_size-4;i++)
{
temp_up += buf_save[i];
}
std::cout << temp_up << std::endl;
temp_up=update_path_;
//temp_up=update_path_+temp_up;
const char *path_up = temp_up.c_str();
mode_t f_attrib = S_IRUSR | S_IWUSR | S_IRGRP | S_IWGRP | S_IROTH;
up_file = open(path_up, O_WRONLY | O_CREAT | O_TRUNC, f_attrib);
if(up_file < 0)
{
mlog->logError("Serial Program:Function sub 0x15 open update file");
}
upgrade_length=buf_save[3]+(buf_save[2]<<8)+(buf_save[1]<<16)+(buf_save[0]<<24);
mlog->logInfo("Agree to update APP");
cout<<"upgrade_length="<<upgrade_length<<endl;
send_data_update[5]=0x01;//允许升级
}
else
{
mlog->logInfo("Serial Program:Disagree with APP upgrade");
send_data_update[5]=0x02;//不允许升级
}
RecState.detail.Update_Px30=true;
}
break;
case 02:
{
mlog->logInfo("Serial Program:Receiving upgrade file");
int offset=0;//前几个文件为文件名，需进行偏移，假设偏移为0,最后一位为CRC校验4位
memcpy(update_buff_,buf_save+offset,n_size-offset-4);
//校验crc
uint32_t check_crc=0,check_CRC;
check_crc=crc32_check(&update_buff_[0], n_size-offset-4);
//https://zhuanlan.zhihu.com/p/256487370
check_CRC=buf_save[n_size-offset-1]+(buf_save[n_size-offset-2]<<8)+(buf_save[n_size-offset-3]<<16)+(buf_save[n_size-offset-4]<<24);
if(check_CRC==check_crc)//if(check_crc==buf_save[n_size-2-1])
{
send_data_update[4]=0x14;
send_data_update[5]=0x03;//校验成功
write(up_file,update_buff_,n_size-offset-4);//buf_save后面多了od oa 读取的是n_size是否会主动丢掉od oa?
upgrade_length=upgrade_length-(n_size-4);//-（每次接收的长度-4）
}
else
{
send_data_update[4]=0x14;
send_data_update[5]=0x04;//校验失败
mlog->logError("Serial Program:Upgrade function sub 0x15 CRC check error");
}
RecState.detail.Update_Px30=true;
if(upgrade_length<1)
{
mlog->logInfo("Serial Program:Receiving upgrade file over");
cout<<"upgrade_length="<<upgrade_length<<endl;
if(fork()==0)
{
execlp("/home/nvidia/positec/robot_update/update.sh","sh",NULL);
}
}
}
break;
case 03:
{
mlog->logInfo("Serial Program:Start Update");
close(up_file);
}
break;
case 04:
{
mlog->logInfo("Serial Program:Start Update");
close(up_file);
}
break;
default:
mlog->logError("Serial Program:Function sub 0x15 not understand");
break;
}
}
break;
case M7_tx2://M7数据->a35
{
if(printf_terminal_)
{
ROS_INFO_STREAM("serial recive");
myprintf(&frame_buff_[0],89);
}
RTK_IMU_ODOM();
}
break;
case M7_px3_ai://m7-px30-ai数据
{
if(printf_terminal_)
{
ROS_INFO_STREAM("serial recive");
myprintf(&frame_buff_[0],89);
}
RTK_IMU_ODOM();
}
break;
case Tx2_p30_Req://请求
{
switch (frame_buff_[10])
{
case Alg_Res_Req://算法复位请求
{
std::cout<<"Slam reset"<<std::endl;
slam_buf.data[0]=0x01;
RecState.detail.Slam_Reset=true;
}
break;
case Dir_Cal_Req://航向校准请求
{
std::cout<<"Dir_Calib"<<std::endl;
slam_buf.data[1]=0x01;
dir_cal=0x00;
RecState.detail.Dir_Cal_bit=true;
}
break;
case DownCar_Req://下车
{
std::cout<<"DownCar_require"<<std::endl;
slam_buf.data[2]=0x02;
ai_buf.data[1]=0x02;
RecState.detail.DownCar_px30=true;
}
break;
case UpCar_Req://上车
{
std::cout<<"UpCar_require"<<std::endl;
slam_buf.data[2]=0x01;
ai_buf.data[1]=0x01;
RecState.detail.UpCar_px30=true;
}
break;
case FinCar_Req://上下车完成
{
std::cout<<"UpDownCar_finish"<<std::endl;
slam_buf.data[2]=0x00;
ai_buf.data[1]=0x00;
RecState.detail.FinCar_px30=true;
}
break;
case FrtCar_Req://前进
{
std::cout<<"Front Car_Req"<<std::endl;
ai_buf.data[2]=0x00;
RecState.detail.Frt_Px30=true;
}
break;
case BckCar_Req://后退
{
std::cout<<"Back Car_Req"<<std::endl;
ai_buf.data[2]=0x01;
RecState.detail.Bck_Px30=true;
}
break;
default:
{
mlog->logError("Serial Program:PX30->TX2 REQUIRE:0x9a not understand");
myprintf(&frame_buff_[0],89);
break;
}
}
}
break;
case Tx2_px3_Hand://握手
{
ROS_INFO_STREAM("Hand Shake back message");
send_data_hand[4]=0x07;
send_data_hand[5]=0x02;
printf("Hand Shake recive message" );
myprintf(&frame_buff_[0],13);
RecState.detail.Hand_Px30=true;
handshake_bit=true;
}
break;
case Bld_map_init://build map init point 接收0x09，回复0x08
{
switch (frame_buff_[10])
{
case 01:
{
ROS_INFO_STREAM("Recive mapping init point 0x09 0x01");
send_data_map[4]=Bld_map_bck;
send_data_map[5]=0x01;
send_data_map[10]=0x01;
map_service++;
if(rtk_x==0)
{
send_data_map[5]=0x02;
ROS_INFO_STREAM("No rtk message from m7");
}
else
{
send_data_map[5]=0x01;
ROS_INFO_STREAM("Mapping init point");
}
*(double *)&send_data_map[11] = rtk_x;
*(double *)&send_data_map[19] = rtk_y;
*(float * )&send_data_map[27] = rtk_z;
myprintf(&frame_buff_[0],13);
std::ofstream Out_dirStream;
Out_dirStream.open(rtk_path_, std::ios::out);
Out_dirStream.setf(std::ios::fixed,std::ios::floatfield);
Out_dirStream.precision(9);
Out_dirStream << rtk_x << " "
<< rtk_y << " "
<< rtk_z << std::endl;
Out_dirStream.close();
RecState.detail.Map_Px30=true;
}
break;
case 02:
{
ROS_INFO_STREAM("Recive mapping init point 0x09 0x02");
send_data_map[4]=0x08;
send_data_map[5]=0x01;
send_data_map[10]=0x02;
map_service++;
ROS_INFO_STREAM("Mapping init point change");
double change_point_x=0.00,change_point_y=0.00;
if(n_size>16)
{
memcpy(&change_point_x,&frame_buff_[11], 8);
memcpy(&change_point_y,&frame_buff_[19], 8);
std::ofstream Out_dirStream;
Out_dirStream.open(rtk_path_, std::ios::out);
Out_dirStream.setf(std::ios::fixed,std::ios::floatfield);
Out_dirStream.precision(9);
Out_dirStream << change_point_x << " "
<< change_point_y << " "
<< 1.00 << std::endl;
std::cout<<"change_point_x= "<<change_point_x<<"change_point_y= "<<change_point_y<<std::endl;
Out_dirStream.close();
RecState.detail.Map_Px30=true;
}
else
{
std::cout<<"change_point length too short,the length is"<<n_size<<std::endl;
}
}
break;
default:
{
mlog->logError("Serial Program:Mapping init point,0x09 not understand");
myprintf(&frame_buff_[0],89);
break;
}
}
}
break;
case Tx2_p30_log://传LOG
{
RecState.detail.Log_Px30=true;
}
break;
default:
{
mlog->logError("Serial Program:Function main not understand");
myprintf(&frame_buff_[0],89);
break;
}
}
}
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
}二、STM32实现
头文件数据定义
typedef enum
{
USART_QUEUE_EMPTY = 0,
USART_QUEUE_FULL = 1,
USART_QUEUE_OK = 2,
} usart_queue_status_t;
#define USART_QUEUE_SIZE 256
typedef struct
{
u16 front;
u16 rear;
u16 size;
u8 data[USART_QUEUE_SIZE];
} usart_queue_t;
extern usart_queue_t usart1_send;
void UsartQueueInit(usart_queue_t *q);
uint8_t UsartQueuePop(usart_queue_t *q, uint8_t *data);
uint8_t UsartQueuePush(usart_queue_t *q, uint8_t data);.c文件
#include "sys.h"
#include "usart.h"
usart_queue_t usart1_send;
#if 1
#pragma import(__use_no_semihosting)
//解决HAL库使用时,某些情况可能报错的bug
int _ttywrch(int ch)
{
ch=ch;
return ch;
}
//标准库需要的支持函数
struct __FILE
{
int handle;
/* Whatever you require here. If the only file you are using is */
/* standard output using printf() for debugging, no file handling */
/* is required. */
};
/* FILE is typedef’ d in stdio.h. */
FILE __stdout;
//定义_sys_exit()以避免使用半主机模式
void _sys_exit(int x)
{
x = x;
}
//重定义fputc函数
int fputc(int ch, FILE *f)
{
while((USART1->ISR&0X40)==0);//循环发送,直到发送完毕
USART1->TDR = (u8) ch;
return ch;
}
#endif
//end
//串口缓冲队列初始化
void UsartQueueInit(usart_queue_t *q)
{
q->size = 0;
q->front = 0;
q->rear = 0;
}
//入队
uint8_t UsartQueuePush(usart_queue_t *q, uint8_t data)
{
if (((q->rear % USART_QUEUE_SIZE) == q->front) && ((q->size == USART_QUEUE_SIZE)))
{
//printf("--------------USART_QUEUE_FULL------------------");
return USART_QUEUE_FULL;
}
q->data[q->rear] = data;
q->rear = (q->rear + 1) % USART_QUEUE_SIZE;
q->size++;
return USART_QUEUE_OK;
}
//出队
uint8_t UsartQueuePop(usart_queue_t *q, uint8_t *data)
{
if ((q->front == q->rear) && (q->size == 0))
{
//printf("--------------USART_QUEUE_EMPTY-------------------");
return USART_QUEUE_EMPTY;
}
*data = q->data[q->front];
q->front = (q->front + 1) % USART_QUEUE_SIZE;
q->size--;
return USART_QUEUE_OK;
}
void USART1_IRQHandler(void)
{
u8 res;
if(USART1->ISR&(1<<5))//接收到数据
{
res=USART1->RDR;
UsartQueuePush(&usart1_send, res);
}
USART1->ICR |=0x0008;
}
//初始化IO 串口1 上位机
//pclk2:PCLK2时钟频率(Mhz)
//bound:波特率
void USART1_Init(u32 pclk2,u32 bound)
{
u32 temp;
temp=(pclk2*1000000+bound/2)/bound; //得到USARTDIV@OVER8=0,采用四舍五入计算
RCC->AHB1ENR|=1<<1; //使能PORTB口时钟
RCC->APB2ENR|=1<<4; //使能串口1时钟
GPIO_Set(GPIOB,PIN14|PIN15,GPIO_MODE_AF,GPIO_OTYPE_PP,GPIO_SPEED_100M,GPIO_PUPD_PU);//PB14,PB15,复用功能,上拉输出
GPIO_AF_Set(GPIOB,14,4); //PB14,AF4
GPIO_AF_Set(GPIOB,15,4);//PB15,AF4
//波特率设置
USART1->BRR=temp; //波特率设置@OVER8=0
USART1->CR1=0; //清零CR1寄存器
USART1->CR1|=0<<28; //设置M1=0
USART1->CR1|=0<<12; //设置M0=0&M1=0,选择8位字长
USART1->CR1|=0<<15; //设置OVER8=0,16倍过采样
USART1->CR1|=1<<3; //串口发送使能
//使能接收中断
USART1->CR1|=1<<2; //串口接收使能
USART1->CR1|=1<<5; //接收缓冲区非空中断使能
MY_NVIC_Init(0,2,USART1_IRQn,2);//组2，最低优先级
USART1->CR1|=1<<0; //串口使能
}
使用时，解析一个丢一个数据，通讯协议定义最短接收指令个数为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;
}
}
}
}
}
}
}
}
}
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
