如何实现智能小车的通信协议
通信协议设计
在Part2《如何开发智能小车的驱动器软件》中我们已经介绍了什么是通讯方式的协议层，它是指通信双方对数据传送控制的一种约定，协议中包括数据格式、同步方式、传送速度、传送步骤、检纠错方式以及控制字符定义等，通信双方必须共同遵守。更具体一些常见的协议如下图所示
接下来以OriginCar机器人为例，介绍其通信协议。
首先关于几个细节需要和大家拉平，上位机一般而言指的控制器，以OriginCar为例是RDK X3开发板；下位机一般指的是驱动板，此处指OriginCar主板。
第二点，上下位机的指令发什么由开发者决定，往往取决于希望获取什么样的数据，例如对于下位机而言需要的是一个速度指令，那么上位机只需要发布X、Y、Z轴的速度指令即可，下位机负责对数据帧进行解析。
第三点，数据帧包含了帧头、帧尾以及校验位，关于这三位方式很多，但是上下位机一定要做到同步，这样确保数据解析正确。
上位机下发指令
下位机上发指令
通信协议的下位机实现
以和上位机通信的串口3为例，关于USART3初始化等在上一个Part已经和大家介绍。
波特率设置
// system.c
void systemInit(void)
{
...
uart3_init(115200);
...
}
//usartx.c
void uart3_init(u32 bound)
{
...
USART_InitStructure.USART_BaudRate = bound;
...
}
数据接收
关于数据的接收，其实是通过串口中断进行获取，并根据上位机下发指令进行数据解析，即可得到上位机下发的X、Y、Z速度，并将该速度给到运动控制进行实现。
int USART3_IRQHandler(void)
{
static u8 Count=0;
u8 Usart_Receive;
if(USART_GetITStatus(USART3, USART_IT_RXNE) != RESET)
{
Usart_Receive = USART_ReceiveData(USART3); //读取数据
if(Time_count<CONTROL_DELAY)
return 0;
Receive_Data.buffer[Count]=Usart_Receive;
if(Usart_Receive == FRAME_HEADER||Count>0)
Count++;
else
Count=0;
if (Count == 11) //验证数据包的长度
{
Count=0; //为串口数据重新填入数组做准备
if(Receive_Data.buffer[10] == FRAME_TAIL) //验证数据包的帧尾
{
//数据异或位校验计算，模式0是发送数据校验
if(Receive_Data.buffer[9] ==Check_Sum(9,0)) {
float Vz;
PS2_ON_Flag=0;
Remote_ON_Flag=0;
APP_ON_Flag=0;
CAN_ON_Flag=0;
Usart1_ON_Flag=0;
Usart5_ON_Flag=0;
command_lost_count=0;
Move_X=XYZ_Target_Speed_transition(Receive_Data.buffer[3],Receive_Data.buffer[4]);
Move_Y=XYZ_Target_Speed_transition(Receive_Data.buffer[5],Receive_Data.buffer[6]);
Vz =XYZ_Target_Speed_transition(Receive_Data.buffer[7],Receive_Data.buffer[8]);
Move_Z=Vz_to_Akm_Angle(Move_X, Vz);
}
}
}
}
return 0;
}
float XYZ_Target_Speed_transition(u8 High,u8 Low)
{
//数据转换的中间变量
short transition;
//将高8位和低8位整合成一个16位的short型数据
transition=((High<<8)+Low);
return transition/1000+(transition%1000)*0.001; //单位转换, mm/s->m/s
}
数据发送
发送数据的方式：将要发送的数据打包到一个数组中，数组的长度是 24 字节，使用串口逐位发送出去。因为串口一次只能发送一个 8 位（1 个字节）的数据，因此 2 个字节（short 型）的数据会拆分成高 8 位和低 8 位发送。
以下给出数据封装代码，其作用就是将下位机上发指令按照代码的形式进行封装。
void data_transition(void)
{
Send_Data.Sensor_Str.Frame_Header = FRAME_HEADER; //帧头
Send_Data.Sensor_Str.Frame_Tail = FRAME_TAIL; //帧尾
Send_Data.Sensor_Str.X_speed = ((MOTOR_A.Encoder+MOTOR_B.Encoder)/2)*1000;
Send_Data.Sensor_Str.Y_speed = 0;
Send_Data.Sensor_Str.Z_speed = ((MOTOR_B.Encoder-MOTOR_A.Encoder)/Wheel_spacing)*1000;
//加速度计三轴加速度
Send_Data.Sensor_Str.Accelerometer.X_data= accel[1]; //加速度计Y轴转换到ROS坐标X轴
Send_Data.Sensor_Str.Accelerometer.Y_data=-accel[0]; //加速度计X轴转换到ROS坐标Y轴
Send_Data.Sensor_Str.Accelerometer.Z_data= accel[2]; //加速度计Z轴转换到ROS坐标Z轴
//角速度计三轴角速度
Send_Data.Sensor_Str.Gyroscope.X_data= gyro[1]; //角速度计Y轴转换到ROS坐标X轴
Send_Data.Sensor_Str.Gyroscope.Y_data=-gyro[0]; //角速度计X轴转换到ROS坐标Y轴
if(Flag_Stop==0)
//如果电机控制位使能状态，那么正常发送Z轴角速度
Send_Data.Sensor_Str.Gyroscope.Z_data=gyro[2];
else
//如果机器人是静止的（电机控制位失能），那么发送的Z轴角速度为0
Send_Data.Sensor_Str.Gyroscope.Z_data=0;
//电池电压(这里将浮点数放大一千倍传输，相应的在接收端在接收到数据后也会缩小一千倍)
Send_Data.Sensor_Str.Power_Voltage = Voltage*1000;
Send_Data.buffer[0]=Send_Data.Sensor_Str.Frame_Header; //帧头
Send_Data.buffer[1]=Flag_Stop; //小车软件失能标志位
//小车三轴速度,各轴都拆分为两个8位数据再发送
Send_Data.buffer[2]=Send_Data.Sensor_Str.X_speed >>8;
Send_Data.buffer[3]=Send_Data.Sensor_Str.X_speed ;
Send_Data.buffer[4]=Send_Data.Sensor_Str.Y_speed>>8;
Send_Data.buffer[5]=Send_Data.Sensor_Str.Y_speed;
Send_Data.buffer[6]=Send_Data.Sensor_Str.Z_speed >>8;
Send_Data.buffer[7]=Send_Data.Sensor_Str.Z_speed ;
//IMU加速度计三轴加速度,各轴都拆分为两个8位数据再发送
Send_Data.buffer[8]=Send_Data.Sensor_Str.Accelerometer.X_data>>8;
Send_Data.buffer[9]=Send_Data.Sensor_Str.Accelerometer.X_data;
Send_Data.buffer[10]=Send_Data.Sensor_Str.Accelerometer.Y_data>>8;
Send_Data.buffer[11]=Send_Data.Sensor_Str.Accelerometer.Y_data;
Send_Data.buffer[12]=Send_Data.Sensor_Str.Accelerometer.Z_data>>8;
Send_Data.buffer[13]=Send_Data.Sensor_Str.Accelerometer.Z_data;
//IMU角速度计三轴角速度,各轴都拆分为两个8位数据再发送
Send_Data.buffer[14]=Send_Data.Sensor_Str.Gyroscope.X_data>>8;
Send_Data.buffer[15]=Send_Data.Sensor_Str.Gyroscope.X_data;
Send_Data.buffer[16]=Send_Data.Sensor_Str.Gyroscope.Y_data>>8;
Send_Data.buffer[17]=Send_Data.Sensor_Str.Gyroscope.Y_data;
Send_Data.buffer[18]=Send_Data.Sensor_Str.Gyroscope.Z_data>>8;
Send_Data.buffer[19]=Send_Data.Sensor_Str.Gyroscope.Z_data;
//电池电压,拆分为两个8位数据发送
Send_Data.buffer[20]=Send_Data.Sensor_Str.Power_Voltage >>8;
Send_Data.buffer[21]=Send_Data.Sensor_Str.Power_Voltage;
//数据校验位计算，模式1是发送数据校验
Send_Data.buffer[22]=Check_Sum(22,1);
Send_Data.buffer[23]=Send_Data.Sensor_Str.Frame_Tail; //帧尾
}
通信协议的上位机实现
数据接收
bool origincar_base::Get_Sensor_Data()
{
short transition_16 = 0, j = 0, Header_Pos = 0, Tail_Pos = 0;
uint8_t Receive_Data_Pr[RECEIVE_DATA_SIZE] = {0};
Stm32_Serial.read(Receive_Data_Pr,sizeof (Receive_Data_Pr));
for (j = 0; j < 24; j++) {
if (Receive_Data_Pr[j] == FRAME_HEADER)
Header_Pos=j;
else if (Receive_Data_Pr[j] == FRAME_TAIL)
Tail_Pos = j;
}
if (Tail_Pos == (Header_Pos + 23)) {
memcpy(Receive_Data.rx, Receive_Data_Pr, sizeof(Receive_Data_Pr));
} else if (Header_Pos == (1 + Tail_Pos)) {
for (j = 0;j < 24; j++)
Receive_Data.rx[j] = Receive_Data_Pr[(j+Header_Pos) % 24];
} else {
return false;
}
Receive_Data.Frame_Header = Receive_Data.rx[0];
Receive_Data.Frame_Tail = Receive_Data.rx[23];
if (Receive_Data.Frame_Header == FRAME_HEADER) {
if (Receive_Data.Frame_Tail == FRAME_TAIL) {
if (Receive_Data.rx[22] == Check_Sum(22,READ_DATA_CHECK)||(Header_Pos == (1 + Tail_Pos))) {
Receive_Data.Flag_Stop=Receive_Data.rx[1];
Robot_Vel.X = Odom_Trans(Receive_Data.rx[2],Receive_Data.rx[3]);
Robot_Vel.Y = Odom_Trans(Receive_Data.rx[4],Receive_Data.rx[5]);
Robot_Vel.Z = Odom_Trans(Receive_Data.rx[6],Receive_Data.rx[7]);
Mpu6050_Data.accele_x_data = IMU_Trans(Receive_Data.rx[8],Receive_Data.rx[9]);
Mpu6050_Data.accele_y_data = IMU_Trans(Receive_Data.rx[10],Receive_Data.rx[11]);
Mpu6050_Data.accele_z_data = IMU_Trans(Receive_Data.rx[12],Receive_Data.rx[13]);
Mpu6050_Data.gyros_x_data = IMU_Trans(Receive_Data.rx[14],Receive_Data.rx[15]);
Mpu6050_Data.gyros_y_data = IMU_Trans(Receive_Data.rx[16],Receive_Data.rx[17]);
Mpu6050_Data.gyros_z_data = IMU_Trans(Receive_Data.rx[18],Receive_Data.rx[19]);
Mpu6050.linear_acceleration.x = Mpu6050_Data.accele_x_data / ACCEl_RATIO;
Mpu6050.linear_acceleration.y = Mpu6050_Data.accele_y_data / ACCEl_RATIO;
Mpu6050.linear_acceleration.z = Mpu6050_Data.accele_z_data / ACCEl_RATIO;
Mpu6050.angular_velocity.x = Mpu6050_Data.gyros_x_data * GYROSCOPE_RATIO;
Mpu6050.angular_velocity.y = Mpu6050_Data.gyros_y_data * GYROSCOPE_RATIO;
Mpu6050.angular_velocity.z = Mpu6050_Data.gyros_z_data * GYROSCOPE_RATIO;
transition_16 = 0;
transition_16 |= Receive_Data.rx[20]<<8;
transition_16 |= Receive_Data.rx[21];
Power_voltage = transition_16/1000+(transition_16 % 1000)*0.001;
return true;
}
}
}
return false;
}
数据发送
void origincar_base::Cmd_Vel_Callback(const geometry_msgs::msg::Twist::SharedPtr twist_aux)
{
short transition;
Send_Data.tx[0]=FRAME_HEADER;
Send_Data.tx[1] = 0;
Send_Data.tx[2] = 0;
transition=0;
transition = twist_aux->linear.x*1000;
Send_Data.tx[4] = transition;
Send_Data.tx[3] = transition>>8;
transition=0;
transition = twist_aux->linear.y*1000;
Send_Data.tx[6] = transition;
Send_Data.tx[5] = transition>>8;
transition=0;
transition = (twist_aux->angular.z)*1000;
Send_Data.tx[8] = transition;
Send_Data.tx[7] = transition>>8;
Send_Data.tx[9]=Check_Sum(9,SEND_DATA_CHECK);
Send_Data.tx[10]=FRAME_TAIL;
try {
if (akm_cmd_vel == "none") {
Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx));
}
} catch (serial::IOException& e) {
RCLCPP_ERROR(this->get_logger(),("Unable to send data through serial port"));
}
}
构建机器人ROS 驱动
其实完成上下位机的解析与数据封装发送后，ROS相关的驱动就自然而然出来了。大家可以看到在origincar_base 中是小车上位机ROS底层驱动实现的代码，笔者当时写的时候关注的点由两个，第一个哪些数据是之后开发需要的，第二个点是如何让代码变得更好用。
哪些数据是之后开发需要的？
其实主要是传感器数据，例如电压的电压值，对于开发者而言，电压值有可能影响后续功能能否正常使用，如何让开发者不看小车就知道小车电量如何是一件很重要的事情。
void origincar_base::Publish_Voltage()
{
std_msgs::msg::Float32 voltage_msgs;
static float Count_Voltage_Pub = 0;
if (Count_Voltage_Pub++ > 10) {
Count_Voltage_Pub = 0;
voltage_msgs.data = Power_voltage;
voltage_publisher->publish(voltage_msgs);
}
}
又好比里程计和IMU值，同样重要，甚至此处可能涉及了里程计和IMU校正的问题，所以自然也需要做一层封装
void origincar_base::Publish_ImuSensor()
{
sensor_msgs::msg::Imu Imu_Data_Pub;
Imu_Data_Pub.header.stamp = rclcpp::Node::now();
Imu_Data_Pub.header.frame_id = gyro_frame_id;
Imu_Data_Pub.orientation.x = Mpu6050.orientation.x;
Imu_Data_Pub.orientation.y = Mpu6050.orientation.y;
Imu_Data_Pub.orientation.z = Mpu6050.orientation.z;
Imu_Data_Pub.orientation.w = Mpu6050.orientation.w;
Imu_Data_Pub.orientation_covariance[0] = 1e6;
Imu_Data_Pub.orientation_covariance[4] = 1e6;
Imu_Data_Pub.orientation_covariance[8] = 1e-6;
Imu_Data_Pub.angular_velocity.x = Mpu6050.angular_velocity.x;
Imu_Data_Pub.angular_velocity.y = Mpu6050.angular_velocity.y;
Imu_Data_Pub.angular_velocity.z = Mpu6050.angular_velocity.z;
Imu_Data_Pub.angular_velocity_covariance[0] = 1e6;
Imu_Data_Pub.angular_velocity_covariance[4] = 1e6;
Imu_Data_Pub.angular_velocity_covariance[8] = 1e-6;
Imu_Data_Pub.linear_acceleration.x = Mpu6050.linear_acceleration.x;
Imu_Data_Pub.linear_acceleration.y = Mpu6050.linear_acceleration.y;
Imu_Data_Pub.linear_acceleration.z = Mpu6050.linear_acceleration.z;
imu_publisher->publish(Imu_Data_Pub);
}
void origincar_base::Publish_Odom()
{
tf2::Quaternion q;
q.setRPY(0,0,Robot_Pos.Z);
geometry_msgs::msg::Quaternion odom_quat=tf2::toMsg(q);
origincar_msg::msg::Data robotpose;
origincar_msg::msg::Data robotvel;
nav_msgs::msg::Odometry odom;
odom.header.stamp = rclcpp::Node::now();
odom.header.frame_id = odom_frame_id;
odom.child_frame_id = robot_frame_id;
odom.pose.pose.position.x = Robot_Pos.X;
odom.pose.pose.position.y = Robot_Pos.Y;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = odom_quat;
odom.twist.twist.linear.x = Robot_Vel.X;
odom.twist.twist.linear.y = Robot_Vel.Y;
odom.twist.twist.angular.z = Robot_Vel.Z;
robotpose.x = Robot_Pos.X;
robotpose.y = Robot_Pos.Y;
robotpose.z = Robot_Pos.Z;
robotvel.x = Robot_Vel.X;
robotvel.y = Robot_Vel.Y;
robotvel.z = Robot_Vel.Z;
odom_publisher->publish(odom);
robotpose_publisher->publish(robotpose);
robotvel_publisher->publish(robotvel);
}
谈到这，再来说如何对里程计和IMU进行校正
大家可以看到这一段：
void origincar_base::Control()
{
...
Robot_Pos.X+=1.03*(Robot_Vel.X * cos(Robot_Pos.Z) - Robot_Vel.Y * sin(Robot_Pos.Z)) * Sampling_Time;
Robot_Pos.Y+=1.125*(Robot_Vel.X * sin(Robot_Pos.Z) + Robot_Vel.Y * cos(Robot_Pos.Z)) * Sampling_Time;
Robot_Pos.Z+=Robot_Vel.Z * Sampling_Time;
...
}
此处对小车具体执行的X、Y、Z进行了控制，大家可以如何解决呢？让小车真实走1m距离，然后观察odom位置是多少，如果存在差异，那么说明控制需要修正。比如此时实际运行的距离是1.023m，而里程计反馈的是1.139m，那么计算线速度的校准系数 = 实际运行距离 / 里程计反馈距离 比如：1.023/1.139 = 0.898；这个值就应该填入以上1.03的位置。
如何让代码变得更好用？
这里其实解决了一个很重要的问题，例如底盘挂掉了，或者你希望一关掉底盘程序车就停下来该如何做。
此处用了一些小ticks，即引入了一个新线程去做调度。
void sigintHandler(int sig)
{
sig = sig;
printf("OriginCar shutdown...\n");
serial::Serial Stm32_Serial;
Stm32_Serial.setPort("/dev/ttyACM0");
Stm32_Serial.setBaudrate(115200);
serial::Timeout _time = serial::Timeout::simpleTimeout(2000);
Stm32_Serial.setTimeout(_time);
Stm32_Serial.open();
SEND_DATA Send_Data;
if (Stm32_Serial.isOpen()) {
Send_Data.tx[0]=FRAME_HEADER;
Send_Data.tx[1] = 0;
Send_Data.tx[2] = 0;
Send_Data.tx[4] = 0;
Send_Data.tx[3] = 0;
Send_Data.tx[6] = 0;
Send_Data.tx[5] = 0;
Send_Data.tx[7] = 0;
Send_Data.tx[8] = 0;
int check_sum = 0;
for (int k = 0; k < 9; k++) {
check_sum = check_sum^Send_Data.tx[k];
}
Send_Data.tx[9]=check_sum;
Send_Data.tx[10]=FRAME_TAIL;
try {
Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx));
} catch (serial::IOException& e) {
}
}
// 关闭ROS2接口，清除资源
rclcpp::shutdown();
}
评论（0）
您还未登录，请登录后发表或查看评论