STM32 ROS控制器底层代码

本文主要对控制器底层代码的整天架构进行讲解。

控制器由两部分组成一部分是BootLoader,另一部分是APP；BootLoader主要用于固件升级，APP则作为应用程序。

BootLoader的地址为:0x8000000~0x8008000

App的地址为:0x8010000~0x8FFFFFF

参数保存地址为：0x8008000~:0x8010000

BootLoader使用的是裸机，主要是对升级标志位进行判断然后对运行区进行跳转和通过UART1、UART2把APP固件写入，目前只支持通过UART1（PA9，PA10）、UART2（PD5，PD6），对于首次更新则只能使用UART1或者UART2对APP进行写入，BootLoader代码目录如下图所示。BootLoader代码目前是不开源的，由博主进行维护，大家在使用中遇到问题博主会及时进行测试修改和同步更新烧入文件。

APP是由FreeRTOS作为嵌入式系统，使用rosserial(roslib库)和ROS1进行交互，使用标准的接口协议，让ROS的开发者不需要去了解底层的通行层，只需要关注底层控制器发布和订阅的节点名称即可，使用rosserial还可以使用ros的参数服务器，在ROS层即可对底层的一些开放参数进行修改（使用方法参考博主的另一篇博客 通过ROS修改控制器参数：https://blog.csdn.net/qq_36349536/article/details/126515022?spm=1001.2014.3001.5501）；除了通信部分控制器编写了D2（差速两轮）、D4（差速四轮）、T2、T4、O3、O4、M4、A1、A2等车型的运动学解算，PS2手柄驱动代码，MPU6050、MPU9250、MPU6500、GY85等IMU驱动代码、位置式PID代码，代码使用函数指针对应用层和驱动层进行了标准接口分离解耦每个文件的相互依赖，让用户可以更好的进行二次开发。下面主要对用户在二次开发过程中可能遇到的一些疑惑进行说明。

1、初始化入口讲解，初始化入口是用户任务代码和驱动代码的初始化位置，因为激活校准函数是授权使用的所以对main函数进行了封装，留出了任务代码入口和驱动代码入口，如下图所示：

用户在开发过程中任务初始化需要放到InitTaskArr[]中，驱动代码需要放到InitSetupArr[]中，InitLoopArr[]是作为运行逻机的预留接口，没用，用户无需对该数组进行操作。

2、roslib库的使用讲解，roslib库是rosserial生成过来用于嵌入式设备的一个库，具体的打包过程和怎么使用自定义msgs可以参考另一篇博客；roslib库主要是提供了3个函数接口，read(),wirte(),time(),我们只需要去实现这三条函数即可，接口函数是在stm32Hardware.h中。

我们需要先确定我们使用的STM32串口或者USB_VCP的收发函数是否正常，然后把实现好的函数在stm32Hardware.h文件中对应的位置加入即可。读写函数实现完成，我们还需要对这个库进行初始化

第一步创建一个nh句柄

ros::NodeHandle nh;

第二步对句柄进行初始化

nh.initNode();

第三步创建发布的msgs，订阅的回调函数，发布的节点名称和订阅的节点名称

1.  
   starrobot_msgs::Imu raw_imu_msg;
2.  
   starrobot_msgs::Velocities raw_vel_msg;
3.  
   starrobot_msgs::analog raw_analog_msg;
4.  
   starrobot_msgs::info_show raw_info_show_msg;
5.  
   starrobot_msgs::Sonar raw_sonar_msg;
6.  
   starrobot_msgs::Relaid raw_Relaid_msg;
7.  
   starrobot_msgs::Upgrader raw_Up_msg;
8.  
    
9.  
   void pid_callback( const starrobot_msgs::PID& pid);
10.  
    void command_callback(const geometry_msgs::Twist& cmd_msg);
11.  
    void swerve_callback(const std_msgs::Int16& swerve_servo);
12.  
    void servo_callback(const starrobot_msgs::Servo& servo_data);
13.  
    void relaid_callback(const starrobot_msgs::Relaid& data_t);
14.  
    void upgrader_callback(const starrobot_msgs::Upgrader& data_t);
15.  
    void handleParam_callback(const std_msgs::Int8& data_t);
16.  
     
17.  
    ros::Subscriber<geometry_msgs::Twist> cmd_sub("cmd_vel", command_callback);
18.  
    ros::Subscriber<starrobot_msgs::PID> pid_sub("pid", pid_callback);
19.  
    ros::Subscriber<std_msgs::Int16> swerve_sub("initial_angle",swerve_callback);
20.  
    ros::Subscriber<starrobot_msgs::Servo>servo_sub("servo",servo_callback);
21.  
    ros::Subscriber<starrobot_msgs::Relaid>relaid_sub("setRelaid",relaid_callback);
22.  
    ros::Subscriber<std_msgs::Int8>handleParam_sub("handleParam",handleParam_callback);
23.  
    ros::Subscriber<starrobot_msgs::Upgrader>upgrader_sub("PubUpgrade",upgrader_callback);
24.  
     
25.  
    ros::Publisher raw_vel_pub("raw_vel", &raw_vel_msg);
26.  
    ros::Publisher raw_imu_pub("raw_imu", &raw_imu_msg);
27.  
    ros::Publisher raw_battery_pub("raw_analog", &raw_analog_msg);
28.  
    ros::Publisher raw_info_show_pub("info_show", &raw_info_show_msg);
29.  
    ros::Publisher raw_sonar_pub("sonar", &raw_sonar_msg);
30.  
    ros::Publisher raw_relaid_pub("getRelaid", &raw_Relaid_msg);
31.  
    ros::Publisher raw_upgrader_pub("SubUpgrade", &raw_Up_msg);

第四步对发布和订阅的节点名称进行注册

1.  
   nh.subscribe(cmd_sub);
2.  
   nh.subscribe(pid_sub);
3.  
   nh.subscribe(servo_sub);
4.  
   nh.subscribe(swerve_sub);
5.  
   nh.subscribe(relaid_sub);
6.  
   nh.subscribe(handleParam_sub);
7.  
   nh.subscribe(upgrader_sub);
8.  
    
9.  
   nh.advertise(raw_vel_pub);
10.  
    nh.advertise(raw_imu_pub);
11.  
    nh.advertise(raw_battery_pub);
12.  
    nh.advertise(raw_info_show_pub);
13.  
    nh.advertise(raw_sonar_pub);
14.  
    nh.advertise(raw_relaid_pub);
15.  
    nh.advertise(raw_upgrader_pub);

第五步在循环中进行等待即可完成roslib库的使用，大家可以根据自己的实际情况对节点进行修改增加和删除

1.  
   for(;;){
2.  
   while (!nh.connected()){
3.  
   nh.spinOnce();
4.  
   hStr_t = 1;
5.  
   vTaskDelay(300);
6.  
   }
7.  
   if(hStr_t){
8.  
   watchdogInit(600);
9.  
   initParam(&nh);
10.  
    ShowHardwareStr();
11.  
    vTaskDelay(100);
12.  
    showParamSet(&nh);
13.  
    setBaseClassInit();
14.  
    setBaseClassPid();
15.  
    imuDriveInit();
16.  
    configParam.IsRosNodePub = 1;
17.  
    setMotorDebugMaxRpm();
18.  
    hStr_t = 0;
19.  
    watchdogInit(WATCHDOG_RESET_MS);
20.  
    }
21.  
    if(IsParamShow){
22.  
    IsParamShow = 0;
23.  
    ShowHardwareStr();
24.  
    showParamSet(&nh);
25.  
    }
26.  
    nh.spinOnce();
27.  
    vTaskDelay(100);
28.  
    }

3、如何编写一个订阅节点，订阅节点第一步是需要编写一个回调函数，用于roslib库当接收到订阅的话题时进行一个响应；第二步需要编写订阅的节点名称和消息类型；第三步则是需要对订阅的节点进行注册。

1.  
   void command_callback(const geometry_msgs::Twist& cmd_msg);//函数声明
2.  
   ros::Subscriber<geometry_msgs::Twist> cmd_sub("cmd_vel", command_callback);//订阅的话题名称和消息类型
3.  
   //回调函数的编写
4.  
   void command_callback(const geometry_msgs::Twist& cmd_msg){
5.  
   stcTwist str_p;
6.  
   str_p.linearVelX = cmd_msg.linear.x;
7.  
   str_p.linearVelY = cmd_msg.linear.y;
8.  
   str_p.angularVelZ = cmd_msg.angular.z;
9.  
   if(false ==twistlinkSendPacket( &str_p))
10.  
    nh.loginfo("twistlinkSendPacket error");
11.  
    }
12.  
    nh.subscribe(cmd_sub);//回调函数的注册

4、如何编写一个发布节点，发布节点第一步需要创建发布的消息类型变量；第二步需要编写发布的节点名称和消息类型；第三步是编写一个函数对发布节点的消息类型数据进行处理和发布；第四步是需要对发布的节点进行注册。

1.  
   starrobot_msgs::Imu raw_imu_msg; //创建消息类型变量
2.  
   ros::Publisher raw_imu_pub("raw_imu", &raw_imu_msg); //发布的节点的名称和消息类型
3.  
   //对发布节点的消息类型数据处理和发布
4.  
   void PublishImu(){
5.  
   stcATBuff sBatBuffr_p;
6.  
   if((configParam.IsRosNodePub != 0) && (upStatus==0)){
7.  
   switch(configParam.IMUType){
8.  
   case 1:
9.  
   raw_imu_msg.linear_acceleration = imu_gy85.readAccelerometer();
10.  
    raw_imu_msg.angular_velocity = imu_gy85.readGyroscope();
11.  
    raw_imu_msg.magnetic_field = imu_gy85.readMagnetometer();
12.  
    break;
13.  
    case 2:
14.  
    case 3:
15.  
    raw_imu_msg.linear_acceleration = imu_mpu9250.readAccelerometer();
16.  
    raw_imu_msg.angular_velocity = imu_mpu9250.readGyroscope();
17.  
    raw_imu_msg.magnetic_field = imu_mpu9250.readMagnetometer();
18.  
    break;
19.  
    case 4:
20.  
    case 5:
21.  
    raw_imu_msg.linear_acceleration = imu_mpu6050.readAccelerometer();
22.  
    raw_imu_msg.angular_velocity = imu_mpu6050.readGyroscope();
23.  
    raw_imu_msg.magnetic_field = imu_mpu6050.readMagnetometer();
24.  
    break;
25.  
    case 6:
26.  
    case 7:
27.  
    raw_imu_msg.linear_acceleration = imu_mpu6500.readAccelerometer();
28.  
    raw_imu_msg.angular_velocity = imu_mpu6500.readGyroscope();
29.  
    raw_imu_msg.magnetic_field = imu_mpu6500.readMagnetometer();
30.  
    break;
31.  
    default:break;
32.  
    }
33.  
        raw_imu_pub.publish(&raw_imu_msg);
34.  
    }
35.  
    }

4、如何定义自己的硬件，底层代码是做了接口解耦合的，硬件定义的代码在HwConfig/hw_xxx.c文中，端口定义则在HwConfig/hw_xxx.h中大家可以根据自己的需要对HwConfig/hw_xxx.h文件里的端口进行修改，HwConfig/hw_xxx.c里面的函数名称最好不要修改，否则需要自己同步把调用该函数的地方全部进行修改才不会报错

5、如何编写一个任务，编写任务第一步是需要写一个任务函数；第二步是创建任务；第三步是把创建的任务初始化函数放到初始化函数入口数组中。

1.  
   static void SysLedBeep_HTask(void *pvParameters){         //任务执行函数
2.  
   for( ;; ){
3.  
   STARBOT_LED_RUN_Toggle();
4.  
   vTaskDelay(300);
5.  
   }
6.  
   }
7.  
   void SysLedBeep_TaskInit(void){   
8.  
   LED_Init();
9.  
   // 创建任务
10.  
    xTaskCreate(SysLedBeep_HTask,(const char *)"SysLedBeepHTask",128, NULL,LedBeep_Pri, NULL);
11.  
    }
12.  
    PrivateFun InitTaskArr[]={
13.  
    //函数名称
14.  
    SysLedBeep_TaskInit,//把任务初始化添加到任务初始化函数入口数组中
15.  
    // #ifndef STM32F10X_HD
16.  
    ros_task_create,
17.  
    OledShow_TaskInit,
18.  
    MoveBase_TaskInit,
19.  
    Terminal_TaskInit,
20.  
    usbLink_TaskInit,
21.  
    uart1_TaskInit,
22.  
    hcBle_TaskInit,
23.  
    uart3Link_TaskInit,
24.  
    uart4Link_TaskInit,
25.  
    uart5Link_TaskInit,
26.  
    ps2Manage_TaskInit,
27.  
    timeout_TaskInit,
28.  
    canLink_TaskInit,
29.  
    ErrorManage_TaskInit,
30.  
    Ws2812Run_TaskInit,
31.  
    // #endif
32.  
    NULL//这个NULL不能删除
33.  
    };

欢迎大家进群一起交流学习：129923584

相关视频讲解链接：https://www.bilibili.com/video/BV1bD4y1g7V5/

博文使用的开发板购买链接：https://item.taobao.com/item.htm?spm=a1z10.1-c-s.w4004-21142386790.2.c0086177uv5IlA&id=606445592295