提示：一个飞行器爱好者，才疏学浅。通过自己学习PX4源码的过程，进行分析和记录。欢迎各路大神讨论，并指正文中错误，请轻拍。
提示：大致内容分为2个部分，其中第一部分是介绍如果添加自己的消息oURB。第二部分是如何实现连接北醒TOF传感器。
提示：以下是本篇文章正文内容，下面案例可供参考
这部分网上的例子非常多。我就正常操作一下。并实现一个简单的订阅发布实例。
1.在消息目录下创建自己的tof信息。
/px4_v1.6.0/Firmware/msg/test_tof.msg
uint32 tof_distance
uint32 tof_phase
uint32 tof_amp
uint16 tof_mode
2.在对应的msg文件目录下将创建的消息加入到CMakeLists.txt。
/px4_v1.6.0/Firmware/msg/CMakeLists.txt
subsystem_info.msg
system_power.msg
task_stack_info.msg
tecs_status.msg
telemetry_status.msg
test_motor.msg
test_tof.msg /* 添加到了这里 */
time_offset.msg
transponder_report.msg
uavcan_parameter_request.msg
uavcan_parameter_value.msg
3.进行编译生成TOPICS头文件
make px4fmu-v2-default
px4_v1.6.0/Firmware/build_px4fmu-v2_default/src/modules/uORB/topics/test_tof.h
#pragma once
#include <stdint.h>
#ifdef __cplusplus
#include <cstring>
#else
#include <string.h>
#endif
#include <uORB/uORB.h>
#ifndef __cplusplus
#endif
#ifdef __cplusplus
struct __EXPORT test_tof_s {
#else
struct test_tof_s {
#endif
uint64_t timestamp; // required for logger
uint32_t tof_distance;
uint32_t tof_phase;
uint32_t tof_amp;
uint16_t tof_mode;
uint8_t _padding0[2]; // required for logger
#ifdef __cplusplus
#endif
};
/* register this as object request broker structure */
ORB_DECLARE(test_tof);
生成了test_tof_s 数据结构和TOPICS ID，test_tof。
4.修改一个简单的发布订阅实例来测试一下。当前使用的是虚假数据，之后在本文第二部分真实的连接TOF传感器数据。
px4_v1.6.0/Firmware/src/modules/tof/px4_distance_tof.c
/****************************************************************************
/**
* @file px4_simple_app.c
* Minimal application example for PX4 autopilot
*
* @author Example User <mail@example.com>
*/
#include <px4_config.h>
#include <px4_tasks.h>
#include <px4_posix.h>
#include <unistd.h>
#include <stdio.h>
#include <poll.h>
#include <string.h>
/* 添加要是用的topics , test_tof*/
#include <uORB/uORB.h>
#include <uORB/topics/test_tof.h>
__EXPORT int px4_tof_app_main(int argc, char *argv[]);
int px4_tof_app_main(int argc, char *argv[])
{
PX4_INFO("Init Tof !!!");
/* 定义两个test_tof_s 数据结构体，tof用于发布数据，tof_copy用于订阅后复制数据 */
struct test_tof_s tof;
struct test_tof_s tof_copy;
memset(&tof, 0, sizeof(tof));
/ * 公告一个消息，目的是发布这个消息 */
orb_advert_t tof_pub = orb_advertise(ORB_ID(test_tof), &tof);
/* 对数据进行虚假赋值*/
tof.tof_amp = 100;
tof.tof_distance = 200;
tof.tof_phase = 300;
/*发送数据*/
orb_publish(ORB_ID(test_tof),tof_pub,&tof);
/* subscribe to sensor_combined topic(订阅一个消息ID) */
int sensor_tof_fd = orb_subscribe(ORB_ID(test_tof));
/* limit the update rate to 5 Hz */
//设置sensor_combined消息的订阅时间间隔200毫秒一次
orb_set_interval(sensor_tof_fd, 200);
/* 将数据copy到新的结构体中进行验证*/
orb_copy(ORB_ID(test_tof),sensor_tof_fd,&tof_copy);
/* 打印数据进行验证*/
PX4_INFO("[px4_tof] amp %d, distance %d ,phase %d\r\n", tof_copy.tof_amp,tof_copy.tof_distance,tof_copy.tof_phase);
PX4_INFO("exiting");
return 0;
}
5.将编写的文件加入总的CMakeList。让它可以被编译。
px4_v1.6.0/Firmware/cmake/configs/nuttx_px4fmu-v2_default.cmake
modules/sensors
modules/tof
之后编译并下载
make px4fmu-v2_default
make px4fmu-v2_default upload
6.打开QGC的NSH控制台输入px4_tof_app
看到输出了对应的虚假数据，简单的测试完毕。
1.查看原理图确定使用的串口。
- TELEM1 : /dev/ttyS1
- TELEM2 : /dev/ttyS2
- GPS : /dev/ttyS3
- NSH : /dev/ttyS5
- SERIAL4: /dev/ttyS6
- N/A : /dev/ttyS4
- IO DEBUG (RX only):/dev/ttyS0
- 根据rcS端口定义所以使用 px4_tof_uart start /dev/ttyS6 ---- USART8 115200 */
因为北醒TOF使用的波特率为115200,所以需要对串口8，连接外设的SERIAL4波特率进行修改。
px4_v1.6.0/Firmware/nuttx-configs/px4fmu-v2/nsh/defconfig
# UART8 Configuration
#
CONFIG_UART8_RXBUFSIZE=300
CONFIG_UART8_TXBUFSIZE=300
CONFIG_UART8_BAUD=115200 /* 修改波特率为115200 */
CONFIG_UART8_BITS=8
CONFIG_UART8_PARITY=0
CONFIG_UART8_2STOP=0
# CONFIG_UART8_IFLOWCONTROL is not set
# CONFIG_UART8_OFLOWCONTROL is not set
# CONFIG_UART8_DMA is not set
# CONFIG_PSEUDOTERM is not set
CONFIG_USBDEV=y
2.编写对应的实现文件
Firmware/src/modules/opt3101/opt3101.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 <systemlib/systemlib.h>
#include <fcntl.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <uORB/topics/test_tof.h>
#include <px4_tasks.h>
#include <poll.h>
#include <px4_posix.h>
static bool thread_should_exit = false; /*Ddemon exit flag*///定义查看进程存在标志变量
static bool thread_running = false; /*Daemon status flag*///定义查看进程运行标志变量
static int px4_tof_task;//定义进程变量
__EXPORT int px4_tof_uart_main(int argc, char *argv[]);
int px4_tof_uart_thread_main(int argc, char *argv[]);
static int tof_uart_init(const char * uart_name);//串口初始化函数，形参为串口路径
static int set_uart_baudrate(const int fd, unsigned int baud);//设置串口波特率函数
static void usage(const char *reason);//进程提示函数
int set_uart_baudrate(const int fd, unsigned int baud)
{
int baudrate;
switch (baud) {
case 9600: baudrate = B9600; break;
case 19200: baudrate = B19200; break;
case 38400: baudrate = B38400; break;
case 57600: baudrate = B57600; break;
case 115200: baudrate = B115200; break;
default:
warnx("ERR: baudrate: %d\n", baudrate);
return -EINVAL;
}
//实例化termios结构体，命名为uart_config
struct termios uart_config;
int termios_state;
/* fill the struct for the new configuration */
tcgetattr(fd, &uart_config);
/* clear ONLCR flag (which appends a CR for every LF) */
uart_config.c_oflag &= ~ONLCR;
/* no parity, one stop bit */
uart_config.c_cflag &= ~(CSTOPB | PARENB);
/* set baud rate */
if ((termios_state = cfsetispeed(&uart_config, baudrate)) < 0) {
warnx("ERR: %d (cfsetispeed)\n", termios_state);
return false;
}
if ((termios_state = cfsetospeed(&uart_config, baudrate)) < 0) {
warnx("ERR: %d (cfsetospeed)\n", termios_state);
return false;
}
if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {
warnx("ERR: %d (tcsetattr)\n", termios_state);
return false;
}
return true;
}
static int tof_uart_init(const char * uart_name)
{
int serial_fd = open(uart_name, O_RDWR | O_NOCTTY| O_NONBLOCK);
if (serial_fd < 0) {
err(1, "failed to open port: %s", uart_name);
return false;
}
return serial_fd;
}
static void usage(const char *reason)
{
if (reason) {
fprintf(stderr, "%s\n", reason);
}
fprintf(stderr, "usage: px4_tof_uart {start|stop|status} [param]\n\n");
exit(1);
}
int px4_tof_uart_main(int argc, char *argv[])
{
if (argc < 2) {
usage("missing command plase enter help");
}
if (!strcmp(argv[1], "start")) {
if (thread_running) {
warnx("px4 tof uart already running\n");
return 0;
}
thread_should_exit = false;
px4_tof_task = px4_task_spawn_cmd("px4_tof_uart",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
1000,
px4_tof_uart_thread_main,
(argv) ? (char * const *)&argv[2] : (char * const *)NULL);
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;
}
/*
* TELEM1 : /dev/ttyS1
* TELEM2 : /dev/ttyS2
* GPS : /dev/ttyS3
* NSH : /dev/ttyS5
* SERIAL4: /dev/ttyS6
* N/A : /dev/ttyS4
* IO DEBUG (RX only):/dev/ttyS0
* rcS px4_tof_uart start /dev/ttyS6 ---- USART8 115200 */
int px4_tof_uart_thread_main(int argc, char *argv[])
{
if (argc < 2) {
errx(1, "need a serial port name as argument");
usage("eg:");
}
const char *uart_name = argv[1];
warnx("opening port %s", uart_name);
/*Uart Init*/
int tof_uart = tof_uart_init(uart_name);
if(false == tof_uart)
{
PX4_INFO("open_uart_port falied\n");
return -1;
}
if(false == set_uart_baudrate(tof_uart,115200)){//设置串口波特率为57600
PX4_INFO("set_uart_baudrate is failed\n");
return -1;
}
PX4_INFO("tof uart init is successed\n");
/*进程标志变量*/
thread_running = true;
/*初始化数据结构体 */
struct test_tof_s g_tof;
struct test_tof_s s_tof;
memset(&g_tof, 0, sizeof(g_tof));
memset(&s_tof, 0, sizeof(s_tof));
/* 公告主题 */
orb_advert_t tof_uart_pub = orb_advertise(ORB_ID(test_tof), &g_tof);
/*定义接收话题变量*/
bool updated = false;
int tof_uart_fd = orb_subscribe(ORB_ID(test_tof));//订阅input_rc消息
/*定义串口事件阻塞结构体及变量*/
px4_pollfd_struct_t fds[] = {
{ .fd = tof_uart, .events = POLLIN },
};
int error_counter = 0;
while(!thread_should_exit){
/*检查更新*/
orb_check(tof_uart_fd, &updated);
if (updated) {
orb_copy(ORB_ID(test_tof), tof_uart_fd, &s_tof);
PX4_INFO("orb_copy tof_distance =%d ,tof_amp =%d , tof_mode =%d\n", s_tof.tof_distance, s_tof.tof_amp,s_tof.tof_mode);
}
int poll_ret = poll(fds,1,5);//阻塞等待5ms
if (poll_ret == 0)
{
/* this means none of our providers is giving us data */
PX4_INFO("No receive data for 5ms\n");
} else if (poll_ret < 0)
{
/* this is seriously bad - should be an emergency */
if (error_counter < 10 || error_counter % 50 == 0)
{
/* use a counter to prevent flooding (and slowing us down) */
PX4_INFO("ERROR return value from poll(): %d\n", poll_ret);
}
error_counter++;
}
else
{
if (fds[0].revents & POLLIN)
{
uint8_t data;
uint8_t buffer[10] = {0};
/*接收服务系统发过来的消息*/
read(tof_uart,&data,1);//读取串口数据
if(data == 0x59)
{//找到帧头0x59
buffer[0] = 0x59;
read(tof_uart,&data,1);//读取串口数据
if(data == 0x59)
{//找到帧头0x59
buffer[1] = 0x59;
for(int i = 2;i <8;i++)
{
read(tof_uart,&data,1);//读取后面的数据
buffer[i] = data;
//PX4_INFO("buffer=%d ",buffer[i]);//将消息打印出来
}
g_tof.tof_distance = buffer[3]<<8 | buffer[2];
g_tof.tof_amp = buffer[5]<<8 | buffer[4];
g_tof.tof_mode = buffer[6];
PX4_INFO("orb_publish tof_distance =%d ,tof_amp =%d , tof_mode =%d\n", g_tof.tof_distance,g_tof.tof_amp,g_tof.tof_mode);//将消息打印出来
orb_publish(ORB_ID(test_tof), tof_uart_pub, &g_tof);//发布话题
}
}
}
}
}
//如果标志位置flase应该退出进程
warnx("exiting");
thread_running = false;
close(tof_uart);
fflush(stdout);
return 0;
}
其中北醒的数据协议是
3.查看实验结果之前，同样需要添加到
px4_v1.6.0/Firmware/cmake/configs/nuttx_px4fmu-v2_default.cmake
在控制台输入 px4_tof_uart start /dev/ttyS6
可以看到订阅和发送的数据是相同的。 并且解析出了北醒TOF的传感器数据。