PX4学习笔记—通过串口收发数据

ariesys

一、参考博客网址

FantasyJXF—Pixhawk原生固件PX4之串口读取信息：https://blog.csdn.net/oqqENvY12/article/details/57131022

FantasyJXF—Pixhawk原生固件PX4之串口添加读取传感器实现：https://blog.csdn.net/oqqENvY12/article/details/59140135

FreeApe—Pixhawk---通过串口方式添加一个自定义传感器（超声波为例）：https://blog.csdn.net/freeape/article/details/47837415

潇齐—PIXHAWK源码分析之三---通过串口方式添加一个自定义传感器(1)：https://blog.csdn.net/msq19895070/article/details/52012190

      该文档前半部分复现了以上网址的内容：添加串口读取信息功能，这部分功能实现后就可以将自己开发或者px4暂不支持的传感器嵌入到px4的代码中；考虑有些传感器是触发式的需要不断向传感器发送命令数据才可以返回测试数据，文档后半部分添加了通过串口发送自定义数据功能，这样可以实现通过pixhawk的串口收发自定义数据的功能。

二、串口的选择

      在Firmware->ROMFS->px4fmu_common->init.d->RCS脚本文件中部分定义了Nuttx端口与STM32串口的对应关系。

1. #UART mapping on FMUv1/2/3/4:
   
2. # UART1            /dev/ttyS0                  IO debug
   
3. # USART2                   /dev/ttyS1                  TELEM1 (flow control)
   
4. # USART3                   /dev/ttyS2                  TELEM2 (flow control)
   
5. # UART4
   
6. # UART7                                                 CONSOLE
   
7. # UART8                                                 SERIAL4

复制代码

      通过全局搜索/dev/ttyS/确定对应关系如下：

Nuttx  端口	Pixhawk  UART
/dev/ttyS0	IO  Debug(RX Only)
/dev/ttyS1	TELE1(USART2)
/dev/ttyS2	TELE2(USART3)
/dev/ttyS3	GPS(UART4)
/dev/ttyS4	PX4IO(UART5)
/dev/ttyS5	SERIAL5(UART7,NSH)
/dev/ttyS6	SERIAL4(UART8)

      开始选择/dev/ttyS2对应的TELE2实现串口二次开发功能。经过测试TELE2的RX引脚可以正常收数据，而TX引脚不能正常发送数据，可能被其他地方调用，改用SERIAL4串口/ttyS6。
      连接硬件如图所示，连接飞控的SERIAL4端口与模块的TTL端口，SERIAL4端口从左至右的接口依次为:+5V(红色)、TX#4(绿色)、RX#4(黄色)、TX#5(棕色)、RX#5(灰色)、GND(黑色)。

三、添加线程
1、添加.c文件                  在Firmware->Src->Modules文件夹下，新建一个文件夹(功能)，命名为serv_sys_uart，在新建的文件夹下，新建一个serv_sys_uart.c文件，复制下面链接的代码至该文件中，
https://blog.csdn.net/oqqENvY12/article/details/57131022，代码的具体分析见代码注释，将代码中的rw_uart改为serv_sys_uart。

2、添加CMakeList.txt文件
      在新建的serv_sys_uart文件夹下，建立CMakeLists.txt文件，内容如下：

1. set(MODULE_CFLAGS)
   
2. px4_add_module(
   
3.     MODULE modules__serv_sys_uart //文件夹名
   
4.     MAIN serv_sys_uart            //文件夹名
   
5.     COMPILE_FLAGS               //编译选项
   
6.         -Os
   
7.     SRCS   
   
8. 
   
9.         serv_sys_uart.c            //c文件名
   
10.     DEPENDS
    
11.         platforms__common
    
12.     )
    
13. # vim: set noet ft=cmake fenc=utf-8 ff=unix :

复制代码

3、添加编译选项
      在Firmware->cmake->configs->nuttx_px4fmu-v2_default.cmake文件中，添加新建功能的编译选项。

4、编译下载固件
    makepx4fmu-v2_default upload

四、测试·

1、测试读取数据
      连接好硬件设备，打开串口助手连接USB转TTL模块，用QGC地面站连接Pixhawk打开QGC->nsh终端，输入help，可以看到系统内已经有serv_sys_uart线程；输入serv_sys_uart命令可以开启该功能，在上节的函数中实现的功能是，将串口接收到的数据(R0011、R0023形式)打印到终端，通过串口助手给Pixhawk发送数据，发送形式为阿斯克码，可以看到serv_sys_uart模块成功将接收到的数据R0001，打印到了终端。

2、测试发送数据
      在调试串口发送过程中遇到了很多坑，下面来一一阐述。

1）坑1—串口配置问题
      首先直接在循环里，利用write()函数通过SERIAL4串口发送数据，形式与read()函数相同。

1. send_data[0]=0x08;
   
2. send_data[1]=0x09;
   
3. send_data[2]=0x0A;
   
4. send_data[3]=0x0B;
   
5. send_data[4]=0x0C;
   
6. write(serv_uart,&send_data,5);//发送数据

复制代码

      结果如下图所示。

      发现存在问题，线程开启后，只发出一条数据，而后只有read后才会write数据。根据与frsky_telemetry.c文件对比，它在uart_init()函数中添加配置串口选项O_NONBLOCK。

1. int serial_fd = open(uart_name, O_RDWR | O_NOCTTY| O_NONBLOCK);

复制代码

      该选项意思是去掉阻塞等待，如不添加该选项，在进入线程主函数serv_sys_uart_thread_main()函数的while循环里，如果没有接收到数据，则read(serv_uart,&data,1);会一直处于读的状态，占用了串口所以发送不出去。添加完该选项之后，启动线程，会一直通过串口发送数据。

2）坑2—线程优先级问题

      测试后发现，串口每次只会发送第一个字节，如上图，并且影响到了串口的接收，接收数据会发生错误，考虑是直接在循环里面添加的write函数，发送的频率太快。所以将数据改成用for循环一次发送一个字节。并且将整个线程通过读取系统时间设置成了10Hz。（事后证明这个方法很占用CPU时间）

      测试过程中还发现了死机的情况，发现是将新的线程的优先级设置的太高了造成的，优先级设置成了SCHED_PRIORITY_MAX – 5，与看门狗是一个级别，系统会优先调用该线程，该线程还一致占用着串口的中断，会导致系统死机。采取的策略是降低新建线程的优先级，根据Firmware->src->modules->platform->px4_tasks.h文件中定义了其他进程的优先级：

1. #pragma once
   
2. #include <px4_tasks.h>
   
3. /*      SCHED_PRIORITY_MAX    */
   
4. 
   
5. #define SCHED_PRIORITY_FAST_DRIVER           SCHED_PRIORITY_MAX
   
6. #define SCHED_PRIORITY_WATCHDOG             (SCHED_PRIORITY_MAX - 5)
   
7. #define SCHED_PRIORITY_ACTUATOR_OUTPUTS     (SCHED_PRIORITY_MAX - 15)
   
8. #define SCHED_PRIORITY_ATTITUDE_CONTROL     (SCHED_PRIORITY_MAX - 25)
   
9. #define SCHED_PRIORITY_SLOW_DRIVER          (SCHED_PRIORITY_MAX - 35)
   
10. #define SCHED_PRIORITY_POSITION_CONTROL     (SCHED_PRIORITY_MAX - 40)
    
11. /*      SCHED_PRIORITY_DEFAULT    */
    
12. 
    
13. #define SCHED_PRIORITY_LOGGING              (SCHED_PRIORITY_DEFAULT - 10)
    
14. #define SCHED_PRIORITY_PARAMS               (SCHED_PRIORITY_DEFAULT - 15)
    
15. /*      SCHED_PRIORITY_IDLE    */

复制代码

      例如，GPS是SCHED_PRIORITY_SLOW_DRIVER  (SCHED_PRIORITY_MAX - 35)，位置控制是SCHED_PRIORITY_POSITION_CONTROL     (SCHED_PRIORITY_MAX - 40)，所以将新建的线程的优先级设置为最小，暂定为SCHED_PRIORITY_DEFAULT。再次测试串口发送正常了。

3）坑3—Pixhawk亮紫色灯，快闪
      将新建线程添加到系统自启动文件中后(见下一节)，Pixhawk上电一开始是正常的蓝色灯慢闪，而后转成紫色灯快闪，懵逼。。。
查找Firmware->msg->led_control.msg，里面定义了LED等是有紫色的，

      然后全局搜索COLOR_PURPLE，发现在Firmware->src->modules->commander->commander.cpp文件中，有如下代码：

1. bool overload = (cpuload_local->load > 0.80f) || (cpuload_local->ram_usage > 0.98f);
   
2. if (overload_start == 0 && overload) {
   
3.                             overload_start = hrt_absolute_time();
   
4.                   } else if (!overload) {
   
5.                             overload_start = 0;
   
6.                   }
   
7. if (overload && ((hrt_absolute_time() - overload_start) > overload_warn_delay)) {
   
8. 
   
9.                             led_mode = led_control_s::MODE_BLINK_FAST;
   
10. 
    
11.                             led_color = led_control_s::COLOR_PURPLE;}

复制代码

      根据overload的定义发现在cpu的利用率超过0.8的时候，会将led灯设置成紫色快闪。
      打开QGC终端，输入top命令，尼玛占用了50%的cpu！！！

      而后观察数据收发量庞大的mavlink线程才不到5%，自己写的程序肯定是有问题的，整个c文件的代码基本上是按照px4中常用的函数写的，一般不会有什么问题，只有自己之前添加的10Hz判断是其他线程没有采用过的，抱着试试的心理将10Hz判断的代码去掉，新建线程的cpu占用率降为1.24%，pixhawk上电之后不再闪紫色的灯，不过1.24%依然比较高，后续会不断优化代码。

3、测试添加/发布话题
      直接参考博文：
FantasyJXF—Pixhawk原生固件PX4之添加uORB主题：https://blog.csdn.net/oqqenvy12/article/details/54577063

FreeApe—PX4/Pixhawk---uORB深入理解和应用：https://blog.csdn.net/freeape/article/details/46880637

4、测试自启动
      在Firmware->ROMFS->px4fmu_common->init.d->RCS脚本文件中添加新建功能模块的自启动项。打开QGC终端，输入serv_sys_uart status，终端返回该进程已经启动。

hugoYJ

好贴，学习了

li7301298

继续学习，GOOD