近期项目有个需求要，使用地面控制站控制无人机外挂设备，在飞行过程中实现一些具体功能。外挂设备只有串口可用，本来想使用单独一套数传控制，但又觉得好蛋疼，明明GCS和飞控有通信链路。能不能直接使用这个链路呢，PX4和QGC官网文档过了一遍，好像没有，网上也很少有方法直接实现或者说没有完全实现。由于篇幅太长我将过程和源码分成相对独立的3部分，一步一步实现整个过程，欢迎围观。
设计构想
目的在QGC和PX4设备上串口上开一个透明的通道，实现QGC直接控制PX4串口外挂设备
QGC <--mavlink(数传)--> PX4(mavlink守护进程) <--uORB--> raw_serial_rtx <--UART--> device
内容提要
- PX4串口读写
- 自定义Mavlink消息实现QGC和PX4 mavlink守护进程通信
- 自定义uORB消息实现，实现PX4模块间数据传递
条件准备
Ubuntu-1804-LTS，虚拟机也可以
-
注意如果你想编译Android APP，按照项目根目录下".travis.yml"，配置对应版本的sdk,ndk,QT
PX4支持的飞控硬件，这里使用市面上最便宜的
pixhawk2.4.8（主要是穷，手动呵呵）PX4项目源码,这里使用
v1.9.2QGC项目,这里使用
v3.5.4USB转UART TTL模块用于调试
飞控串口对应的插头线，我的飞控对应的是 DF-13（JST 1.25）6P插头，将它另一头改为2.54杜邦头，插到USB转TTL上，收发交叉
1. PX4串口读写
-
Nuttx和Pixhawk串口对照表
NuttX UART Pixhawk UART /dev/ttyS0 IO DEBUG(RX ONLY) /dev/ttyS1 TELEM1(USART2) /dev/ttyS2 TELEM2(USART3) /dev/ttyS3 GPS(UART4) /dev/ttyS4 N/A(UART5, IO link) /dev/ttyS5 SERIAL5(UART7,NSH Console Only) /dev/ttyS6 SERIAL4(UART8) 这里使用pixhawk上的SERIAL4口，这个口在为文档里好像没有提到有什么特殊用途
1.1 硬件连接
-
pixhawk 2.4.8 接口说明
pix248port.jpg
-
SERIAL4与USB转UART模块的连接，注意收发交叉连接
wring.jpg
-
测试连接
- 打开窗口调试工具，找到USB串口，波特率设置为px4的57600
- 打开QGC的Mavlink控制台，输入echo abcd9876 > /dev/ttyS6
-
窗口看到abcd9876说明连接正常
QGCmvconsole.png
sertest.png
1.2 查看分析样版代码
控制台输入ps或top有
mavlink_if1 mavlink start -d /dev/ttyS1 -d p:SER_TEL1_BAUD -m p:MAV_0_MODE -r p:MAV_0_RATE -f -x
- 主要考虑mavlink也使用串口，顺便把mavlink熟悉一下，而且抄被验证的代码片段比自己写可靠性高。
- src/moudiles/mavlink/mavlink_main.cpp
//line 2635
int Mavlink::start(int argc, char *argv[])
{
//line 2658
px4_task_spawn_cmd(buf,
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
2650 + MAVLINK_NET_ADDED_STACK,
(px4_main_t)&Mavlink::start_helper,
(char *const *)argv);
}
//line 2611
int Mavlink::start_helper(int argc, char *argv[])
{
//line 2626
res = instance->task_main(argc, argv);
}
//line 1838
int Mavlink::task_main(int argc, char *argv[])
{
//line 2073
if (get_protocol() == SERIAL) {
if (Mavlink::instance_exists(_device_name, this)) {
PX4_ERR("%s already running", _device_name);
return PX4_ERROR;
}
PX4_INFO("mode: %s, data rate: %d B/s on %s @ %dB",
mavlink_mode_str(_mode), _datarate, _device_name, _baudrate);
/* flush stdout in case MAVLink is about to take it over */
fflush(stdout);
/* default values for arguments */
_uart_fd = mavlink_open_uart(_baudrate, _device_name, _force_flow_control);
//line 2197 打开串口接收任务
/* start the MAVLink receiver last to avoid a race */
MavlinkReceiver::receive_start(&_receive_thread, this);
//主循环用于处理订阅的消息，可能向串口发送数据
while (!_task_should_exit) {
/* main loop */
px4_usleep(_main_loop_delay);
...
}
}
//line 515
int
Mavlink::mavlink_open_uart(int baud, const char *uart_name, bool force_flow_control)
{
//line 611
_uart_fd = ::open(uart_name, O_RDWR | O_NOCTTY);
//line 651
/* Try to set baud rate */
struct termios uart_config;
int termios_state;
_is_usb_uart = false;
/* 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 -1;
}
/* Clear ONLCR flag (which appends a CR for every LF) */
uart_config.c_oflag &= ~ONLCR;
//line 670
/* 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 -1;
}
//line 690
if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) {
PX4_WARN("ERR SET CONF %s\n", uart_name);
::close(_uart_fd);
return -1;
}
}
//line 872 发送
void
Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
{
/* If the wait until transmit flag is on, only transmit after we've received messages.
Otherwise, transmit all the time. */
if (!should_transmit()) {
return;
}
_last_write_try_time = hrt_absolute_time();
if (_mavlink_start_time == 0) {
_mavlink_start_time = _last_write_try_time;
}
if (get_protocol() == SERIAL) {
/* check if there is space in the buffer, let it overflow else */
unsigned buf_free = get_free_tx_buf();
if (buf_free < packet_len) {
/* not enough space in buffer to send */
count_txerrbytes(packet_len);
return;
}
}
size_t ret = -1;
/* send message to UART */
if (get_protocol() == SERIAL) {
ret = ::write(_uart_fd, buf, packet_len);
}
- 数据接收
src/modules/mavlink/mavlink_receiver.cpp
//line 2542
void *
MavlinkReceiver::receive_thread(void *arg){
//line 2593 接收循环
while (!_mavlink->_task_should_exit) {
//如果读取数据量小于阈值，就要暂停一下
/* non-blocking read. read may return negative values */
if ((nread = ::read(fds[0].fd, buf, sizeof(buf))) < (ssize_t)character_count) {
const unsigned sleeptime = character_count * 1000000 / (_mavlink->get_baudrate() / 10);
px4_usleep(sleeptime);
}
//line 2644 解析处理mavlink消息
if (_mavlink->get_client_source_initialized()) {
/* if read failed, this loop won't execute */
for (ssize_t i = 0; i < nread; i++) {
if (mavlink_parse_char(_mavlink->get_channel(), buf[i], &msg, &_status)) {
/* check if we received version 2 and request a switch. */
if (!(_mavlink->get_status()->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1)) {
/* this will only switch to proto version 2 if allowed in settings */
_mavlink->set_proto_version(2);
}
/* handle generic messages and commands */
handle_message(&msg);
//消息转发
/* handle packet with parent object */
_mavlink->handle_message(&msg);
}
}
}
}
}
1.3 编写串口读写代码
- 主要功能，向串口写数据，然后原样返回，实现串口的读写
- 1.创建模块目录
src/modules/raw_serial_rtx - 2.创建编辑C代码，主要有一下两个文件
src/modules/raw_serial_rtx/raw_serial_rtx_main.h
#pragma once
#include <pthread.h>
#include <stdbool.h>
#ifdef __PX4_NUTTX
#include <nuttx/fs/fs.h>
#else
#include <arpa/inet.h>
#include <drivers/device/device.h>
#include <sys/socket.h>
#endif
#if defined(CONFIG_NET) || !defined(__PX4_NUTTX)
#include <net/if.h>
#include <netinet/in.h>
#endif
#include <containers/List.hpp>
#include <drivers/device/ringbuffer.h>
#include <parameters/param.h>
#include <perf/perf_counter.h>
#include <px4_cli.h>
#include <px4_config.h>
#include <px4_defines.h>
#include <px4_getopt.h>
#include <px4_module.h>
#include <px4_module_params.h>
#include <px4_posix.h>
#include <systemlib/mavlink_log.h>
#include <systemlib/uthash/utlist.h>
#include <uORB/topics/mavlink_log.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/radio_status.h>
#include <uORB/topics/telemetry_status.h>
#include <uORB/uORB.h>
namespace raw_serial_rtx{
class RawSerialRTX : public ModuleParams
{
public:
RawSerialRTX();
~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 void uartEcho(int uartFd,int buad);
};
}
src/modules/raw_serial_rtx/raw_serial_rtx_main.cpp
#include <termios.h>
#ifdef CONFIG_NET
#include <arpa/inet.h>
#include <netinet/in.h>
#include <netutils/netlib.h>
#endif
#include <lib/ecl/geo/geo.h>
#include <mathlib/mathlib.h>
#include <version/version.h>
#include <errno.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;
RawSerialRTX::RawSerialRTX():
ModuleParams(nullptr)
{
}
RawSerialRTX::~RawSerialRTX(){
}
/**
* 任务启动函数
*/
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;
}
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;
uartEcho(_uart_fd,_baudrate);
::close(_uart_fd);
thread_should_exit = true;
PX4_INFO("task has stop");
return PX4_OK;
}
/* 打开串口 */
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;
}
void RawSerialRTX::uartEcho(int uartFd,int buad){
if(uartFd<0) return;
int chmin = 20;
uint8_t buf[64];
int nread = 0;
int tread = 0;
const unsigned sleeptime = chmin * 1000000 / (buad / 10);
while(!thread_should_exit){
nread = 0;
tread = 0;
//使用非阻塞读取 O_NONBLOCK
//一直读取知道缓存读满或者外部数据发送结束
while(nread<(int)sizeof(buf)){
tread = read(uartFd, buf+nread, sizeof(buf)-nread);
if(tread>0) nread+=tread;
else break;
px4_usleep(sleeptime);
}
if(nread>0){
::write(uartFd, buf, nread);
PX4_INFO("echo %d bytes",nread);
}
//数据接受完毕或者没有数据时，以免过度使用占用cpu
if(tread<1) px4_usleep(sleeptime);
}
}
/**----------------------------------------------------**/
//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");
}
//入口函数
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_DEFAULT,
1000,
RawSerialRTX::start,
(char *const *)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;
}
- 添加cmake配置
src/modules/raw_serial_rtx/CMakeLists.txt
px4_add_module(
MODULE modules__raw_serial_rtx
MAIN raw_serial_rtx
COMPILE_FLAGS -Os
SRCS
raw_serial_rtx_main.cpp
DEPENDS
airspeed
git_mavlink_v2
conversion
git_ecl
ecl_geo
version
)
- 模块注册在
boards/px4/fmu-v2/default.cmake,根据实际情况选择，添加到MODULES节下添加raw_serial_rtx - 添加自启动 在
ROMFS/px4fmu_common/init.d/rcS或者在sd卡上/fs/microsd/etc/extras.txt文件最后添加raw_serial_rtx start -d /dev/ttyS6 -b 38400 - 编译
make px4fmu-v2_default - 使用QGC上传
参数 > 固件选 PX4 ,勾选高级设置，使用自定义固件，选择上一步编译结束后提示的*.px4文件。 - 进Mavlink控制台，使用top或ps查看,进程正常驻留。
- 使用串口调试工具，波特率38400，发送数据，数据正常回写。
- 测试性能，CPU占用率
- 空载，不发送数据，0.44%
- 200字节，每1秒发送一次 0.62%
- 200字节，每50ms发送一次 1.4%
- 对照组 mavlink 空载2.5%，top指令下11.5%
传送门: PX4 QGC透明串口转发
1. PX4串口读写
2.自定义Mavlink消息实现QGC和PX4 mavlink守护进程通信
3.自定义uORB消息实现，实现PX4模块间数据传递
