目录
3.1.创建read_uart_sensor文件夹以及相关文件
3.3.添加串口读取程序read_uart_sensor.c内容
学无止境,不进则退。
这个项目对于我来说很难,我和好友一起搞了许久才搞好,参考了许多大佬的文章才整理出来了的。
大佬链接:(28条消息) PX4实战之旅(二):通过自定义mavlink消息和QGC通信_老王机器人的博客-CSDN博客
(28条消息) PX4飞控读取UART串口信息通过Mavlink传给QGC地面站显示_XXX_UUU_XXX的博客-CSDN博客_飞控串口协议
(28条消息) PX4读取串口消息,并通过MAVLINK发送给地面站_WENLAISIJEI的博客-CSDN博客
望君参考之余,先赏个赞!!谢谢!!
1——建立.xml并生成mavlink自定义消息文件
1.1.打开终端克隆MAVLINK生成器
git clone https://github.com/mavlink/mavlink.git
1.2.进入克隆目录,执行以下命令
git submodule update --init --recursive
sudo apt-get install python3-tk
sudo pip3 install future
1.3.随手建立储存文件夹
所有的所有路径要英文
1.4.创建read_uart_sensor.xml文件
其内容(因为我的是两个拉力传感器,其数据是4位的,所以选用了float):
<?xml version="1.0"?>
<!-- MESSAGE IDs 150 - 240: Space for custom messages in individual projectname_messages.xml files -->
<mavlink>
<version>3</version>
<messages>
<message id="166" name="READ_UART_SENSOR">
<description>READ_UART_SENSOR</description>
<field type="uint64_t" name="timestamp">Timestamp in milliseconds since system boot</field>
<field type="float" name="data">float</field>
<field type="float" name="data1">float</field>
</message>
</messages>
</mavlink>
1.5.打开MAVLINK Generator生成器
来到上面这个页面下,在终端执行下面的指令,从而打开MAVLINK Generator生成器
python3 -m mavgenerate
1.6.生成文件自定义MAVLINK消息文件
会弹出一个框,选OK就好。下面是生成的内容。
这个文件就是我们要的MAVLINK库文件
2——修改PX4的文件(添加自定义消息所需相关设置)
2.1.放置自定义MAVLINK消息文件
把上面生成的文件放到此路径下,如下图:
2.2.创建read_uart_sensor.msg
所在位置及其内容,瞅瞅下图:
uint64 timestamp # time since system start (microseconds)
char[10] datastr
char[10] data1str
float32 data
float32 data1
# TOPICS read_uart_sensor
2.3.将创建的.msg添加到编译目录
瞅瞅下图:
2.4.修改common.h文件的内容
对应位置放:
#include "./mavlink_msg_read_uart_sensor.h"
MAVLINK_MESSAGE_INFO_READ_UART_SENSOR,
注:在我们之前的实验中,一开始用的是ID223,但是没有效果。。多次实验后,选用166就得行了。目前没找到原因,也许是我们没有做尝试更多的ID。虽然有QGC有ID166,但是我们换取内容后ID166是不会影响飞控的,
{ "REAR_UART_SENSOR", 166 },
复制下面文件的这段代码。我的是这个哈,你们的我就不晓得,要看你们文件里的是啥
{166, 218, 16, 16, 0, 0, 0},
添加到这里(这里是第27行的数组#define MAVLINK_MESSAGE_CRCS):
2.5.修改mavlink_messages.cpp的内容
注:时刻区分大小写
#include "streams/READ_UART_SENSOR.hpp"
注:学会仿照添加
#if defined(READ_UART_SENSOR_HPP)
create_stream_list_item<MavlinkStreamReadUartSensor>(),
#endif // READ_UART_SENSOR_HPP
2.6.修改mavlink_main.cpp的内容
设置发送频率(MHZ),同时设置发送方式,我们这里直接放在switch语句的最后面,就可以不用管发送方式了:
configure_stream_local("READ_UART_SENSOR", 20.0f);
3——添加串口读程序以及uorb发送
3.1.创建read_uart_sensor文件夹以及相关文件
3.2.添加CMakeLists.txt编译脚本内容
简单理解就是告诉编译器,记得把resd_uart_sensor.c文件给编译哈
set(MODULE_CFLAGS)
px4_add_module(
MODULE modules_read_uart_sensor
MAIN read_uart_sensor
COMPILE_FLAGS
-Os
SRCS
read_uart_sensor.c
DEPENDS
)
3.3.添加串口读取程序read_uart_sensor.c内容
这里面是精华:如果你要换文件,那么改的东西会不少哦。(具体的参考最上面的链接,那都是大佬)
/*
* read_uart_sensor.c
*
* read sensor through uart
*/
#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 <px4_platform_common/tasks.h>
#include <uORB/topics/read_uart_sensor.h>
__EXPORT int read_uart_sensor_main(int argc, char *argv[]);
static bool thread_should_exit = false; /*Ddemon exit flag*/
static bool thread_running = false; /*Daemon status flag*/
static int daemon_task;
/**
* Main loop
*/
int read_uart_thread_main(int argc, char *argv[]);
static int 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 speed;
switch (baud) {
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;
default:
warnx("ERR: baudrate: %d\n", baud);
return -EINVAL;
}
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;// 将NL转换成CR(回车)-NL后输出
/* no parity, one stop bit */
/* 无偶校验,一个停止位 */
uart_config.c_cflag &= ~(CSTOPB | PARENB);// CSTOPB 使用两个停止位,PARENB 表示偶校验
/* set baud rate */
/* 设置波特率 */
if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
warnx("ERR: %d (cfsetispeed)\n", termios_state);
return false;
}
if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
warnx("ERR: %d (cfsetospeed)\n", termios_state);
return false;
}
// 设置与终端相关的参数,TCSANOW 立即改变参数
if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {
warnx("ERR: %d (tcsetattr)\n", termios_state);
return false;
}
return true;
}
int uart_init(const char * uart_name)
{
int serial_fd = open(uart_name, O_RDWR | O_NOCTTY);
/*Linux中,万物皆文件,打开串口设备和打开普通文件一样,使用的是open()系统调用*/
// 选项 O_NOCTTY 表示不能把本串口当成控制终端,否则用户的键盘输入信息将影响程序的执行
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: read_uart_sensor {start|stop|status} [param]\n\n");
exit(1);
}
//主函数入口
int read_uart_sensor_main(int argc, char *argv[])
{
if (argc < 2) {
usage("[Fantasy]missing command");
}
//输入为start
if (!strcmp(argv[1], "start")) {
if (thread_running) {//进程在运行
warnx("[Fantasy]already running\n");//打印提示已经在运行
return 0;;//跳出代码
}
//如果是第一次运行
thread_should_exit = false;//定义一个守护进程
//建立名为read_uart_sensor进程SCHED_PRIORITY_MAX - 55,
daemon_task = px4_task_spawn_cmd("read_uart_sensor",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,//调度优先级
//SCHED_PRIORITY_MAX - 1,
2000,//堆栈分配大小
read_uart_thread_main,
(argv) ? (char * const *)&argv[2] : (char * const *)NULL);
return 0;
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;//进程标志变量置true
return 0;
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
warnx("[Fantasy]running");
return 0;
} else {
warnx("[Fantasy]stopped");
return 1;
}
return 0;
}
//若果是其他,则打印不支持该类型
usage("unrecognized command");
return 1;
}
int read_uart_thread_main(int argc, char *argv[])
{
int index = 0;
char data1 = '0';
char buffer[10] = "0";
const char douhao[2] = ","; //逗号
char *result = NULL;
// const char douhao[2] = ","; //逗号
//char *result = NULL;
// int index = 0;
/*
* 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
*/
/*配置串口*/
int uart_read = uart_init("/dev/ttyS3");//初始化串口设备
if(false == uart_read)return -1;
if(false == set_uart_baudrate(uart_read,115200)){ //设置波特率
printf("[Fantasy]set_uart_baudrate is failed\n");
return -1;
}
printf("[Fantasy]uart init is successful\n");
/*进程标志变量*/
thread_running = true;
/*初始化数据结构体 */
struct read_uart_sensor_s sensordata;//实例化read_uart_sensor变量,该变量是自己定义的变量
memset(&sensordata, 0, sizeof(sensordata));//初始化sensordata变量
/* 公告主题 */
orb_advert_t read_uart_sensor_pub = orb_advertise(ORB_ID(read_uart_sensor), &sensordata);
/*定义串口事件阻塞结构体及变量*/
//px4_pollfd_struct_t fds[] = {
// { .fd = serv_uart, .events = POLLIN },
//};
//int error_counter = 0;
while(!thread_should_exit){
read(uart_read,&data1,1);
if(data1 == 'r'){
for(int i = 0;i <10;++i){
read(uart_read,&data1,1);
buffer[i] = data1;
data1 = '0';
}
//逗号分割,返回下一个分割后的字符串指针,如果没有可检索的字符串,则返回一个空指针。
result = strtok(buffer, douhao);
while(result != NULL) {
index++;
switch(index){
case 1:
strncpy(sensordata.datastr,result,4);
break;
case 2:
strncpy(sensordata.data1str,result,4);
break;
}
result = strtok(NULL, douhao);
}
index = 0;
//strncpy(sensordata.datastr,buffer,5);
//atoi()把字符串转换成整型数
// atof()把字符串转换成浮点数,默认为6位小数
//sensordata.data = 0;
//sensordata.data = sensordata.datastr;
//sensordata.data = atoi(sensordata.datastr);
sensordata.data = atof(sensordata.datastr);
sensordata.data1 = atof(sensordata.data1str);
//printf("[YCM]sonar.data=%d\n",sonardata.data);
orb_publish(ORB_ID(read_uart_sensor), read_uart_sensor_pub, &sensordata);
}
}
warnx("[YCM]exiting");
thread_running = false;
close(uart_read);
fflush(stdout);
return 0;
}
3.4.到对应机型中添加编译文件路径
我用的是雷迅X7品牌的,它的型号是雷迅X7pro。编译px4源码的指令是:
make cuav_x7pro_default
那么在这个型号基础上找到这个文件:
之前创建的文件夹是在modules文件夹下,所以添加位置:
read_uart_sensor
3.5.创建READ_UART_SENSOR.hpp
订阅read_uart_sensor消息然后将消息内容打包为MAVLINK数据帧并发送:
内容如下:
#ifndef READ_UART_SENSOR_HPP
#define READ_UART_SENSOR_HPP
//#include <stdio.h>
#include <uORB/topics/read_uart_sensor.h>//包含uorb消息结构体的头文件
#include <v2.0/common/mavlink_msg_read_uart_sensor.h> //包含生成器生成的头文件
#include <modules/mavlink/mavlink_stream.h> //自定义类继承与MavlinkStream,所以要包含
//#include <v2.0/mavlink_types.h>
//#include <uORB/SubscriptionInterval.hpp>
//#include <uORB/uORB.h>
class MavlinkStreamReadUartSensor : public MavlinkStream
{
public:
//explicit MavlinkStreamReadUartSensor(Mavlink *mavlink) : MavlinkStream(mavlink) {}
static MavlinkStream *new_instance(Mavlink *mavlink)
{ return new MavlinkStreamReadUartSensor(mavlink); }
//void update(orb_advert_t *mavlink_log_pub);
static constexpr const char *get_name_static()
{
return "READ_UART_SENSOR";
}
static constexpr uint16_t get_id_static()
{
return MAVLINK_MSG_ID_READ_UART_SENSOR;
}
const char *get_name() const override
{
return get_name_static();
//return MavlinkStreamReadUartSensor::get_name_static();
}
uint16_t get_id() override
{
return get_id_static();
}
unsigned get_size() override
{
return _read_uart_sensor_sub.advertised() ? (MAVLINK_MSG_ID_READ_UART_SENSOR_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES): 0 ;
}
//private:
//uORB::Subscription _read_uart_sensor_sub{ORB_ID(read_uart_sensor)};
//uint64_t _read_uart_sensor_time;
/* do not allow top copying this class */
// MavlinkStreamread_uart_sensor(MavlinkStreamread_uart_sensor &) = delete;
// MavlinkStreamread_uart_sensor &operator = (const MavlinkStreamread_uart_sensor &) = delete;
private:
explicit MavlinkStreamReadUartSensor(Mavlink *mavlink) : MavlinkStream(mavlink) {}
//uORB::Subscription _read_uart_sensor_sub{ORB_ID(read_uart_sensor)};//订阅
//_read_uart_sensor.copy(&_read_uart_sensor);//获取消息数据
//~MavlinkStreamReadUartSensor();
uORB::Subscription _read_uart_sensor_sub{ORB_ID(read_uart_sensor)};
bool send() override //用于PX4真正发送的函数
{
read_uart_sensor_s orbtest;
//read_uart_sensor_s orbtest = {};
//struct read_uart_sensort_s; //定义uorb消息结构体
// mavlink_read_uart_sensor_t msg{};
//_read_uart_sensor_sub.copy(&read_uart_sensor);
// orb_copy(ORB_ID(read_uart_sensor), orbtest_sub_fd, &orbtest);
//if (true)
if(_read_uart_sensor_sub.update(&orbtest)){
//int sensor_sub_fd = orb_subscribe(ORB_ID(read_uart_sensor));//订阅
//orb_copy(ORB_ID(read_uart_sensor), sensor_sub_fd, &_read_uart_sensor);//获取消息数据
mavlink_read_uart_sensor_t msg{};
//uORB::Subscription _read_uart_sensor{ORB_ID(read_uart_sensor)};//订阅
//_read_uart_sensor.copy(&_read_uart_sensor);//获取消息数据
//mavlink_read_uart_sensor_t msg;//定义mavlink消息结构体
msg.timestamp = orbtest.timestamp; //这里uorb数据赋值到mavlink结构体上
//msg.datastr = orbtest.datastr;
msg.data = orbtest.data;
msg.data1 = orbtest.data1;
//
//msg.ll_sensor = orbtest.ll_sensor;
mavlink_msg_read_uart_sensor_send_struct(_mavlink->get_channel(), &msg);//利用生成器生成的mavlink_msg_read_uart_sensor.h头文件里面的这个函数将msg(mavlink结构体)封装成mavlink消息帧并发送;
printf("[Fantasy]uart init is successful\n");
return true;
}
return false;
}
};
#endif
4——功能:设置uorb自启动
如果没有写这个自启动,你需要在QGC的调试台,输入 read_uart_sensor start
版本不同,一般情况根据上下文找到这个指令的放置位置
read_uart_sensor start
5——功能:打印串口消息,到MAVLINK终端
5.1.创建文件夹px4_test及其文件
5.2.添加CMakeLists.txt内容
px4_add_module(
MODULE examples__px4_test
MAIN px4_test
SRCS
px4_test.c
DEPENDS
)
5.3.添加 px4_test.c内容
/****************************************************************************/
/*
* px4_test.c
*
* test the uart sensor app
*/
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/tasks.h>
#include <px4_platform_common/posix.h>
#include <unistd.h>
#include <stdio.h>
#include <poll.h>
#include <string.h>
#include <math.h>
#include <uORB/uORB.h>
#include <uORB/topics/read_uart_sensor.h>
__EXPORT int px4_test_main(int argc, char *argv[]);
int px4_test_main(int argc, char *argv[])
{
printf("Hello Sky!\n");
/* subscribe to rw_uart_sensor topic */
int sensor_sub_fd = orb_subscribe(ORB_ID(read_uart_sensor));
/*设置以一秒钟接收一次,并打印出数据*/
orb_set_interval(sensor_sub_fd, 1000);
// bool updated;
// struct read_uart_sensor_data_s sensordata;
/* one could wait for multiple topics with this technique, just using one here */
// px4_pollfd_struct_t
struct pollfd fds[] = {
{ .fd = sensor_sub_fd, .events = POLLIN },
/* there could be more file descriptors here, in the form like:
* { .fd = other_sub_fd, .events = POLLIN },
*/
};
int error_counter = 0;
for (int i = 0;i<20 ; i++) { // infinite loop
/* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */
int poll_ret = poll(fds, 1, 1000);
/* handle the poll result */
if (poll_ret == 0) {
/* this means none of our providers is giving us data */
printf("[px4_test] Got no data within a second\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) */
printf("[px4_test] ERROR return value from poll(): %d\n", poll_ret);
}
error_counter++;
} else {
if (fds[0].revents & POLLIN) {
/* obtained data for the first file descriptor */
struct read_uart_sensor_s sensordata;
/* copy sensors raw data into local buffer */
orb_copy(ORB_ID(read_uart_sensor), sensor_sub_fd, &sensordata);
printf("[px4_test] sensor datastr:\t%s\t%d\n",
sensordata.datastr,
(double)sensordata.data,
sensordata.data1str,
(double)sensordata.data1);
// printf("[px4_test] sensor data:\t%s\t%d\n",(int)sensordata.data);
}
/* there could be more file descriptors here, in the form like:
* if (fds[1..n].revents & POLLIN) {}
*/
}
}
// PX4_INFO("exiting");
return 0;
}
注:这个功能实在QGC上用的。在调试台输入px4_test就可以参看串口数据了。后面会演示一哈
5.4.到对应机型中添加编译文件路径
这里的“#”号功能是注释(也就是屏蔽的意思)。
px4_test
6——编译调试
6.1.编译固件
我的是雷迅X7pro,你们的有如果不一样,编译指令就要改哈(具体的自己去搜哦)
make cuav_x7pro_default
编译成功
6.2.下载固件
找到这个文件(后缀.px4)
点击确定 ,找到刚刚的.px4文件,点击打开,然后就是一个短暂的发呆时间,好了该醒一醒,地面站已经运行了。