STM32下位機與帶ROS系統的上位機通信的學習

項目使用TX2上位機與下位機STM32進行通信，故對此進行一些研究。之前也用過STM32通過串口與裝有ROS的筆記進行通信，使用的是 “基於STM32的rosserial_client的節點開發”，見鏈接，文章中使用的是STM32F4系列的單片機，本人在F1系列中實現了，但是存在代碼復雜和運行效率不高的問題。因此本次直接使用串口與TX2建立通信。

上位機（TX2）與下位機（STM32）通過串口進行通信

參考鏈接：https://www.ncnynl.com/archives/201703/1417.html

下位機

發送程序

for(i=0;i<21;i++)
{
    USART_ClearFlag(USART1,USART_FLAG_TC);  //在發送第一個數據前加此句，解決第一個數據不能正常發送的問題             
    USART_SendData(USART1,odometry_data[i]);//發送一個字節到串口 
    while(USART_GetFlagStatus(USART1, USART_FLAG_TC) == RESET); //等待發送結束            
}

 

接收程序

if(USART_RX_STA&0x8000)  // 串口1接收函數
{           
    //接收左右輪速度
    for(t=0;t<4;t++)
    {
        rightdata.data[t]=USART_RX_BUF[t];
        leftdata.data[t]=USART_RX_BUF[t+4];
    }

    //儲存左右輪速度
    odometry_right=rightdata.d;//單位mm/s
    odometry_left=leftdata.d;//單位mm/s

    USART_RX_STA=0;//清楚接收標志位
}

 

串口接收中斷程序

void USART1_IRQHandler(void)//串口中斷函數
{
  if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET) //是否接受到數據
    {
    serial_rec =USART_ReceiveData(USART1);//(USART1->DR);    //讀取接收到的數據
          
    if((USART_RX_STA&0x8000)==0)//接收未完成
        {
            if(USART_RX_STA&0x4000)//接收到了0x0d
            {
                if(serial_rec==0x0a)
                {
                    if((USART_RX_STA&0x3f)==8)
                    {                            
                        USART_RX_STA|=0x8000;    //接收完成了 
                        main_sta|=0x04;
                        main_sta&=0xF7;
                    }
                    else
                    {
                        main_sta|=0x08;
                        main_sta&=0xFB;
                        USART_RX_STA=0;//接收錯誤,重新開始
                    }
                }
                else 
                {
                    main_sta|=0x08;
                    USART_RX_STA=0;//接收錯誤,重新開始
                }
            }
            else //還沒收到0X0D
            {    
                if(serial_rec==0x0d)USART_RX_STA|=0x4000;
                else
                {
                    USART_RX_BUF[USART_RX_STA&0X3FFF]=serial_rec ;
                    USART_RX_STA++;
                    if(USART_RX_STA>(USART_REC_LEN-1))
                    {
                        main_sta|=0x08;
                        USART_RX_STA=0;//接收數據錯誤,重新開始接收
                    }                            
                }         
            }
        }            
    }
}

 

這之中我們需要規避一個問題，串口接收中斷是以/r/n為中止條件，我們需要避免其中的數據中包含了/r（0x0d）的情況，而上述程序並沒有規避這一點。

void USART1_IRQHandler(void)//串口中斷函數
{
  if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET) //是否接受到數據
    {
    serial_rec =USART_ReceiveData(USART1);//(USART1->DR);    //讀取接收到的數據
          
    if((USART_RX_STA&0x8000)==0)//接收未完成
        {
            if(USART_RX_STA&0x4000)//接收到了0x0d
            {
                if(serial_rec==0x0a)
                {
                    if((USART_RX_STA&0x3f)==8)
                    {                            
                        USART_RX_STA|=0x8000;    //接收完成了 
                        main_sta|=0x04;
                        main_sta&=0xF7;
                    }
                    else
                    {
                        main_sta|=0x08;
                        main_sta&=0xFB;
                        USART_RX_STA=0;//接收錯誤,重新開始
                    }
                }
                else 
                {
                    main_sta|=0x08;
                    USART_RX_STA=0;//接收錯誤,重新開始
                }
            }
            else //還沒收到0X0D
            {    
                if(serial_rec==0x0d&&(USART_RX_STA&0x3f)==8)USART_RX_STA|=0x4000;
                else
                {
                    USART_RX_BUF[USART_RX_STA&0X3FFF]=serial_rec ;
                    USART_RX_STA++;
                    if(USART_RX_STA>(USART_REC_LEN-1))
                    {
                        main_sta|=0x08;
                        USART_RX_STA=0;//接收數據錯誤,重新開始接收
                    }                            
                }         
            }
        }            
    }
}

 

 

上位機

發送代碼

void callback(const geometry_msgs::Twist & cmd_input)//訂閱/cmd_vel主題回調函數
{
    string port("/dev/ttyUSB0");    //小車串口號
    unsigned long baud = 115200;    //小車串口波特率
    serial::Serial my_serial(port, baud, serial::Timeout::simpleTimeout(1000)); //配置串口

    angular_temp = cmd_input.angular.z ;//獲取/cmd_vel的角速度,rad/s
    linear_temp = cmd_input.linear.x ;//獲取/cmd_vel的線速度.m/s

    //將轉換好的小車速度分量為左右輪速度
    left_speed_data.d = linear_temp - 0.5f*angular_temp*D ;
    right_speed_data.d = linear_temp + 0.5f*angular_temp*D ;

    //存入數據到要發布的左右輪速度消息
    left_speed_data.d*=ratio;   //放大１０００倍，mm/s
    right_speed_data.d*=ratio;//放大１０００倍，mm/s

    for(int i=0;i<4;i++)    //將左右輪速度存入數組中發送給串口
    {
        speed_data[i]=right_speed_data.data[i];
        speed_data[i+4]=left_speed_data.data[i];
    }

    //在寫入串口的左右輪速度數據后加入”/r/n“
    speed_data[8]=data_terminal0;
    speed_data[9]=data_terminal1;
    //寫入數據到串口
    my_serial.write(speed_data,10);
}

 

接收程序

rec_buffer =my_serial.readline(25,"\n");    //獲取串口發送來的數據
const char *receive_data=rec_buffer.data(); //保存串口發送來的數據