发布日期: 2021-09-23

更新日期: 2021-09-23

文章字数: 1.9k

阅读次数:

作者: LightningMaster

PX4飞控-uORB主题的发布与订阅

一、添加msg文件

在源代码目录/msg文件夹下新建一个.msg文件。文件内容如下。

其中test1、test2、test3均为自定义的变量。timestamp为时间戳。

二、修改CMakeLists.txt文件

在同目录下找到CMakeLists.txt文件，打开后添加刚才定义的.msg文件。

三、编译源代码

编译完源代码后，进入源代码目录/build/px4_fmu-v2_default/uORB/topics文件夹，找到与之前新建的.msg文件同名的.h文件。在我这里即为test_uorb.h文件。

可以打开这个文件查看内容。

里面有一个test_uorb_s的结构体，这个结构体里面包含之前定义的几个变量。

四、编写发布文件

.c文件编写：

/**
 * @file px4_uorb_adver_app.c

 */

#include <stdio.h>
#include <termios.h>
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <errno.h>
#include <drivers/drv_hrt.h>
#include <systemlib/err.h>
#include <fcntl.h>
#include <systemlib/mavlink_log.h>
#include <uORB/uORB.h>
#include <uORB/topics/test_uorb.h>
#include <px4_platform_common/log.h>
#include  <px4_platform_common/px4_config.h>
#include  <px4_platform_common/tasks.h>
#include  <px4_platform_common/time.h>
#include  <px4_platform_common/posix.h>
//#include  <px4_platform_common/shutdown.h>    // 包含就会报错
#include  <px4_platform_common/defines.h>

static bool thread_should_exit = false;		/**< daemon exit flag */
static bool thread_running = false;		/**< daemon status flag */
static int px4_uorb_adver_task;				/**< Handle of daemon task / thread */

/**
 * daemon management function.
 */
__EXPORT int px4_uorb_adver_main(int argc, char *argv[]);

/**
 * Mainloop of daemon.
 */
int px4_uorb_adver_thread_main(int argc, char *argv[]);

/**
 * Print the correct usage.
 */
static void usage(const char *reason);

static void
usage(const char *reason)
{
	if (reason) {
		warnx("%s\n", reason);
	}

	warnx("usage: px4_uorb_adver {start|stop|status} [-p <additional params>]\n\n");
}

/**
消息发布进程，会不断的发送自定义的消息
**/
int px4_uorb_adver_main(int argc, char *argv[])
{
	if (argc < 2) {
		usage("missing command");
		return 1;
	}

	if (!strcmp(argv[1], "start")) {

		if (thread_running) {
			warnx("daemon already running\n");
			/* this is not an error */
			return 0;
		}

		thread_should_exit = false;//定义一个守护进程
		px4_uorb_adver_task = px4_task_spawn_cmd("px4_uorb_adver",
						 SCHED_DEFAULT,
						 SCHED_PRIORITY_DEFAULT,//调度优先级
						 2000,//堆栈分配大小
						 px4_uorb_adver_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("\trunning\n");

		} else {
			warnx("\tnot started\n");
		}

		return 0;
	}

	usage("unrecognized command");
	return 1;
}

int px4_uorb_adver_thread_main(int argc, char *argv[])
{

        struct test_uorb_s test_uorb_ad;   // 自定义消息的结构体
        // 初始化数据 全零
        memset(&test_uorb_ad, 0 , sizeof(test_uorb_ad));
        // 公告主题
        orb_advert_t test_pub = orb_advertise(ORB_ID(test_uorb), &test_uorb_ad);
	warnx("[daemon] starting\n");

	thread_running = true;

	// 每隔1s 发布一次新的数据
	while (!thread_should_exit) {
        test_uorb_ad.test1 = 5;
        test_uorb_ad.test2 = 6;
        test_uorb_ad.test3 = 7;
	orb_publish(ORB_ID(test_uorb), test_pub, &test_uorb_ad);
	usleep(1000);
	}

	warnx("[daemon] exiting.\n");

	thread_running = false;

	return 0;
}

CMakeLists.txt文件编写：

############################################################################
#
#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
#    notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
#    notice, this list of conditions and the following disclaimer in
#    the documentation and/or other materials provided with the
#    distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
#    used to endorse or promote products derived from this software
#    without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_module(
	MODULE modules__px4_uorb_adver
	MAIN px4_uorb_adver
	STACK_MAIN 2000  #程序堆栈大小
	SRCS
		px4_uorb_adver.c
	DEPENDS
	)
# vim: set noet ft=cmake fenc=utf-8 ff=unix :

五、编写订阅文件

.c文件编写：

/**
 * @file px4_uorb_subs.c

 */

#include <stdio.h>
#include <termios.h>
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <errno.h>
#include <drivers/drv_hrt.h>
#include <systemlib/err.h>
#include <fcntl.h>
#include <systemlib/mavlink_log.h>
#include <uORB/uORB.h>
#include <uORB/topics/test_uorb.h>
#include <px4_platform_common/log.h>
#include  <px4_platform_common/px4_config.h>
#include  <px4_platform_common/tasks.h>
#include  <px4_platform_common/time.h>
#include  <px4_platform_common/posix.h>
//#include  <px4_platform_common/shutdown.h>    // 包含就会报错
#include  <px4_platform_common/defines.h>

static bool thread_should_exit = false;		/**< px4_uorb_subs exit flag */
static bool thread_running = false;		/**< px4_uorb_subs status flag */
static int px4_uorb_subs_task;				/**< Handle of px4_uorb_subs task / thread */

/**
 * daemon management function.
 */
__EXPORT int px4_uorb_subs_main(int argc, char *argv[]);

/**
 * Mainloop of daemon.
 */
int px4_uorb_subs_thread_main(int argc, char *argv[]);

/**
 * Print the correct usage.
 */
static void usage(const char *reason);

static void
usage(const char *reason)
{
	if (reason) {
		warnx("%s\n", reason);
	}

	warnx("usage: px4_uorb_adver {start|stop|status} [-p <additional params>]\n\n");
}

/**
消息发布进程，会不断的接收自定义消息
 */
int px4_uorb_subs_main(int argc, char *argv[])
{
	if (argc < 2) {
		usage("missing command");
		return 1;
	}

	if (!strcmp(argv[1], "start")) {

		if (thread_running) {
			warnx("px4_uorb_subs already running\n");
			/* this is not an error */
			return 0;
		}

		thread_should_exit = false;//定义一个守护进程
		px4_uorb_subs_task = px4_task_spawn_cmd("px4_uorb_subs",
						 SCHED_DEFAULT,
						 SCHED_PRIORITY_DEFAULT,//调度优先级
						 2000,//堆栈分配大小
						 px4_uorb_subs_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("\trunning\n");

		} else {
			warnx("\tnot started\n");
		}

		return 0;
	}

	usage("unrecognized command");
	return 1;
}

int px4_uorb_subs_thread_main(int argc, char *argv[])
{

	warnx("[px4_uorb_subs] starting\n");
	// 订阅主题
    int test_sub_fd = orb_subscribe(ORB_ID(test_uorb));
    // 初始化结构体
    struct test_uorb_s test_uorb_sub;
    // 初始化数据 全零
    memset(&test_uorb_sub, 0 , sizeof(test_uorb_sub));
    // 定义变量
    int test1 = 0,test2 = 0,test3 = 0;



	thread_running = true;

	while (!thread_should_exit) {
		warnx("Hello px4_uorb_subs!\n");
        	bool updated;

	/* Check if vehicle control mode has changed */
        // 检查是否更新
	orb_check(test_sub_fd, &updated);

	if (updated)
    {
    	// copy数据
        orb_copy(ORB_ID(test_uorb),test_sub_fd,&test_uorb_sub);
        // 赋值到变量
        test1 = test_uorb_sub.test1;
        test2 = test_uorb_sub.test2;
        test3 =  test_uorb_sub.test3;
	}
	// 打印
	warnx("test_uorb.test1 = %d, test_uorb.test2 = %d ,test_uorb.test3 = %d\n",test1,test2,test3);
    	usleep(500);
	}

	warnx("[px4_uorb_subs] exiting.\n");

	thread_running = false;

	return 0;
}

CMakeLists.txt文件编写：

############################################################################
#
#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
#    notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
#    notice, this list of conditions and the following disclaimer in
#    the documentation and/or other materials provided with the
#    distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
#    used to endorse or promote products derived from this software
#    without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_module(
	MODULE modules__px4_uorb_subs
	MAIN px4_uorb_subs
	STACK_MAIN 2000  #程序堆栈大小
	SRCS
		px4_uorb_subs.c
	DEPENDS
	)
# vim: set noet ft=cmake fenc=utf-8 ff=unix :

六、测试

编写完之后就可以正常的进行调用然后测试效果了。

需要先运行发布脚本再运行订阅脚本才能有效果。

LightningMaster

https://lightningleader.github.io/posts/17.html

本博客所有文章除特別声明外，均采用 CC BY 4.0 许可协议。转载请注明来源 LightningMaster !

学习笔记 PX4 无人机

【MindSpore】简单使用Resnet50实现狗狼图片分类

2022-04-15 LightningMaster

深度学习学习笔记 MindSpore

PX4飞控-自定义发送MavLink消息

2021-09-23 LightningMaster

学习笔记 PX4 无人机

PX4飞控-uORB主题的发布与订阅

PX4飞控-uORB主题的发布与订阅

一、添加msg文件

二、 修改CMakeLists.txt文件

三、 编译源代码

四、编写发布文件

五、 编写订阅文件

六、测试

二、修改CMakeLists.txt文件

三、编译源代码

五、编写订阅文件