ros与stm32串口通信方案升级
基于STM32搭建ROS机器人完整的串口通信方案,函数封装合理,易用、易读、易扩展。
如果在你解决ROS与STM32通信问题的时候,这篇文章有幸被你看到,一定要仔细看一下,这篇文章一定可以解决你的问题。
之前写过一遍搭建基于STM32的ROS机器人需要的串口通信方案,利用了共用体的特性较好的完成了通信功能。之前写的博文:https://blog.csdn.net/zhao_ke_xue/article/details/105493907。最近仔细又看看了源码,有种怀疑不是自己写的代码的感觉。于是,这里对代码进行升级,去掉冗余的部分,增加扩展项。使得代码更加容易使用和扩展修改。
关于该方案的原理,请看之前的的文章比较详细,方案原理与说明。
/----------------------------------------------------------------------------------------------------------------------------------
看下面文章之前,尽量看一下之前的原理性文章,虽然函数名字和函数内部有出入,但是原理一致。
----------------------------------------------------------------------------------------------------------------------------------/
下面直接上代码:
STM32下位机代码(局部):
说明:
这里的usartReceiveOneData函数就是之前receiveTo103函数
这里的usartSendData函数就是之前usartSendSpeed函数
/**************************************************************************
函数功能:通过串口中断服务函数,获取上位机发送的左右轮控制速度、预留控制标志位,分别存入参数中
入口参数:左轮轮速控制地址、右轮轮速控制地址、预留控制标志位
返回 值:无特殊意义
**************************************************************************/
int usartReceiveOneData(int *p_leftSpeedSet,int *p_rightSpeedSet,unsigned char *p_crtlFlag)
{
unsigned char USART_Receiver = 0; //接收数据
static unsigned char checkSum = 0;
static unsigned char USARTBufferIndex = 0;
static short j=0,k=0;
static unsigned char USARTReceiverFront = 0;
static unsigned char Start_Flag = START; //一帧数据传送开始标志位
static short dataLength = 0;
USART_Receiver = USART_ReceiveData(USART1); //@@@@@#####如果你使用不是USART1更改成相应的,比如USART3
//接收消息头
if(Start_Flag == START)
{
if(USART_Receiver == 0xaa) //buf[1]
{
if(USARTReceiverFront == 0x55) //数据头两位 //buf[0]
{
Start_Flag = !START; //收到数据头,开始接收数据
//printf("header ok\n");
receiveBuff[0]=header[0]; //buf[0]
receiveBuff[1]=header[1]; //buf[1]
USARTBufferIndex = 0; //缓冲区初始化
checkSum = 0x00; //校验和初始化
}
}
else
{
USARTReceiverFront = USART_Receiver;
}
}
else
{
switch(USARTBufferIndex)
{
case 0://接收左右轮速度数据的长度
receiveBuff[2] = USART_Receiver;
dataLength = receiveBuff[2]; //buf[2]
USARTBufferIndex++;
break;
case 1://接收所有数据,并赋值处理
receiveBuff[j + 3] = USART_Receiver; //buf[3] - buf[7]
j++;
if(j >= dataLength)
{
j = 0;
USARTBufferIndex++;
}
break;
case 2://接收校验值信息
receiveBuff[3 + dataLength] = USART_Receiver;
checkSum = getCrc8(receiveBuff, 3 + dataLength);
// 检查信息校验值
if (checkSum != receiveBuff[3 + dataLength]) //buf[8]
{
printf("Received data check sum error!");
return 0;
}
USARTBufferIndex++;
break;
case 3://接收信息尾
if(k==0)
{
//数据0d buf[9] 无需判断
k++;
}
else if (k==1)
{
//数据0a buf[10] 无需判断
//进行速度赋值操作
for(k = 0; k < 2; k++)
{
leftVelSet.data[k] = receiveBuff[k + 3]; //buf[3] buf[4]
rightVelSet.data[k] = receiveBuff[k + 5]; //buf[5] buf[6]
}
//速度赋值操作
*p_leftSpeedSet = (int)leftVelSet.d;
*p_rightSpeedSet = (int)rightVelSet.d;
//ctrlFlag
*p_crtlFlag = receiveBuff[7]; //buf[7]
//-----------------------------------------------------------------
//完成一个数据包的接收,相关变量清零,等待下一字节数据
USARTBufferIndex = 0;
USARTReceiverFront = 0;
Start_Flag = START;
checkSum = 0;
dataLength = 0;
j = 0;
k = 0;
//-----------------------------------------------------------------
}
break;
default:break;
}
}
return 0;
}
/**************************************************************************
函数功能:将左右轮速和角度数据、控制信号进行打包,通过串口发送给Linux
入口参数:实时左轮轮速、实时右轮轮速、实时角度、控制信号(如果没有角度也可以不发)
返回 值:无
**************************************************************************/
void usartSendData(short leftVel, short rightVel,short angle,unsigned char ctrlFlag)
{
// 协议数据缓存数组
unsigned char buf[13] = {0};
int i, length = 0;
// 计算左右轮期望速度
leftVelNow.d = leftVel;
rightVelNow.d = rightVel;
angleNow.d = angle;
// 设置消息头
for(i = 0; i < 2; i++)
buf[i] = header[i]; // buf[0] buf[1]
// 设置机器人左右轮速度、角度
length = 7;
buf[2] = length; // buf[2]
for(i = 0; i < 2; i++)
{
buf[i + 3] = leftVelNow.data[i]; // buf[3] buf[4]
buf[i + 5] = rightVelNow.data[i]; // buf[5] buf[6]
buf[i + 7] = angleNow.data[i]; // buf[7] buf[8]
}
// 预留控制指令
buf[3 + length - 1] = ctrlFlag; // buf[9]
// 设置校验值、消息尾
buf[3 + length] = getCrc8(buf, 3 + length); // buf[10]
buf[3 + length + 1] = ender[0]; // buf[11]
buf[3 + length + 2] = ender[1]; // buf[12]
//发送字符串数据
USART_Send_String(buf,sizeof(buf));
}
/**************************************************************************
函数功能:发送指定大小的字符数组,被usartSendData函数调用
入口参数:数组地址、数组大小
返回 值:无
**************************************************************************/
void USART_Send_String(u8 *p,u16 sendSize)
{
static int length =0;
while(length<sendSize)
{
//@@@@@#####如果你使用不是USART1更改成相应的,比如USART3,这里有两处修改
while( !(USART1->SR&(0x01<<7)) );//发送缓冲区为空
USART1->DR=*p;
p++;
length++;
}
length =0;
}
/**************************************************************************
函数功能:计算八位循环冗余校验,被usartSendData和usartReceiveOneData函数调用
入口参数:数组地址、数组大小
返回 值:无
**************************************************************************/
unsigned char getCrc8(unsigned char *ptr, unsigned short len)
{
unsigned char crc;
unsigned char i;
crc = 0;
while(len--)
{
crc ^= *ptr++;
for(i = 0; i < 8; i++)
{
if(crc&0x01)
crc=(crc>>1)^0x8C;
else
crc >>= 1;
}
}
return crc;
}
/**********************************END***************************************/
//中断服务函数
//void USART1_IRQHandler()
//{
// if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET)
// {
// USART_ClearITPendingBit(USART1,USART_IT_RXNE);//首先清除中断标志位
// usartReceiveOneData(&leftSpeedSet,&rightSpeedSet,&ctrlFlag);
// }
//}
linux上位机代码(局部):
/********************************************************
函数功能:将机器人的线速度和角速度分解成左右轮子速度,打包发送给下位机
入口参数:机器人线速度、角速度
出口参数:
********************************************************/
void writeSpeed(double RobotV, double YawRate,unsigned char ctrlFlag)
{
unsigned char buf[11] = {0};//
int i, length = 0;
double r = RobotV / YawRate;//mm
// 计算左右轮期望速度
if(RobotV == 0) //旋转
{
leftVelSet.d = (short)(-YawRate * ROBOT_RADIUS);//mm/s
rightVelSet.d = (short)(YawRate * ROBOT_RADIUS);//mm/s
}
else if(YawRate == 0)//直线
{
leftVelSet.d = (short)RobotV;//mm/s
rightVelSet.d = (short)RobotV;
}
else //速度不一致
{
leftVelSet.d = (short)(YawRate * (r - ROBOT_RADIUS));//mm/s
rightVelSet.d = (short)(YawRate * (r + ROBOT_RADIUS));
}
// 设置消息头
for(i = 0; i < 2; i++)
buf[i] = header[i]; //buf[0] buf[1]
// 设置机器人左右轮速度
length = 5;
buf[2] = length; //buf[2]
for(i = 0; i < 2; i++)
{
buf[i + 3] = leftVelSet.data[i]; //buf[3] buf[4]
buf[i + 5] = rightVelSet.data[i]; //buf[5] buf[6]
}
// 预留控制指令
buf[3 + length - 1] = ctrlFlag; //buf[7]
// 设置校验值、消息尾
buf[3 + length] = getCrc8(buf, 3 + length);//buf[8]
buf[3 + length + 1] = ender[0]; //buf[9]
buf[3 + length + 2] = ender[1]; //buf[10]
// 通过串口下发数据
boost::asio::write(sp, boost::asio::buffer(buf));
}
/********************************************************
函数功能:从下位机读取数据,解析出线速度、角速度、角度
入口参数:机器人线速度、角速度、角度,引用参数
出口参数:bool
********************************************************/
bool readSpeed(double &vx,double &vth,double &th,unsigned char &ctrlFlag)
{
char i, length = 0;
unsigned char checkSum;
unsigned char buf[150]={0};
//=========================================================
//此段代码可以读数据的结尾,进而来进行读取数据的头部
try
{
boost::asio::streambuf response;
boost::asio::read_until(sp, response, "\r\n",err);
copy(istream_iterator<unsigned char>(istream(&response)>>noskipws),
istream_iterator<unsigned char>(),
buf);
}
catch(boost::system::system_error &err)
{
ROS_INFO("read_until error");
}
//=========================================================
// 检查信息头
if (buf[0]!= header[0] || buf[1] != header[1]) //buf[0] buf[1]
{
ROS_ERROR("Received message header error!");
return false;
}
// 数据长度
length = buf[2]; //buf[2]
// 检查信息校验值
checkSum = getCrc8(buf, 3 + length); //buf[10] 计算得出
if (checkSum != buf[3 + length]) //buf[10] 串口接收
{
ROS_ERROR("Received data check sum error!");
return false;
}
// 读取速度值
for(i = 0; i < 2; i++)
{
leftVelNow.data[i] = buf[i + 3]; //buf[3] buf[4]
rightVelNow.data[i] = buf[i + 5]; //buf[5] buf[6]
angleNow.data[i] = buf[i + 7]; //buf[7] buf[8]
}
// 读取控制标志位
ctrlFlag = buf[9];
// 打印数据信息
/*
ROS_INFO("Left_v:%d\n",leftVelNow.d);
ROS_INFO("Right_v:%d\n",rightVelNow.d);
ROS_INFO("Angle:%d\n",angleNow.d);
ROS_INFO("crtlFlag:%d\n",ctrlFlag);
*/
//===========================速度计算和Angle获取===========================================================
// x方向速度,以及角速度
vx = (rightVelNow.d + leftVelNow.d) / 2.0 / 1000.0; //m/s
vth = (rightVelNow.d - leftVelNow.d) / ROBOT_LENGTH ; //rad/s
th = angleNow.d*0.01745;//实时角度信息(rad)
return true;
}
/********************************************************
函数功能:获得8位循环冗余校验值
入口参数:数组地址、长度
出口参数:校验值
********************************************************/
unsigned char getCrc8(unsigned char *ptr, unsigned short len)
{
unsigned char crc;
unsigned char i;
crc = 0;
while(len--)
{
crc ^= *ptr++;
for(i = 0; i < 8; i++)
{
if(crc&0x01)
crc=(crc>>1)^0x8C;
else
crc >>= 1;
}
}
return crc;
}
关于升级版的串口通信源文件获取,在公众号:沉醉不知路,发送:串口通信升级,即可获得下载链接。
如果源码文件有什么使用上的问题或者修改的意见,可以在公众号给我发送信息,我一定会积极回复大家。如果你是刚刚入行的小白,关注我,或多或少可以帮你解决一些问题的。