3. 自定義uORB消息實(shí)現(xiàn)，實(shí)現(xiàn)PX4模塊間數(shù)據(jù)傳遞
設(shè)計(jì)構(gòu)想
QGC <--mavlink(數(shù)傳)--> PX4(mavlink守護(hù)進(jìn)程) <--uORB--> raw_serial_rtx <--UART--> device
- 上一節(jié)實(shí)現(xiàn)了 QGC <--mavlink(數(shù)傳)--> PX4(mavlink守護(hù)進(jìn)程)
- 本節(jié)實(shí)現(xiàn) PX4(mavlink守護(hù)進(jìn)程) <--uORB--> raw_serial_rtx飞傀，從而打通整個數(shù)據(jù)鏈路
- 技術(shù)點(diǎn)：uORB消息的定義朴摊，發(fā)布和訂閱
- 未加說明下文中文件路徑均以PX4-Framework項(xiàng)目根路徑作為起始路徑的相對路徑。
3.1 自定義uORB消息
- 依據(jù)上節(jié)自定義的mavlink消息,參考
msg/qshell_req.msg千扔，在msg目錄創(chuàng)建raw_serial_rx.msg和raw_serial_tx.msg憎妙。為什么要創(chuàng)建兩個库正，主要考慮，以后還想在其他模塊重用厘唾，避免混淆 -
msg/raw_serial_rx.msg,對應(yīng)串口接收
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,對應(yīng)串口發(fā)送
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下會出現(xiàn)兩個頭文件,在消息定義時timestamp必選诀诊，字段賦值會自動生成常量
uORBMSG.png
3.2 uORB消息的提出/發(fā)布/訂閱/讀取 流程（advertise/public/subscribe/copy）
- 簡單把uORB的流程梳理一下，以便閱讀后面實(shí)際代碼
- 提出/發(fā)布必須在同一線程阅嘶，如果提出失敗属瓣，需要再次提出，貿(mào)然發(fā)布會導(dǎo)致系統(tǒng)宕機(jī)重啟
- 訂閱/讀取必須在同一線程
/*消息發(fā)送線程---------------------------------------------------*/
//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.發(fā)布消息抡蛙，可發(fā)布多次，_raw_serial_tx_pub不可為空魂迄，否者會出現(xiàn)系統(tǒng)宕機(jī)
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.消息讀取循環(huán)
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端實(shí)際代碼
src/modules/mavlink/mavlink_raw_serial_rtx.h
#pragma once
#include "mavlink_bridge_header.h"
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/raw_serial_rx.h>
#include <uORB/topics/raw_serial_tx.h>
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 <stdio.h>
#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){//發(fā)布uORB消息
orb_publish(ORB_ID(raw_serial_tx), _raw_serial_tx_pub, &tx_pkg);
}
}
default:
break;
}
}
//向地面站直接發(fā)送消息
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);
//優(yōu)先級放到最低吧粗截，以免影響飛控核心功能
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);
}
//開線程要用到的工具函數(shù),必須是靜態(tài)的
void * MavlinkRawSerialRTX::start_helper(void *instance){
((MavlinkRawSerialRTX *) instance)->rx_check_update_thread();
return nullptr;
}
//線程接收訂閱raw_serial_rx消息
void MavlinkRawSerialRTX::rx_check_update_thread(){
//設(shè)置線程名
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) {//如果更新了就取數(shù)據(jù)，并使用mavlink發(fā)送
orb_copy(ORB_ID(raw_serial_rx), raw_serial_rx_sub, &rxpkg);
//mavlink 發(fā)給地面站
sendData(rxpkg.dev,rxpkg.len,rxpkg.data);
}
}
}
3.4 串口端實(shí)際代碼
- 代碼結(jié)構(gòu)和第一章有所區(qū)別捣炬，可同時啟動多個守護(hù)進(jìn)程熊昌，相互間不干擾，通過消息中的dev字段區(qū)別到底時那個串口發(fā)送/接收湿酸。
src/modules/raw_serial_rtx/raw_serial_rtx_main.h
#pragma once
#include <pthread.h>
#include <stdbool.h>
#include <containers/List.hpp>
#include <parameters/param.h>
#include <px4_cli.h>
#include <px4_getopt.h>
#include <px4_module.h>
#include <px4_module_params.h>
#include <px4_posix.h>
#include <termios.h>
#include <errno.h>
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/raw_serial_rx.h>
#include <uORB/topics/raw_serial_tx.h>
//直接從mavlink消息定義處復(fù)制過來婿屹，想直接使用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 <string.h>
#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", "<file:dev>", "Select Serial Device", true);
PRINT_MODULE_USAGE_PARAM_INT('b', 57600, 9600, 3000000, "Baudrate (can also be p:<param_name>)", 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");
}
//入口函數(shù)
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;
}
//通過設(shè)備名獲取串口的枚舉值，沒有返回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;
}
/**
* 任務(wù)啟動函數(shù)
*/
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();
//循環(huán)不會退出推溃，除非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(){
}
//讀取數(shù)據(jù)循環(huán),發(fā)布raw_serial_rx uORB消息
void RawSerialRTX::readUartLoop(){
if(_uart_fd<0) return;
//設(shè)置線程名
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
//一直讀取知道緩存讀滿或者外部數(shù)據(jù)發(fā)送結(jié)束
while(nread<MAX_SERIAL_SIZE){
tread = read(_uart_fd, buf+nread, MAX_SERIAL_SIZE-nread);
if(tread>0) 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之后才能發(fā)布昂利，不然系統(tǒng)會宕機(jī)
orb_publish(ORB_ID(raw_serial_rx), _raw_serial_rx_pub, &rx_pkg);
}
}
//數(shù)據(jù)接受完畢或者沒有數(shù)據(jù)時，以免過度使用占用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);
}
//開線程要用到的工具函數(shù),必須是靜態(tài)的
void * RawSerialRTX::start_helper(void *instance){
((RawSerialRTX *) instance)->tx_check_update_thread();
return nullptr;
}
//線程接收訂閱raw_serial_tx消息
void RawSerialRTX::tx_check_update_thread(){
//設(shè)置線程名
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) {//如果更新了就取數(shù)據(jù)铁坎，并使用mavlink發(fā)送
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蜂奸，在調(diào)試過程中最好使用Serial 5連USB轉(zhuǎn)UART，操作NSH硬萍，如果mavlink起不來扩所，QGC里也就看不見了，PX4調(diào)試說明
- NSH top命令查看活動進(jìn)程,用綠線標(biāo)出朴乖，我們產(chǎn)生的進(jìn)程祖屏。兩個
mavlink_serial_rx_ifx,接收串口守護(hù)進(jìn)程發(fā)送的rx消息，分別在USB和TELEM1上的兩個mavlink守護(hù)進(jìn)程上產(chǎn)生寒砖；raw_serial_rtx:/dev/ttyS6,ttyS6上的串口守護(hù)進(jìn)程赐劣，產(chǎn)生raw_serial_tx:/dev/ttyS6，接收mavlink守護(hù)進(jìn)程上收到的串口發(fā)送消息哩都。
FTOP.png
- QGC 發(fā)送接收消息
FQGC.png
- 串口收發(fā)
FUART.png
- 串口收發(fā)短接魁兼，USB連接QGC，收發(fā)來回30+ms
FQECHOU.png
- 串口收發(fā)短接，sik數(shù)傳@57600連接QGC咐汞，收發(fā)來回270+ms
FQECHOT.png
至此QGC PX4串口轉(zhuǎn)發(fā)全部功能實(shí)現(xiàn)完畢盖呼，感謝圍觀
傳送門: PX4 QGC透明串口轉(zhuǎn)發(fā)
1. PX4串口讀寫
2.自定義Mavlink消息實(shí)現(xiàn)QGC和PX4 mavlink守護(hù)進(jìn)程通信
3.自定義uORB消息實(shí)現(xiàn)，實(shí)現(xiàn)PX4模塊間數(shù)據(jù)傳遞
