PX4分析系列之添加北醒TOF传感器（使用UART）

编程入门行业动态更新时间:2024-10-22 23:38:23

PX4分析系列之添加北醒TOF<a href=https://www.elefans.com/category/jswz/34/1770119.html style= 传感器（使用UART）"/>

PX4分析系列之添加北醒TOF传感器（使用UART）

提示：一个飞行器爱好者，才疏学浅。通过自己学习PX4源码的过程，进行分析和记录。欢迎各路大神讨论，并指正文中错误，请轻拍。

文章目录

- PX4分析系列之添加北醒TOF传感器（使用UART）
前言
一、如何自定义uORB消息，创建TOF数据结构
二、连接北醒TOF传感器并通过串口获取数据
总结

前言

提示：大致内容分为2个部分，其中第一部分是介绍如果添加自己的消息oURB。第二部分是如何实现连接北醒TOF传感器。

提示：以下是本篇文章正文内容，下面案例可供参考

一、如何自定义uORB消息，创建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 {
#endifuint64_t timestamp; // required for loggeruint32_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>*/#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

看到输出了对应的虚假数据，简单的测试完毕。

二、连接北醒TOF传感器并通过串口获取数据

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_configstruct 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)){//设置串口波特率为57600PX4_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);//阻塞等待5msif (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){//找到帧头0x59buffer[0] = 0x59;read(tof_uart,&data,1);//读取串口数据if(data == 0x59){//找到帧头0x59buffer[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的传感器数据。