PX4中通过串口读取STM32F4串口发送过来消息并发布UORB主题

PX4中通过串口读取STM32F4串口发送过来消息并发布UORB主题

本次小项目是通过PX4读取STM32F4发过来的数据，之前博客介绍了我做的STM32端项目，再稍微啰嗦一下：解析AIRMAR和测深仪数据，该数据采用NEMA0183协议传输的。PX4接收到数据会通过mavlink消息发布出去，通过433MHz来进行无线传输，然后在岸边读取内河中的气象信息。
本项目会通过几篇博客去介绍，本次主要介绍PX4中通过串口读取STM32F4串口发送过来消息并发布UORB主题，本次帖子主要是参考网络上一些大神方法进行编写，希望对大家有用，欢迎各位朋友及时提出问题。
在Firmware/msg目录下新建read_uart_sensor.msg文件并添加到CMakeLists.txt中，编译后自动生成uORB/topics/read_uart_sensor.h头文件

char[10] jingdustr
char[10] weidustr
char[10] hangsustr
char[10] hangxiangstr
char[10] fengsustr
char[10] fengxiangstr
char[10] wendustr
char[10] shendustr

float32 longitude
float32 latitude
float32 hang_su
float32 hang_xiang
float32 feng_su
float32 feng_xiang
float32 wen_du
float32 shen_du

# TOPICS read_uart_sensor

在Firmware/src/modules目录下新建文件夹read_uart_sensor。添加文件read_uart_sensor.c

PX4接收串口接收代码

/*
 * 串口读取函数
 * read_uart_sensor.c
 */
#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 <nuttx/config.h>
#include <fcntl.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <poll.h>
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/read_uart_sensor.h>
#include <px4_tasks.h>
__EXPORT int read_uart_sensor_main(int argc, char *argv[]);

static int daemon_task;
static bool thread_should_exit = true;
static bool thread_running = false;

int read_uart_thread_main(int argc, char *argv[]);
static void usage(const char *reason);
static int uart_init(char * uart_name);
static int set_uart_baudrate(const int fd, unsigned int baud);
int set_uart_baudrate(const int fd, unsigned int baud)
{
    int speed;
    switch (baud) {
    case 9600:   speed = 9600;   break;
    case 19200:  speed = 19200;  break;
    case 38400:  speed = 38400;  break;
    case 57600:  speed = 57600;  break;
    case 115200: speed = 115200; break;
    default:
        warnx("ERR: baudrate: %d\n", baud);
        return -EINVAL;
    }

    struct termios uart_config;

    int termios_state;

    /* 以新的配置填充结构体 */
    /* 设置某个选项，那么就使用"|="运算，
     * 如果关闭某个选项就使用"&="和"~"运算
     * */
    tcgetattr(fd, &uart_config); // 获取终端参数

    /* clear ONLCR flag (which appends a CR for every LF) */
    uart_config.c_oflag &= ~ONLCR;// 将NL转换成CR(回车)-NL后输出。

    /* 无偶校验，一个停止位 */
    uart_config.c_cflag &= ~(CSTOPB | PARENB);// CSTOPB 使用两个停止位，PARENB 表示偶校验

    /* 设置波特率 */
    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;
}
int uart_init(char * uart_name)
{
    int serial_fd = open(uart_name, O_RDWR | O_NOCTTY);
    if (serial_fd < 0)
    {
        err(1, "failed to open port: %s", uart_name);
        return false;
    }
    //    printf("Open the %s\n",serial_fd);
    return serial_fd;
}
static void usage(const char *reason)
{
    if (reason) {
        fprintf(stderr, "%s\n", reason);
    }

    fprintf(stderr, "usage: read_uart_sensor {start|stop|status} [param]\n\n");
    exit(1);
}
int read_uart_sensor_main(int argc, char *argv[])
{

    if (argc < 2)
    {
        usage("missing command");
    }

    if (!strcmp(argv[1], "start"))
    {
        if (thread_running)
        {
            warnx("already running\n");
            return 0;
        }

        thread_should_exit = false;
        daemon_task = px4_task_spawn_cmd("read_uart_sensor",
                                         SCHED_DEFAULT,
                                         SCHED_PRIORITY_DEFAULT,
                                         //SCHED_PRIORITY_MAX - 5,
                                         2000,
                                         read_uart_thread_main,
                                         (argv) ? (char * const *)&argv[2] : (char * const *)NULL);
        warnx("already running2222\n");
        return 0;
    }
    if (!strcmp(argv[1], "stop"))
    {
        thread_should_exit = true;
        return 0;
    }
    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 read_uart_thread_main(int argc,char *argv[])
{
    //char *uart_name = argv[1];
    char data = '0';
    char jingdu[10] = "";
    char weidu[10]  = "";
    char hangsu[10]  = "";
    char hangxiang[10]  = "";
    char fengsu[10] = "";
    char fengxiang[10]  = "";
    char wendu[10]  = "";
    char shendu[10]  = "";
    int index=0;
    int jingdu_counter=0;
    int weidu_counter=0;
    int hangsu_counter=0;
    int hangxiang_counter=0;
    int fengsu_counter=0;
    int fengxiang_counter=0;
    int wendu_counter=0;
    int shendu_counter=0;
    int end=0;
    int uart_read = uart_init("/dev/ttyS6");
    if(false == uart_read)
        return -1;
    if(false == set_uart_baudrate(uart_read,9600))
    {
        printf("set_uart_baudrate is failed\n");
        return -1;
    }
    printf("uart init is successful\n");
    thread_running = true;
    struct read_uart_sensor_s orbtest;
    memset(&orbtest,0,sizeof(orbtest));
    orb_advert_t pub_fd=orb_advertise(ORB_ID(read_uart_sensor),&orbtest);   //公告，公告必须在发布之前，否则即使订阅也得不到任何数据
    while (!thread_should_exit)
    {
        read(uart_read,&data,1);
        warnx("already running3333\n");
        if(data=='$')
        {
           while(end==0)
           {
              read(uart_read,&data,1);
              if(data ==',')
   	      {
            	index++;
              }
              else
              {
                switch(index)
                {
                  case 1:jingdu[jingdu_counter++]=data;break;
              	  case 2:weidu[weidu_counter++]=data;break;
              	  case 3:hangsu[hangsu_counter++]=data;break;
                  case 4:hangxiang[hangxiang_counter++]=data;break;
                  case 5:fengsu[fengsu_counter++]=data;break;
                  case 6:fengxiang[fengxiang_counter++]=data;break;
                  case 7:wendu[wendu_counter++]=data;break;
                  case 8:shendu[shendu_counter++]=data;break;
                  case 9:end=1;break;
                }
              }
          }
       }
       if(end==1)
       {
          jingdu_counter=0;
	  weidu_counter=0;
          hangsu_counter=0;
	  hangxiang_counter=0;
	  fengsu_counter=0;
          fengxiang_counter=0;
          wendu_counter=0;
          shendu_counter=0;
          index=0;
          end=0;
          //printf("%s\n",jingdu);
          //printf("%s\n",weidu);
          //printf("%s\n",hangsu);
          //printf("%s\n",hangxiang);
          //printf("%s\n",fengsu);
          //printf("%s\n",fengxiang);
          //printf("%s\n",wendu);
          //printf("%s\n",shendu);
          warnx("already running4444\n");
          strncpy(orbtest.jingdustr,jingdu,10);// 复制字符串
          strncpy(orbtest.weidustr,weidu,10);
          strncpy(orbtest.hangsustr,hangsu,10);
          strncpy(orbtest.hangxiangstr,hangxiang,10);
          strncpy(orbtest.fengsustr,fengsu,10);
          strncpy(orbtest.fengxiangstr,fengxiang,10);
          strncpy(orbtest.wendustr,wendu,10);
          strncpy(orbtest.shendustr,shendu,10);
          orbtest.longitude= atof(orbtest.jingdustr);
          orbtest.latitude = atof(orbtest.weidustr);
          orbtest.hang_su  = atof(orbtest.hangsustr);
	  orbtest.hang_xiang= atof(orbtest.hangxiangstr);
	  orbtest.feng_su= atof(orbtest.fengsustr);
	  orbtest.feng_xiang= atof(orbtest.fengxiangstr);
	  orbtest.wen_du= atof(orbtest.wendustr);
	  orbtest.shen_du= atof(orbtest.shendustr);    
          orb_publish(ORB_ID(read_uart_sensor),pub_fd,&orbtest); 
          /*warnx("DATA:longitude=%f,latitude=%f,hang_su=%f,hang_xiang=%f",
                     (double) orbtest.longitude,
                     (double) orbtest.latitude,
                     (double) orbtest.hang_su,
                     (double) orbtest.hang_xiang);*/
          printf("success!\n");
          int sensor_sub_fd = orb_subscribe(ORB_ID(read_uart_sensor));//
          orb_set_interval(sensor_sub_fd, 1000);
          struct read_uart_sensor_s data_copy;
          orb_copy(ORB_ID(read_uart_sensor),sensor_sub_fd,&data_copy);
                    //PX4_INFO("DATA:%f\t%f",
                       //      (double)data_copy.lat,
                         //    (double)data_copy.lng);
         warnx("DATA:longitude=%f,latitude=%f,hang_su=%f,hang_xiang=%f",
                     (double) data_copy.longitude,
                     (double) data_copy.latitude,
                     (double) data_copy.hang_su,
                     (double) data_copy.hang_xiang);
      }
    }
    warnx("exiting");
    thread_running = false;
    close(uart_read);
    fflush(stdout);
    return 0;
}

添加CMakeLists.txt文件，并编写里面内容。

set(MODULE_CFLAGS)
px4_add_module(
        MODULE modules_read_uart_sensor
        MAIN read_uart_sensor
    COMPILE_FLAGS
        -Os
    SRCS
                read_uart_sensor.c
    DEPENDS
        platforms__common
    )
# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

对应位置如下：

在Firmware/cmake/configs/nuttx/nuttx_px4fmu-v2_default.cmake中注册该模块

以上就是对PX4中通过串口读取STM32F4串口发送过来消息并发布UORB主题的代码编写和环境配置，接下来最关心的事情便是查看结果正确性，具体操作如下：
显示在nsh终端上面

1、输入read_uart_sensor start 就会运行对应程序并运行打印程序打印在nsh终端上。
2、输入read_uart_sensor stop就会停止对程序运行

如上方法，如果可以看到如上图所示效果，则说明成功了，恭喜你可以开始后面自定义mavlink消息！