PX4学习笔记—通过串口发送自定义数据

最近因为项目需要实现一个通过pixhawk串口收发自定义数据的功能。搜索发现，博客上大神们FantasyJXF、FreeApe的博文已经详细介绍了通过pixhawk串口读取自定义数据，这部分功能实现后就可以将自己开发或者px4暂不支持的传感器嵌入到px4的代码中；考虑有些传感器是触发式的需要不断向传感器发送命令数据才可以返回测试数据，这篇博文添加了通过串口发送自定义数据功能，主要介绍一下在调试过程中遇到的坑。。。

首先参考大神FantasyJXF的博文，复现了telem2串口的数据读取功能，过程很顺利。在测试串口发送代码时就相当不顺利了—_—~，

下面介绍工具，开始选择/dev/ttyS2对应的TELE2实现串口二次开发功能。经过测试TELE2的RX引脚可以正常收数据，而TX引脚不能正常发送数据，可能被其他地方调用，改用SERIAL4串口/ttyS6。

           连接硬件如图所示，连接飞控的SERIAL4端口与模块的TTL端口，SERIAL4端口从左至右的接口依次为:+5V(红色)、TX#4(绿色)、RX#4(黄色)、TX#5(棕色)、RX#5(灰色)、GND(黑色)。

下面来一一阐述遇到的坑：

1、坑1—串口配置问题

首先直接在循环里，利用write()函数通过SERIAL4串口发送数据，形式与read()函数相同。

           send_data[0]=0x08;

           send_data[1]=0x09;

           send_data[2]=0x0A;

           send_data[3]=0x0B;

           send_data[4]=0x0C;

           write(serv_uart,&send_data,5);//发送数据

           结果如下图所示。

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

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文件中定义了其他进程的优先级：

#pragma once

#include <px4_tasks.h>

/*      SCHED_PRIORITY_MAX    */

#define SCHED_PRIORITY_FAST_DRIVER           SCHED_PRIORITY_MAX

#define SCHED_PRIORITY_WATCHDOG             (SCHED_PRIORITY_MAX - 5)

#define SCHED_PRIORITY_ACTUATOR_OUTPUTS     (SCHED_PRIORITY_MAX - 15)

#define SCHED_PRIORITY_ATTITUDE_CONTROL     (SCHED_PRIORITY_MAX - 25)

#define SCHED_PRIORITY_SLOW_DRIVER          (SCHED_PRIORITY_MAX - 35)

#define SCHED_PRIORITY_POSITION_CONTROL     (SCHED_PRIORITY_MAX - 40)

/*      SCHED_PRIORITY_DEFAULT    */

#define SCHED_PRIORITY_LOGGING              (SCHED_PRIORITY_DEFAULT - 10)

#define SCHED_PRIORITY_PARAMS               (SCHED_PRIORITY_DEFAULT - 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文件中，有如下代码：

bool overload = (cpuload_local->load > 0.80f) || (cpuload_local->ram_usage > 0.98f);

if (overload_start == 0 && overload) {

                 overload_start = hrt_absolute_time();

          } else if (!overload) {

                  overload_start = 0;

          }

         if (overload && ((hrt_absolute_time() - overload_start) > overload_warn_delay)) {

                            led_mode = led_control_s::MODE_BLINK_FAST;

                            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%依然比较高，后续会不断优化代码。

然后将新建线程添加到自主启动脚本文件中去。

最后附上代码。

//头文件包含
#include <stdio.h>
#include <termios.h>
#include <unistd.h>
#include <stdbool.h>
#include <errno.h>
#include <drivers/drv_hrt.h>
#include <string.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <fcntl.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <uORB/topics/serv_sys_uart.h>
#include <uORB/topics/input_rc.h>
#include <px4_tasks.h>
#include <poll.h>
#include <px4_posix.h>
//外部声明serv_sys_uart_main主函数
__EXPORT int serv_sys_uart_main(int argc, char *argv[]);


static bool thread_should_exit = false; /*Ddemon exit flag*///定义查看进程存在标志变量
static bool thread_running = false;  /*Daemon status flag*///定义查看进程运行标志变量
static int daemon_task;//定义进程变量

//定义线程主函数
int serv_sys_uart_thread_main(int argc, char *argv[]);
//静态声明函数
static int uart_init(const char * uart_name);//串口初始化函数，形参为串口路径
static int set_uart_baudrate(const int fd, unsigned int baud);//设置串口波特率函数
static void usage(const char *reason);//进程提示函数
//设置串口波特率函数
int set_uart_baudrate(const int fd, unsigned int baud)//配置不全，仿照GPS配置尝试
{
    int speed;
    //选择波特率
    switch (baud) {
        case 9600:   speed = B9600;   break;
        case 19200:  speed = B19200;  break;
        case 38400:  speed = B38400;  break;
        case 57600:  speed = B57600;  break;
        case 115200: speed = B115200; break;
        default:
            warnx("ERR: baudrate: %d\n", baud);
            return -EINVAL;
    }
    //实例化termios结构体，命名为uart_config
    struct termios uart_config;
 
    int termios_state;
    /* fill the struct for the new configuration */
    tcgetattr(fd, &uart_config);// 获取终端参数
    /* clear ONLCR flag (which appends a CR for every LF) */
    uart_config.c_oflag &= ~ONLCR;// 将NL转换成CR(回车)-NL后输出。
    /* no parity, one stop bit */
    uart_config.c_cflag &= ~(CSTOPB | PARENB);
    /* set baud rate */
    if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
        warnx("ERR: %d (cfsetispeed)\n", termios_state);
        return false;
    }

    if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
        warnx("ERR: %d (cfsetospeed)\n", termios_state);
        return false;
    }
    // 设置与终端相关的参数，TCSANOW 立即改变参数
    if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {
        warnx("ERR: %d (tcsetattr)\n", termios_state);
        return false;
    }

    return true;
}

//串口初始化函数，传入形参为"/dev/ttyS2"
int uart_init(const char * uart_name)
{
    int serial_fd = open(uart_name, O_RDWR | O_NOCTTY| O_NONBLOCK);//调用Nuttx系统的open函数，形参为串口文件配置模式，可读写，
    /*Linux中，万物皆文件，打开串口设备和打开普通文件一样，使用的是open（）系统调用*/
    // 选项 O_NOCTTY 表示不能把本串口当成控制终端，否则用户的键盘输入信息将影响程序的执行
    if (serial_fd < 0) {
        err(1, "failed to open port: %s", uart_name);
        return false;
    }
    return serial_fd;
}
//进程提示函数，用来提示可输入的操作
static void usage(const char *reason)
{
    if (reason) {
        fprintf(stderr, "%s\n", reason);
    }

    fprintf(stderr, "usage: serv_sys_uart {start|stop|status} [param]\n\n");
    exit(1);
}
//主函数入口
int serv_sys_uart_main(int argc, char *argv[])
{
    if (argc < 2) {
        usage("missing command");
    }
    //输入为start
    if (!strcmp(argv[1], "start")) {
        if (thread_running) {//进程在运行
            warnx("already running\n");//打印提示已经在运行
            return 0;//跳出代码
        }
        //如果是第一次运行
        thread_should_exit = false;
        //建立名为serv_sys_uart进程SCHED_PRIORITY_MAX - 55,
        daemon_task = px4_task_spawn_cmd("serv_sys_uart",
                         SCHED_DEFAULT,
                         SCHED_PRIORITY_DEFAULT,
                         1000,
                         serv_sys_uart_thread_main,
                         (argv) ? (char * const *)&argv[2] : (char * const *)NULL);//正常命令形式为serv_sys_uart start /dev/ttyS2
        return 0;//跳出代码
    }
    //如果是stop
    if (!strcmp(argv[1], "stop")) {
        thread_should_exit = true;//进程标志变量置true
        return 0;
    }
    //如果是status
    if (!strcmp(argv[1], "status")) {
        if (thread_running) {
            warnx("running");
            return 0;

        } else {
            warnx("stopped");
            return 1;
        }

        return 0;
    }
    //若果是其他，则打印不支持该类型
    usage("unrecognized command");
    return 1;
}

int serv_sys_uart_thread_main(int argc, char *argv[])
{
    //正常命令形式为serv_sys_uart start /dev/ttyS2 
    if (argc < 2) {
        errx(1, "need a serial port name as argument");
        usage("eg:");
    }
    /*将/dev/ttyS2赋值给argv[1],进而赋值给uart_name*/
    const char *uart_name = argv[1];
    warnx("opening port %s", uart_name);
    
    /*
     * TELEM1 : /dev/ttyS1
     * TELEM2 : /dev/ttyS2
     * GPS    : /dev/ttyS3
     * NSH    : /dev/ttyS5
     * SERIAL4: /dev/ttyS6
     * N/A    : /dev/ttyS4
     * IO DEBUG (RX only):/dev/ttyS0
     */

    /*配置串口*/
    int serv_uart = uart_init(uart_name);//初始化串口路径-
    if(false == serv_uart)return -1;
    if(false == set_uart_baudrate(serv_uart,57600)){//设置串口波特率为57600
        printf("set_uart_baudrate is failed\n");
        return -1;
    }
    printf("uart init is successful\n");

    /*进程标志变量*/
    thread_running = true;//进程正在运行

    /*初始化数据结构体 */
    struct serv_sys_uart_s servdata;//实例化serv_sys_uart变量，该变量是自己定义的变量
    memset(&servdata, 0, sizeof(servdata));//初始化servdata变量

    /* 公告主题 */
    orb_advert_t serv_uart_pub = orb_advertise(ORB_ID(serv_sys_uart), &servdata);

    /*定义接收话题变量*/
    uint8_t state;
    uint8_t data;
    uint8_t buffer[8];//串口接收缓冲数组
    int already_send1=0;
    int already_send2=0;
    int already_send3=0;
    bool updated = false;
    int rc_sub_fd = orb_subscribe(ORB_ID(input_rc));//订阅input_rc消息
    struct input_rc_s rc_botton;                    //实例化遥控器通道变量
    memset(&rc_botton, 0, sizeof(rc_botton));       //初始化遥控器通道变量

    /*更新计时器变量*/
    //static uint64_t last_time = 0;
    //static uint64_t cur_time = 0;
    //last_time=hrt_absolute_time();//先更新一下时间戳

    /*定义串口事件阻塞结构体及变量*/
    px4_pollfd_struct_t fds[] = {
    	{ .fd = serv_uart,   .events = POLLIN },
    };
    int error_counter = 0;

    while(!thread_should_exit){

    	//cur_time=hrt_absolute_time();//更新事件,事实证明这么做不可以，占用了CPU50%的利用率
    	//if((cur_time-last_time)<100000) continue;//时间少于100000微秒，即0.1s
    	orb_check(rc_sub_fd, &updated);

    	/*先订阅遥控器的值，加入遥控器拨杆变化判断*/
    	if (updated) {
    		orb_copy(ORB_ID(input_rc), rc_sub_fd, &rc_botton);
    		/*
    		PX4_INFO("rc_botton=:\t%d\t%d\t%d\n",
    		    				rc_botton.values[2],
    							rc_botton.values[6],
    							rc_botton.values[7]);
    		*/
    		/*根据遥控器的值发送协议*/
    		//rc_botton.values[7]控制运服对接，下位-1094-nothing、上位-1934-输入运服对接0x10
    		//rc_botton.values[8]控制放置回收，下位-1094-放置命令0x11、中位-1514-nothing、上位-1934-回收命令0x21
    		uint8_t send_data[8];//定义数据发送数组
    		send_data[0]=0x01;
    		send_data[1]=0x00;
    		send_data[2]=0x01;
    		send_data[3]=0x00;
    		send_data[4]=0x00;
    		send_data[5]=0x00;
    		send_data[6]=0x00;
    		if(rc_botton.values[7]<1500)//放置命令，同时启动服务系统
    		{
    		    send_data[7]=0x12;//启动服务系统
    		    send_data[7]=0x11;//开始放置
    		    if(already_send1<5)
    		    {
    		    	for(int i=0;i<8;i++)
    		    	{
    		    		write(serv_uart,&send_data[i],1);//一个字节一个字节发送
    		    	}
    		    	already_send1++;//发送五次
    		    }
    		}
    		else if(rc_botton.values[7]<1700)
    		{
    		    //do nothing
    		    already_send1=0;
    		    already_send2=0;
    		}
    		else
    		{
    		    send_data[7]=0x21;//开始回收
    		    if(already_send2<5)
    		    {
    		    	for(int i=0;i<8;i++)
    		    	{
    		    		write(serv_uart,&send_data[i],1);//一个字节一个字节发送
    		    	}
    		    	already_send2++;
    		    }
    		}

    		if(rc_botton.values[6]>1500)//运服对接
    		{
    		    send_data[7]=0x10;//运服对接
    		    if(already_send3<5)
    		    {
    		    	for(int i=0;i<8;i++)
    		    	{
    		    		 write(serv_uart,&send_data[i],1);//一个字节一个字节发送
    		    	}
    		    	already_send3++;
    		    }

    		 }
    		 else
    		 {
    		    already_send3=0;
    		 }
    	}

    	int poll_ret = poll(fds,1,10);//阻塞等待10ms
    	if (poll_ret == 0)
    	{
    		/* this means none of our providers is giving us data */
    	    printf("No receive data for 10ms\n");
    	} else if (poll_ret < 0)
    	{
    	   /* this is seriously bad - should be an emergency */
    	   if (error_counter < 10 || error_counter % 50 == 0)
    	   {
    		   /* use a counter to prevent flooding (and slowing us down) */
    		   printf("ERROR return value from poll(): %d\n", poll_ret);
    	   }
    	       error_counter++;
    	}
    	else
    	{
    	   if (fds[0].revents & POLLIN)
    	   {
    		   /*接收服务系统发过来的消息*/
    		   read(serv_uart,&data,1);//读取串口数据
    		   if(data == 0x01)
    		   {//找到帧头0x01
    			   buffer[0] = 0x01;
    			   for(int i = 1;i <8;i++)
    			   {
    				   read(serv_uart,&data,1);//读取后面的数据
    				   buffer[i] = data;
    				   //printf("buffer=%d\n",buffer[i]);//将消息打印出来
    		       }
    		   }
        	   for(int j=0;j<8;j++)
        	   {
        		   servdata.datastr[j]=buffer[j];
        	   }
        	   state = buffer[7];
        	   servdata.data=state;
        	   //strncpy(servdata.datastr,buffer,8);// 复制字符串Buffer中前４个数字到Datastr中
        	   //servdata.data = atoi(servdata.datastr);//将字符串转换成整形数据
        	   printf("servdata.data=%d\n",servdata.data);//将消息打印出来
        	   orb_publish(ORB_ID(serv_sys_uart), serv_uart_pub, &servdata);//发布话题
    	   }
    	}
    	//last_time = hrt_absolute_time();
    }
    //如果标志位置flase应该退出进程
    warnx("exiting");
    thread_running = false;
    close(serv_uart);

    fflush(stdout);
    return 0;
}