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

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|PX4 QGC透明串口转发三--自定义uORB消息实现，实现PX4模块间数据传递

文章图片
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(); returnnullptr; }//线程接收订阅raw_serial_rx消息 voidMavlinkRawSerialRTX::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 【PX4|PX4 QGC透明串口转发三--自定义uORB消息实现，实现PX4模块间数据传递】#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 250namespace 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_RATE10000000///< max data rate in bytes/s #define MAIN_LOOP_DELAY10000///< 100 Hz @ 1000 bytes/s data rateusing 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( ### Descriptiontransport 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(); returnnullptr; }//线程接收订阅raw_serial_tx消息 voidRawSerialRTX::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守护进程上收到的串口发送消息。