PX4 QGC透明串口转发三--自定义uORB消息实现，实现PX4模块间数据传递

3. 自定义uORB消息实现，实现PX4模块间数据传递

uORB官方说明

设计构想

QGC <--mavlink(数传)--> PX4(mavlink守护进程) <--uORB--> raw_serial_rtx <--UART--> device

上一节实现了 QGC <--mavlink(数传)--> PX4(mavlink守护进程)
本节实现 PX4(mavlink守护进程) <--uORB--> raw_serial_rtx，从而打通整个数据链路
技术点：uORB消息的定义，发布和订阅
未加说明下文中文件路径均以PX4-Framework项目根路径作为起始路径的相对路径。

3.1 自定义uORB消息

依据上节自定义的mavlink消息,参考 msg/qshell_req.msg，在msg目录创建raw_serial_rx.msg和raw_serial_tx.msg。为什么要创建两个，主要考虑，以后还想在其他模块重用，避免混淆
msg/raw_serial_rx.msg,对应串口接收

uint64 timestamp        # time since system start (microseconds)
uint8 dev               # which port received data, define in Mavlink enum RSRTX_OPT_DEV_ENUM
uint8 len               # pakage length
uint8[250] data         # payload
uint8 MAX_LEN = 250     # max length of payload

msg/raw_serial_tx.msg,对应串口发送

uint64 timestamp        # time since system start (microseconds)
uint8 dev               # which port data send to, define in Mavlink enum RSRTX_OPT_DEV_ENUM
uint8 len               # pakage length
uint8[250] data         # payload
uint8 MAX_LEN = 250     # max length of payload

在 msg/CMakeLists.txt 添加消息

#...
set(msg_files
#...
    raw_serial_rx.msg
    raw_serial_tx.msg
    )
#...

编译一遍 make px4_fmu-v2_default
在build/px4_fmu-v2_default/uORB/topics下会出现两个头文件,在消息定义时timestamp必选，字段赋值会自动生成常量

PX4 QGC透明串口转发三--自定义uORB消息实现，实现PX4模块间数据传递_第1张图片

uORBMSG.png

3.2 uORB消息的提出/发布/订阅/读取流程（advertise/public/subscribe/copy）

简单把uORB的流程梳理一下，以便阅读后面实际代码
提出/发布必须在同一线程，如果提出失败，需要再次提出，贸然发布会导致系统宕机重启
订阅/读取必须在同一线程

/*消息发送线程---------------------------------------------------*/
//1.提出消息，一次就够
struct raw_serial_tx_s tx_pkg = {};
orb_advert_t _raw_serial_tx_pub{nullptr};
_raw_serial_tx_pub = orb_advertise(ORB_ID(raw_serial_tx),&tx_pkg);

//2.发布消息，可发布多次，_raw_serial_tx_pub不可为空，否者会出现系统宕机
tx_pkg.timestamp = hrt_absolute_time();
tx_pkg.dev = mavpkg.dev;
tx_pkg.len = mavpkg.len;
memcpy(tx_pkg.data,mavpkg.data,mavpkg.len);
orb_publish(ORB_ID(raw_serial_tx), _raw_serial_tx_pub, &tx_pkg);

/*开新线程接收消息--------------------------------------------------*/
//3.订阅
int raw_serial_rx_sub = orb_subscribe(ORB_ID(raw_serial_rx));

//4.消息读取循环
px4_pollfd_struct_t fds[1];
fds[0].fd = raw_serial_rx_sub;
fds[0].events = POLLIN;
while (true)
    {
        //阻塞等待10ms
        int ret = px4_poll(fds, 1, 10);
        if (ret < 0) {
            break;
        } if(ret==0){
            continue;
        }
        bool updated;
        // 检查消息更新 
        orb_check(raw_serial_rx_sub, &updated);
        if (updated) {
            orb_copy(ORB_ID(raw_serial_rx), raw_serial_rx_sub, &rxpkg);
            ...
        }
    }

3.3 Mavlink端实际代码

src/modules/mavlink/mavlink_raw_serial_rtx.h

#pragma once
#include "mavlink_bridge_header.h"
#include 

#include 
#include 
#include 

class Mavlink;

class MavlinkRawSerialRTX
{
public:
    explicit MavlinkRawSerialRTX(Mavlink *mavlink);
    ~MavlinkRawSerialRTX();
    void handle_message(const mavlink_message_t *msg);
private:
    int sendData(uint8_t dev,uint8_t len,uint8_t * data );
    MavlinkRawSerialRTX(MavlinkRawSerialRTX &);
    MavlinkRawSerialRTX &operator = (const MavlinkRawSerialRTX &);
    Mavlink *_mavlink;

    orb_advert_t _raw_serial_tx_pub{nullptr};
    void pubUorbTx(mavlink_rtx_gcs2uav_t mavpkg);

    void startRXThread(void);
    static void * start_helper(void *instance);
    void rx_check_update_thread(void);

};

src/modules/mavlink/mavlink_raw_serial_rtx.cpp

#include 
#include "mavlink_raw_serial_rtx.h"
#include "mavlink_main.h"

MavlinkRawSerialRTX::MavlinkRawSerialRTX(Mavlink *mavlink) :
    _mavlink(mavlink)
{
    //开始rx的uORB订阅
    startRXThread();
}

MavlinkRawSerialRTX::~MavlinkRawSerialRTX()
{

}

//接收地面站消息
void
MavlinkRawSerialRTX::handle_message(const mavlink_message_t *msg)
{
    //接收消息
    switch (msg->msgid){
        case MAVLINK_MSG_ID_RTX_GCS2UAV:{
            mavlink_rtx_gcs2uav_t mavpkg ;
            mavlink_msg_rtx_gcs2uav_decode(msg,&mavpkg);

            struct raw_serial_tx_s tx_pkg = {};
            tx_pkg.timestamp = hrt_absolute_time();
            tx_pkg.dev = mavpkg.dev;
            tx_pkg.len = mavpkg.len;
            memcpy(tx_pkg.data,mavpkg.data,mavpkg.len);

            if(_raw_serial_tx_pub==nullptr){//如果一开始没有提出advert或者没有成功
                _raw_serial_tx_pub = orb_advertise(ORB_ID(raw_serial_tx),&tx_pkg);
            }
            if(_raw_serial_tx_pub!=nullptr){//发布uORB消息
                orb_publish(ORB_ID(raw_serial_tx), _raw_serial_tx_pub, &tx_pkg);
            }

        }
        default:
            break;
    }
}

//向地面站直接发送消息
int
MavlinkRawSerialRTX::sendData(uint8_t dev,uint8_t len,uint8_t * data ){
    if(len>sizeof(mavlink_rtx_uav2gcs_t::data) || _mavlink==nullptr )
        return -1;

    mavlink_rtx_uav2gcs_t pkg;
    pkg.dev = dev;
    pkg.len = len;
    memcpy(pkg.data,data,len);

    mavlink_msg_rtx_uav2gcs_send_struct(_mavlink->get_channel(),&pkg);

    return 0;
}



//开启一个线程接收raw_serial_rx消息
void MavlinkRawSerialRTX::startRXThread(){
    pthread_t _receive_thread {};
    pthread_attr_t rxsubloop_attr;
    pthread_attr_init(&rxsubloop_attr);

    struct sched_param param;
    (void)pthread_attr_getschedparam(&rxsubloop_attr, ¶m);
    //优先级放到最低吧，以免影响飞控核心功能
    param.sched_priority = SCHED_PRIORITY_MIN;
    (void)pthread_attr_setschedparam(&rxsubloop_attr, ¶m);

    pthread_attr_setstacksize(&rxsubloop_attr, PX4_STACK_ADJUSTED(2840));
    pthread_create(&_receive_thread, &rxsubloop_attr, MavlinkRawSerialRTX::start_helper, (void *)this);

    pthread_attr_destroy(&rxsubloop_attr);
}

//开线程要用到的工具函数,必须是静态的
void * MavlinkRawSerialRTX::start_helper(void *instance){
    ((MavlinkRawSerialRTX *) instance)->rx_check_update_thread();
    return  nullptr;
}

//线程接收订阅raw_serial_rx消息
void  MavlinkRawSerialRTX::rx_check_update_thread(){
    //设置线程名
    char thread_name[30];
    sprintf(thread_name, "mavlink_serial_rx_if%d", _mavlink->get_instance_id());
    px4_prctl(PR_SET_NAME, thread_name, px4_getpid());

    int raw_serial_rx_sub = orb_subscribe(ORB_ID(raw_serial_rx));
    struct raw_serial_rx_s rxpkg;

    px4_pollfd_struct_t fds[1];
    fds[0].fd = raw_serial_rx_sub;
    fds[0].events = POLLIN;
    //和mavlink一起停止
    while (!_mavlink->_task_should_exit)
    {
        //阻塞等待10ms
        int ret = px4_poll(fds, 1, 10);

        if (ret < 0) {
            break;
        } if(ret==0){
            continue;
        }

        bool updated;

        /* 检查消息更新 */
        orb_check(raw_serial_rx_sub, &updated);

        if (updated) {//如果更新了就取数据，并使用mavlink发送

            orb_copy(ORB_ID(raw_serial_rx), raw_serial_rx_sub, &rxpkg);
            //mavlink 发给地面站
            sendData(rxpkg.dev,rxpkg.len,rxpkg.data);
        }
    }

}

3.4 串口端实际代码

代码结构和第一章有所区别，可同时启动多个守护进程，相互间不干扰，通过消息中的dev字段区别到底时那个串口发送/接收。
src/modules/raw_serial_rtx/raw_serial_rtx_main.h

#pragma once

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

#include 
#include 
#include 


//直接从mavlink消息定义处复制过来，想直接使用rsrtx.h,好像还需要其他依赖
#ifndef HAVE_ENUM_RSRTX_OPT_DEV_ENUM
#define HAVE_ENUM_RSRTX_OPT_DEV_ENUM
typedef enum RSRTX_OPT_DEV_ENUM
{
   DEV_TTYS0=0, /* /dev/ttyS0 | */
   DEV_TTYS1=1, /* /dev/ttyS1 | */
   DEV_TTYS2=2, /* /dev/ttyS2 | */
   DEV_TTYS3=3, /* /dev/ttyS3 | */
   DEV_TTYS4=4, /* /dev/ttyS4 | */
   DEV_TTYS5=5, /* /dev/ttyS5 | */
   DEV_TTYS6=6, /* /dev/ttyS6 | */
   RSRTX_OPT_DEV_ENUM_ENUM_END=7, /*  | */
} RSRTX_OPT_DEV_ENUM;
#endif

#define MAX_SERIAL_SIZE 250

namespace raw_serial_rtx{

class RawSerialRTX : public ModuleParams
{

public:
    RawSerialRTX(int baudrate ,int datarate,const char* device_name , int uart_fd);
    ~RawSerialRTX();
    static void usage();
    static int start(int argc,char *argv[]);
    static int getSpeedCodeFormBuadrate(int baud);
    static int openUART(const char * uart_name,int baud,int speed);
    static uint8_t getOptDevEnumByName(const char * uart_name);

private:
    int _baudrate;
    int _datarate;
    const char* _device_name ;
    int _uart_fd;
    uint8_t _opt_dev;
    orb_advert_t _raw_serial_rx_pub{nullptr};

    void readUartLoop();
    void pubUorbRx(uint8_t *buf,int len);
    void startTXThread(void);
    static void * start_helper(void *instance);
    void tx_check_update_thread(void);
};
}

src/modules/raw_serial_rtx/raw_serial_rtx_main.cpp

#include 
#include "raw_serial_rtx_main.h"

#define MAX_DATA_RATE                  10000000        ///< max data rate in bytes/s
#define MAIN_LOOP_DELAY                10000           ///< 100 Hz @ 1000 bytes/s data rate

using namespace raw_serial_rtx;

extern "C" __EXPORT int raw_serial_rtx_main(int argc, char *argv[]);

bool thread_should_exit = true;

/**----------------------------------------------------**/
//C代码
/**----------------------------------------------------**/
 void RawSerialRTX::usage(){

    PRINT_MODULE_DESCRIPTION(
        R"DESCR_STR(
### Description

transport raw serial data via Mavlink

### Examples
start raw_serial_rtx on ttyS6 serial with baudrate 57600 and maximum sending rate of 2500B/s:
$ raw_serial_rtx start -d /dev/ttyS6 -b 57600 -r 2500

)DESCR_STR");

    PRINT_MODULE_USAGE_NAME("raw_serial_rtx", "communication");
    PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start instance");
    PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyS1", "", "Select Serial Device", true);
    PRINT_MODULE_USAGE_PARAM_INT('b', 57600, 9600, 3000000, "Baudrate (can also be p:)", true);
    PRINT_MODULE_USAGE_PARAM_INT('r', 0, 10, 10000000, "Maximum sending data rate in B/s (if 0, use baudrate / 20)", true);
    PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop instances");
    PRINT_MODULE_USAGE_COMMAND_DESCR("status", "Print status for instance");
}
//入口函数
int raw_serial_rtx_main(int argc, char *argv[]){
if (argc < 2) {
        RawSerialRTX::usage();
        return 1;
    }

    if (!strcmp(argv[1], "start")) {

        px4_task_spawn_cmd("raw_serial_rtx_1",
                         SCHED_DEFAULT,
                         SCHED_PRIORITY_MIN,
                         3000,
                         RawSerialRTX::start,
                         (char *const *)argv);

    //RawSerialRTX::start(argc,argv);

    }else if (!strcmp(argv[1], "stop")) {

        thread_should_exit = true;

    }else if (!strcmp(argv[1], "status")) {
        if(thread_should_exit){
            PX4_INFO("task is not running");
        } else {
            PX4_INFO("task is running");
        }

    }else {
        RawSerialRTX::usage();
        return 1;
    }

    return 0;
}

/*C++ 代码*/

/* 打开串口 */
int RawSerialRTX::openUART(const char * uart_name,int baud,int speed){
    int uart_fd = ::open(uart_name, O_RDWR | O_NOCTTY | O_NONBLOCK);

    if(uart_fd < 0){
        PX4_ERR("can't open device : %s ",uart_name);
        return PX4_ERROR;
    }

    struct termios uart_config;
    int termios_state;

    /* Initialize the uart config */
    if ((termios_state = tcgetattr(uart_fd, &uart_config)) < 0) {
        PX4_ERR("ERR GET CONF %s: %d\n", uart_name, termios_state);
        ::close(uart_fd);
        return PX4_ERROR;
    }

    /* Clear ONLCR flag (which appends a CR for every LF) */
    uart_config.c_oflag &= ~ONLCR;

    /* Set baud rate */
    if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
        PX4_ERR("ERR SET BAUD %s: %d\n", uart_name, termios_state);
        ::close(uart_fd);
        return PX4_ERROR;
    }
    /* Set UART conf */
    if ((termios_state = tcsetattr(uart_fd, TCSANOW, &uart_config)) < 0) {
        PX4_WARN("ERR SET CONF %s\n", uart_name);
        ::close(uart_fd);
        return PX4_ERROR;
    }

    return uart_fd;
}

/* process baud rate */
int RawSerialRTX::getSpeedCodeFormBuadrate(int baud){
#ifndef B460800
#define B460800 460800
#endif
#ifndef B500000
#define B500000 500000
#endif
#ifndef B921600
#define B921600 921600
#endif
#ifndef B1000000
#define B1000000 1000000
#endif
    int speed;
    switch (baud) {
    case 0:      speed = B0;      break;
    case 50:     speed = B50;     break;
    case 75:     speed = B75;     break;
    case 110:    speed = B110;    break;
    case 134:    speed = B134;    break;
    case 150:    speed = B150;    break;
    case 200:    speed = B200;    break;
    case 300:    speed = B300;    break;
    case 600:    speed = B600;    break;
    case 1200:   speed = B1200;   break;
    case 1800:   speed = B1800;   break;
    case 2400:   speed = B2400;   break;
    case 4800:   speed = B4800;   break;
    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;
    case 230400: speed = B230400; break;
    case 460800: speed = B460800; break;
    case 500000: speed = B500000; break;
    case 921600: speed = B921600; break;
    case 1000000: speed = B1000000; break;
#ifdef B1500000
    case 1500000: speed = B1500000; break;
#endif
#ifdef B2000000
    case 2000000: speed = B2000000; break;
#endif
#ifdef B3000000
    case 3000000: speed = B3000000; break;
#endif
    default:
        PX4_ERR("Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n500000\n921600\n1000000\n",
            baud);
        return -EINVAL;
    }
    return speed;
}

//通过设备名获取串口的枚举值，没有返回RSRTX_OPT_DEV_ENUM::RSRTX_OPT_DEV_ENUM_ENUM_END
uint8_t RawSerialRTX::getOptDevEnumByName(const char* device_name){
    if(!strcasecmp(device_name,"/dev/ttyS0")){
        return RSRTX_OPT_DEV_ENUM::DEV_TTYS0;
    } else if(!strcasecmp(device_name,"/dev/ttyS1")){
        return RSRTX_OPT_DEV_ENUM::DEV_TTYS1;
    } else if(!strcasecmp(device_name,"/dev/ttyS2")){
        return RSRTX_OPT_DEV_ENUM::DEV_TTYS2;
    } else if(!strcasecmp(device_name,"/dev/ttyS3")){
        return RSRTX_OPT_DEV_ENUM::DEV_TTYS3;
    } else if(!strcasecmp(device_name,"/dev/ttyS4")){
        return RSRTX_OPT_DEV_ENUM::DEV_TTYS4;
    } else if(!strcasecmp(device_name,"/dev/ttyS5")){
        return RSRTX_OPT_DEV_ENUM::DEV_TTYS5;
    } else if(!strcasecmp(device_name,"/dev/ttyS6")){
        return RSRTX_OPT_DEV_ENUM::DEV_TTYS6;
    }
    return RSRTX_OPT_DEV_ENUM::RSRTX_OPT_DEV_ENUM_ENUM_END;
}

/**
 * 任务启动函数
 */
int RawSerialRTX::start(int argc,char *argv[]){
    int baudrate = 57600;
    int datarate = 0;
    const char* device_name = nullptr;

        int uart_fd = -1;

    bool err_flag = false;
    int myoptind = 1;
    const char *myoptarg = nullptr;
    int ch;

    //解析命令
    while ((ch = px4_getopt(argc, argv, "b:r:d:n:u:o:m:t:c:fwxz", &myoptind, &myoptarg)) != EOF) {
        switch (ch) {
        case 'b':
            if (px4_get_parameter_value(myoptarg, baudrate) != 0) {
                PX4_ERR("baudrate parsing failed");
                err_flag = true;
            }

            if (baudrate < 9600 || baudrate > 3000000) {
                PX4_ERR("invalid baud rate '%s'", myoptarg);
                err_flag = true;
            }

            break;

        case 'r':

            if (px4_get_parameter_value(myoptarg, datarate) != 0) {
                PX4_ERR("datarate parsing failed");
                err_flag = true;
            }

            if (datarate > MAX_DATA_RATE) {
                PX4_ERR("invalid data rate '%s'", myoptarg);
                err_flag = true;
            }

            break;

        case 'd':
            device_name = myoptarg;
            break;

        default:
            err_flag = true;
            break;
        }
    }


    if(device_name==nullptr){
        PX4_ERR("serial device name must be setted.");
        err_flag = true;
    }

    uint8_t opt_dev_enum =getOptDevEnumByName(device_name);

    if(opt_dev_enum==RSRTX_OPT_DEV_ENUM::RSRTX_OPT_DEV_ENUM_ENUM_END){
        PX4_ERR("device name is name a serial port");
        err_flag = true;
    }

    int speedcode = RawSerialRTX::getSpeedCodeFormBuadrate(baudrate);
    if(speedcode == -EINVAL){
        err_flag = true;
    }

    if (err_flag ) {
        usage();
        return PX4_ERROR;
    }

    if (datarate == 0) {
        /* convert bits to bytes and use 1/2 of bandwidth by default */
        datarate = baudrate / 20;
    }

    if (datarate > MAX_DATA_RATE) {
        datarate = MAX_DATA_RATE;
    }

    PX4_INFO("data rate: %d B/s on %s @ %dB",datarate, device_name, baudrate);
    fflush(stdout);
    uart_fd = openUART(device_name,baudrate,speedcode);

    if(uart_fd < 0){
        return PX4_ERROR;
    }

    thread_should_exit = false;

    RawSerialRTX  *instance = new RawSerialRTX(baudrate,datarate,device_name,uart_fd);

    //开uORB订阅线程
    instance->startTXThread();
    //循环不会退出，除非thread_should_exit===true;
    instance->readUartLoop();
    //uartEcho(uart_fd,baudrate);
    //readUartLoop(uart_fd,baudrate,opt_dev_enum);
    delete instance;

    ::close(uart_fd);

    return PX4_OK;
}

RawSerialRTX::RawSerialRTX(int baudrate ,int datarate,const char* device_name , int uart_fd):
ModuleParams(nullptr),
_baudrate(baudrate),
_datarate(datarate),
_device_name(device_name),
_uart_fd(uart_fd)
{
    _opt_dev = getOptDevEnumByName(device_name);
}

RawSerialRTX::~RawSerialRTX(){

}

//读取数据循环,发布raw_serial_rx uORB消息
void RawSerialRTX::readUartLoop(){
    if(_uart_fd<0) return;

    //设置线程名
    char thread_name[64];
    sprintf(thread_name, "raw_serial_rtx:%s", _device_name);
    px4_prctl(PR_SET_NAME, thread_name, px4_getpid());

    struct raw_serial_rx_s rx_pkg = {};
    int chmin = 20;
    uint8_t buf[MAX_SERIAL_SIZE];
    int nread = 0;
    int tread = 0;
    const unsigned sleeptime = chmin * 1000000 / (_baudrate / 10);

    while(!thread_should_exit){

        nread = 0;
        tread = 0;
        //使用非阻塞读取  O_NONBLOCK
        //一直读取知道缓存读满或者外部数据发送结束
        while(nread0) nread+=tread;
            else break;
            px4_usleep(sleeptime);
        }
        if(nread>0){

            rx_pkg.timestamp = hrt_absolute_time();
            rx_pkg.dev = _opt_dev;
            rx_pkg.len = nread;
            memcpy(rx_pkg.data,buf,nread);
            if(_raw_serial_rx_pub==nullptr){//如果一开始没有提出advert或者没有成功
                _raw_serial_rx_pub = orb_advertise(ORB_ID(raw_serial_rx),&rx_pkg);
            }
            if(_raw_serial_rx_pub!=nullptr){ //成功advertise之后才能发布，不然系统会宕机
                orb_publish(ORB_ID(raw_serial_rx), _raw_serial_rx_pub, &rx_pkg);
            }
        }

        //数据接受完毕或者没有数据时，以免过度使用占用cpu
        if(tread<1) px4_usleep(sleeptime);
    }


}


//开启一个线程接收raw_serial_rx消息
void RawSerialRTX::startTXThread(){
    pthread_t _receive_thread {};
    pthread_attr_t txsubloop_attr;
    pthread_attr_init(&txsubloop_attr);

    struct sched_param param;
    (void)pthread_attr_getschedparam(&txsubloop_attr, ¶m);
    param.sched_priority = SCHED_PRIORITY_MIN;
    (void)pthread_attr_setschedparam(&txsubloop_attr, ¶m);

    pthread_attr_setstacksize(&txsubloop_attr, PX4_STACK_ADJUSTED(2840));
    pthread_create(&_receive_thread, &txsubloop_attr, RawSerialRTX::start_helper, (void *)this);

    pthread_attr_destroy(&txsubloop_attr);
}

//开线程要用到的工具函数,必须是静态的
void * RawSerialRTX::start_helper(void *instance){
    ((RawSerialRTX *) instance)->tx_check_update_thread();
    return  nullptr;
}

//线程接收订阅raw_serial_tx消息
void  RawSerialRTX::tx_check_update_thread(){
    //设置线程名
    char thread_name[64];
    sprintf(thread_name, "raw_serial_tx:%s", _device_name);
    px4_prctl(PR_SET_NAME, thread_name, px4_getpid());

    int raw_serial_tx_sub = orb_subscribe(ORB_ID(raw_serial_tx));

    struct raw_serial_rx_s txpkg;

    px4_pollfd_struct_t fds[1];
    fds[0].fd = raw_serial_tx_sub;
    fds[0].events = POLLIN;

    while (!thread_should_exit)
    {
        //阻塞等待10ms
        int ret = px4_poll(fds, 1, 10);
        if (ret < 0) {
            break;
        } if(ret==0){
            continue;
        }

        bool updated;
        // 检查消息更新
        orb_check(raw_serial_tx_sub, &updated);
        if (updated) {//如果更新了就取数据，并使用mavlink发送

            orb_copy(ORB_ID(raw_serial_tx), raw_serial_tx_sub, &txpkg);
            if(txpkg.dev==_opt_dev){
            ::write(_uart_fd, txpkg.data, txpkg.len);
            }
        }
    }
}

3.5 简单测试

涉及到mavlink，在调试过程中最好使用Serial 5连USB转UART，操作NSH，如果mavlink起不来，QGC里也就看不见了，PX4调试说明
NSH top命令查看活动进程,用绿线标出，我们产生的进程。两个mavlink_serial_rx_ifx,接收串口守护进程发送的rx消息，分别在USB和TELEM1上的两个mavlink守护进程上产生；raw_serial_rtx:/dev/ttyS6,ttyS6上的串口守护进程，产生raw_serial_tx:/dev/ttyS6，接收mavlink守护进程上收到的串口发送消息。