一般情况，上位机由ROS框架运行在Ubuntu的树莓派构成，下位机由STM32F103VET6芯片板载电机，舵机，陀螺仪，里程计等构成。里程计提供ROS需要的速度信息，陀螺仪提供加速度方向等信息给ROS，再加上连接到树莓派上的激光雷达，ROS就可以进行SLAM制图和导航了。下位机接收到ROS下发的速度信息后，转换成电机的PWM信号和舵机的PWM信号进行方向和速度控制。
Content:
本篇就介绍上位机和下位机进行通信的一种方式：串口通信。
STM32串口发送与接收
首先进行串口3的初始化，其对应的GPIO口为C10发送，C11接收。
void uart3_init(u32 bound) { GPIO_InitTypeDef GPIO_InitStructure; USART_InitTypeDef USART_InitStructure; NVIC_InitTypeDef NVIC_InitStructure; RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE); //Enable the AFIO clock //使能AFIO时钟 RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE); //Enable the gpio clock //使能GPIO时钟 RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE); //Enable the Usart clock //使能USART时钟 GPIO_PinRemapConfig(GPIO_PartialRemap_USART3, ENABLE); //Pin remapping //引脚重映射 //USART_TX 发送端口配置 GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10; //C10 GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; //Reuse push-pull output //复用推挽输出 GPIO_Init(GPIOC, &GPIO_InitStructure); //USART_RX 接收端口配置 GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11; //PC11 GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU; //Pull up input//上拉输入 GPIO_Init(GPIOC, &GPIO_InitStructure); //UsartNVIC configuration //UsartNVIC配置 中断优先级 NVIC_InitStructure.NVIC_IRQChannel = USART3_IRQn; //Preempt priority //抢占优先级 NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=2 ; //Preempt priority //抢占优先级 NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; //Enable the IRQ channel //IRQ通道使能 NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //Initialize the VIC register with the specified parameters //根据指定的参数初始化VIC寄存器 NVIC_Init(&NVIC_InitStructure); //USART Initialization Settings 串口初始化设置 USART_InitStructure.USART_BaudRate = bound; //Port rate //串口波特率 USART_InitStructure.USART_WordLength = USART_WordLength_8b; //The word length is 8 bit data format //字长为8位数据格式 USART_InitStructure.USART_StopBits = USART_StopBits_1; //A stop bit //一个停止 USART_InitStructure.USART_Parity = USART_Parity_No; //Prosaic parity bits //无奇偶校验位 USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; //No hardware data flow control //无硬件数据流控制 USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx; //Sending and receiving mode //收发模式 USART_Init(USART3, &USART_InitStructure); //Initialize serial port 3 //初始化串口3 //开启中断，其对应的中断函数为USART3_IRQHandler，这是启动函数里面hard code的 USART_ITConfig(USART3, USART_IT_RXNE, ENABLE); //Open the serial port to accept interrupts //开启串口接受中断 USART_Cmd(USART3, ENABLE); //Enable serial port 3 //使能串口3 } |
中断后，从串口中读取上位机发送的数据到Receive_Data变量中。并从Receive_Data中解析出3轴的速度：Move_X，Move_Y和Move_Z。
int USART3_IRQHandler(void) { static u8 Count=0; u8 Usart_Receive; if(USART_GetITStatus(USART3, USART_IT_RXNE) != RESET) //Check if data is received //判断是否接收到数据 { Usart_Receive = USART_ReceiveData(USART3);//Read the data //读取数据 if(Time_count<CONTROL_DELAY) // Data is not processed until 10 seconds after startup //开机10秒前不处理数据 return 0; //Fill the array with serial data //串口数据填入数组 Receive_Data.buffer[Count]=Usart_Receive; // Ensure that the first data in the array is FRAME_HEADER //确保数组第一个数据为FRAME_HEADER if(Usart_Receive == FRAME_HEADER||Count>0) Count++; else Count=0; if (Count == 11) //Verify the length of the packet //验证数据包的长度 { Count=0; //Prepare for the serial port data to be refill into the array //为串口数据重新填入数组做准备 if(Receive_Data.buffer[10] == FRAME_TAIL) //Verify the frame tail of the packet //验证数据包的帧尾 { //Data exclusionary or bit check calculation, mode 0 is sent data check //数据异或位校验计算，模式0是发送数据校验 if(Receive_Data.buffer[9] ==Check_Sum(9,0)) { //All modes flag position 0, USART3 control mode //所有模式标志位置0，为Usart3控制模式 PS2_ON_Flag=0; Remote_ON_Flag=0; APP_ON_Flag=0; CAN_ON_Flag=0; Usart_ON_Flag=0; //Calculate the target speed of three axis from serial data, unit m/s //从串口数据求三轴目标速度， 单位m/s 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]); Move_Z=XYZ_Target_Speed_transition(Receive_Data.buffer[7],Receive_Data.buffer[8]); } } } } return 0; } |
下位机定时给上位机发送当前小车的速度，加速度，角速度等信息。
void data_transition(void) { Send_Data.Sensor_Str.Frame_Header = FRAME_HEADER; //Frame_header //帧头 Send_Data.Sensor_Str.Frame_Tail = FRAME_TAIL; //Frame_tail //帧尾 //According to different vehicle types, different kinematics algorithms were selected to carry out the forward kinematics solution, //and the three-axis velocity was obtained from each wheel velocity //根据不同车型选择不同运动学算法进行运动学正解，从各车轮速度求出三轴速度 switch(Car_Mode) { case Akm_Car: 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; break; } //The acceleration of the triaxial acceleration //加速度计三轴加速度 Send_Data.Sensor_Str.Accelerometer.X_data= accel[1]; //The accelerometer Y-axis is converted to the ros coordinate X axis //加速度计Y轴转换到ROS坐标X轴 Send_Data.Sensor_Str.Accelerometer.Y_data=-accel[0]; //The accelerometer X-axis is converted to the ros coordinate y axis //加速度计X轴转换到ROS坐标Y轴 Send_Data.Sensor_Str.Accelerometer.Z_data= accel[2]; //The accelerometer Z-axis is converted to the ros coordinate Z axis //加速度计Z轴转换到ROS坐标Z轴 //The Angle velocity of the triaxial velocity //角速度计三轴角速度 Send_Data.Sensor_Str.Gyroscope.X_data= gyro[1]; //The Y-axis is converted to the ros coordinate X axis //角速度计Y轴转换到ROS坐标X轴 Send_Data.Sensor_Str.Gyroscope.Y_data=-gyro[0]; //The X-axis is converted to the ros coordinate y axis //角速度计X轴转换到ROS坐标Y轴 if(Flag_Stop==0) //If the motor control bit makes energy state, the z-axis velocity is sent normall //如果电机控制位使能状态，那么正常发送Z轴角速度 Send_Data.Sensor_Str.Gyroscope.Z_data=gyro[2]; else //If the robot is static (motor control dislocation), the z-axis is 0 //如果机器人是静止的（电机控制位失能），那么发送的Z轴角速度为0 Send_Data.Sensor_Str.Gyroscope.Z_data=0; //Battery voltage (this is a thousand times larger floating point number, which will be reduced by a thousand times as well as receiving the data). //电池电压(这里将浮点数放大一千倍传输，相应的在接收端在接收到数据后也会缩小一千倍) Send_Data.Sensor_Str.Power_Voltage = Voltage*1000; Send_Data.buffer[0]=Send_Data.Sensor_Str.Frame_Header; //Frame_heade //帧头 Send_Data.buffer[1]=Flag_Stop; //Car software loss marker //小车软件失能标志位 //The three-axis speed of / / car is split into two eight digit Numbers //小车三轴速度,各轴都拆分为两个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 ; //The acceleration of the triaxial axis of / / imu accelerometer is divided into two eight digit reams //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; //The axis of the triaxial velocity of the / /imu is divided into two eight digits //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; //Battery voltage, split into two 8 digit Numbers //电池电压,拆分为两个8位数据发送 Send_Data.buffer[20]=Send_Data.Sensor_Str.Power_Voltage >>8; Send_Data.buffer[21]=Send_Data.Sensor_Str.Power_Voltage; //Data check digit calculation, Pattern 1 is a data check //数据校验位计算，模式1是发送数据校验 Send_Data.buffer[22]=Check_Sum(22,1); Send_Data.buffer[23]=Send_Data.Sensor_Str.Frame_Tail; //Frame_tail //帧尾 } void USART3_SEND(void) { unsigned char i = 0; for(i=0; i<24; i++) { usart3_send(Send_Data.buffer[i]); } } void usart3_send(u8 data) { USART3->DR = data; while((USART3->SR&0x40)==0); } |
ROS串口发送与接收
在ROS中注册小车节点，并利用节点的回调函数Cmd_Vel_Callback组装数据，通过库serial完成串口数据发送。
void turn_on_robot::Cmd_Vel_Callback(const geometry_msgs::Twist &twist_aux) { short transition; //intermediate variable //中间变量 Send_Data.tx[0]=FRAME_HEADER; //frame head 0x7B //帧头0X7B Send_Data.tx[1] = 0; //set aside //预留位 Send_Data.tx[2] = 0; //set aside //预留位 //The target velocity of the X-axis of the robot //机器人x轴的目标线速度 transition=0; transition = twist_aux.linear.x*1000; //将浮点数放大一千倍，简化传输 Send_Data.tx[4] = transition; //取数据的低8位 Send_Data.tx[3] = transition>>8; //取数据的高8位 //The target velocity of the Y-axis of the robot //机器人y轴的目标线速度 transition=0; transition = twist_aux.linear.y*1000; Send_Data.tx[6] = transition; Send_Data.tx[5] = transition>>8; //The target angular velocity of the robot's Z axis //机器人z轴的目标角速度 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); //For the BBC check bits, see the Check_Sum function //BBC校验位，规则参见Check_Sum函数 Send_Data.tx[10]=FRAME_TAIL; //frame tail 0x7D //帧尾0X7D try { if(akm_cmd_vel=="none") Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx)); //Sends data to the downloader via serial port //通过串口向下位机发送数据 //else ROS_INFO("akm mode, not subcribe cmd_vel"); } catch (serial::IOException& e) { ROS_ERROR_STREAM("Unable to send data through serial port"); //If sending data fails, an error message is printed //如果发送数据失败，打印错误信息 } } |
同时，在ROS的节点循环中，从串口读Stm32_Serial.read下位机发送过来的数据。
bool turn_on_robot::Get_Sensor_Data() { short transition_16=0, j=0, Header_Pos=0, Tail_Pos=0; //Intermediate variable //中间变量 uint8_t Receive_Data_Pr[RECEIVE_DATA_SIZE]={0}; //Temporary variable to save the data of the lower machine //临时变量，保存下位机数据 Stm32_Serial.read(Receive_Data_Pr,sizeof (Receive_Data_Pr)); //Read the data sent by the lower computer through the serial port //通过串口读取下位机发送过来的数据 /*//View the received raw data directly and debug it for use//直接查看接收到的原始数据，调试使用 ROS_INFO("%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x", Receive_Data_Pr[0],Receive_Data_Pr[1],Receive_Data_Pr[2],Receive_Data_Pr[3],Receive_Data_Pr[4],Receive_Data_Pr[5],Receive_Data_Pr[6],Receive_Data_Pr[7], Receive_Data_Pr[8],Receive_Data_Pr[9],Receive_Data_Pr[10],Receive_Data_Pr[11],Receive_Data_Pr[12],Receive_Data_Pr[13],Receive_Data_Pr[14],Receive_Data_Pr[15], Receive_Data_Pr[16],Receive_Data_Pr[17],Receive_Data_Pr[18],Receive_Data_Pr[19],Receive_Data_Pr[20],Receive_Data_Pr[21],Receive_Data_Pr[22],Receive_Data_Pr[23]); */ //Record the position of the head and tail of the frame //记录帧头帧尾位置 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)) { //If the end of the frame is the last bit of the packet, copy the packet directly to receive_data.rx //如果帧尾在数据包最后一位，直接复制数据包到Receive_Data.rx // ROS_INFO("1----"); memcpy(Receive_Data.rx, Receive_Data_Pr, sizeof(Receive_Data_Pr)); } else if(Header_Pos==(1+Tail_Pos)) { //如果帧头在帧尾后面，纠正数据位置后复制数据包到Receive_Data.rx // If the header is behind the end of the frame, copy the packet to receive_data.rx after correcting the data location // ROS_INFO("2----"); for(j=0;j<24;j++) Receive_Data.rx[j]=Receive_Data_Pr[(j+Header_Pos)%24]; } else { //其它情况则认为数据包有错误 // In other cases, the packet is considered to be faulty // ROS_INFO("3----"); return false; } /* //Check receive_data.rx for debugging use //查看Receive_Data.rx，调试使用 ROS_INFO("%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x", Receive_Data.rx[0],Receive_Data.rx[1],Receive_Data.rx[2],Receive_Data.rx[3],Receive_Data.rx[4],Receive_Data.rx[5],Receive_Data.rx[6],Receive_Data.rx[7], Receive_Data.rx[8],Receive_Data.rx[9],Receive_Data.rx[10],Receive_Data.rx[11],Receive_Data.rx[12],Receive_Data.rx[13],Receive_Data.rx[14],Receive_Data.rx[15], Receive_Data.rx[16],Receive_Data.rx[17],Receive_Data.rx[18],Receive_Data.rx[19],Receive_Data.rx[20],Receive_Data.rx[21],Receive_Data.rx[22],Receive_Data.rx[23]); */ Receive_Data.Frame_Header= Receive_Data.rx[0]; //The first part of the data is the frame header 0X7B //数据的第一位是帧头0X7B Receive_Data.Frame_Tail= Receive_Data.rx[23]; //The last bit of data is frame tail 0X7D //数据的最后一位是帧尾0X7D if (Receive_Data.Frame_Header == FRAME_HEADER ) //Judge the frame header //判断帧头 { if (Receive_Data.Frame_Tail == FRAME_TAIL) //Judge the end of the frame //判断帧尾 { if (Receive_Data.rx[22] == Check_Sum(22,READ_DATA_CHECK)||(Header_Pos==(1+Tail_Pos))) //BBC check passes or two packets are interlaced //BBC校验通过或者两组数据包交错 { Receive_Data.Flag_Stop=Receive_Data.rx[1]; //set aside //预留位 Robot_Vel.X = Odom_Trans(Receive_Data.rx[2],Receive_Data.rx[3]); //Get the speed of the moving chassis in the X direction //获取运动底盘X方向速度 Robot_Vel.Y = Odom_Trans(Receive_Data.rx[4],Receive_Data.rx[5]); //Get the speed of the moving chassis in the Y direction, The Y speed is only valid in the omnidirectional mobile robot chassis //获取运动底盘Y方向速度，Y速度仅在全向移动机器人底盘有效 Robot_Vel.Z = Odom_Trans(Receive_Data.rx[6],Receive_Data.rx[7]); //Get the speed of the moving chassis in the Z direction //获取运动底盘Z方向速度 //MPU6050 stands for IMU only and does not refer to a specific model. It can be either MPU6050 or MPU9250 //Mpu6050仅代表IMU，不指代特定型号，既可以是MPU6050也可以是MPU9250 Mpu6050_Data.accele_x_data = IMU_Trans(Receive_Data.rx[8],Receive_Data.rx[9]); //Get the X-axis acceleration of the IMU //获取IMU的X轴加速度 Mpu6050_Data.accele_y_data = IMU_Trans(Receive_Data.rx[10],Receive_Data.rx[11]); //Get the Y-axis acceleration of the IMU //获取IMU的Y轴加速度 Mpu6050_Data.accele_z_data = IMU_Trans(Receive_Data.rx[12],Receive_Data.rx[13]); //Get the Z-axis acceleration of the IMU //获取IMU的Z轴加速度 Mpu6050_Data.gyros_x_data = IMU_Trans(Receive_Data.rx[14],Receive_Data.rx[15]); //Get the X-axis angular velocity of the IMU //获取IMU的X轴角速度 Mpu6050_Data.gyros_y_data = IMU_Trans(Receive_Data.rx[16],Receive_Data.rx[17]); //Get the Y-axis angular velocity of the IMU //获取IMU的Y轴角速度 Mpu6050_Data.gyros_z_data = IMU_Trans(Receive_Data.rx[18],Receive_Data.rx[19]); //Get the Z-axis angular velocity of the IMU //获取IMU的Z轴角速度 //Linear acceleration unit conversion is related to the range of IMU initialization of STM32, where the range is ±2g=19.6m/s^2 //线性加速度单位转化，和STM32的IMU初始化的时候的量程有关,这里量程±2g=19.6m/s^2 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; //The gyroscope unit conversion is related to the range of STM32's IMU when initialized. Here, the range of IMU's gyroscope is ±500°/s //Because the robot generally has a slow Z-axis speed, reducing the range can improve the accuracy //陀螺仪单位转化，和STM32的IMU初始化的时候的量程有关，这里IMU的陀螺仪的量程是±500°/s //因为机器人一般Z轴速度不快，降低量程可以提高精度 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; //Get the battery voltage //获取电池电压 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; //Unit conversion millivolt(mv)->volt(v) //单位转换毫伏(mv)->伏(v) return true; } } } return false; } |
本作品采用知识共享署名 4.0 国际许可协议进行许可。