示例：PX4——添加msguORB

Posted 2021-09-13 XXX_UUU_XXX

git clone https://github.com/PX4/Firmware

cd Firmware

git submodule update --init --recursive

git checkout v1.11.0-beta1

make distclean

在Firmware/msg/目录下添加文件test_tof.msg，内容如下

uint64 timestamp
uint32 tof_distance
uint32 tof_phase
uint32 tof_amp
uint16 tof_mode

在Firmware/msg/CMakeLists.txt中添加一行

test_tof.msg

在Firmware目录下编译

make px4_fmu-v5_default

 在Firmware/build/px4_fmu-v5_default/uORB/topics/目录下生成test_tof.h文件

发布订阅代码参考Firmware/src/examples/px4_simple_app

/**
 * @file px4_simple_app.c
 * Minimal application example for PX4 autopilot
 *
 * @author Example User <[e-mail]>
 */

#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/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>

__EXPORT int px4_simple_app_main(int argc, char *argv[]);

int px4_simple_app_main(int argc, char *argv[])
{
	PX4_INFO("Hello Sky!");

	/* subscribe to sensor_combined topic */
	int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));
	/* limit the update rate to 5 Hz */
	orb_set_interval(sensor_sub_fd, 200);

	/* advertise attitude topic */
	struct vehicle_attitude_s att;
	memset(&att, 0, sizeof(att));
	orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);

	/* one could wait for multiple topics with this technique, just using one here */
	px4_pollfd_struct_t 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 < 5; i++) {
		/* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */
		int poll_ret = px4_poll(fds, 1, 1000);

		/* handle the poll result */
		if (poll_ret == 0) {
			/* this means none of our providers is giving us data */
			PX4_ERR("Got no data within a second");

		} 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_ERR("ERROR return value from poll(): %d", poll_ret);
			}

			error_counter++;

		} else {

			if (fds[0].revents & POLLIN) {
				/* obtained data for the first file descriptor */
				struct sensor_combined_s raw;
				/* copy sensors raw data into local buffer */
				orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw);
				PX4_INFO("Accelerometer:\\t%8.4f\\t%8.4f\\t%8.4f",
					 (double)raw.accelerometer_m_s2[0],
					 (double)raw.accelerometer_m_s2[1],
					 (double)raw.accelerometer_m_s2[2]);

				/* set att and publish this information for other apps
				 the following does not have any meaning, it's just an example
				*/
				att.q[0] = raw.accelerometer_m_s2[0];
				att.q[1] = raw.accelerometer_m_s2[1];
				att.q[2] = raw.accelerometer_m_s2[2];

				orb_publish(ORB_ID(vehicle_attitude), att_pub, &att);
			}

			/* there could be more file descriptors here, in the form like:
			 * if (fds[1..n].revents & POLLIN) {}
			 */
		}
	}

	PX4_INFO("exiting");

	return 0;
}

 在Firmware/src/modules/目录下创建test_tof文件夹（发布订阅实例），并在其中创建test_tof.c和CMakeLists.txt文件 

/**
 * @file px4_simple_app.c
 * Minimal application example for PX4 autopilot
 *
 * @author Example User <[e-mail]>
 */


#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>

/* 添加要是用的topics , test_tof*/
#include <uORB/uORB.h>
#include <../build/px4_fmu-v5_default/uORB/topics/test_tof.h>

__EXPORT int test_tof_main(int argc, char *argv[]);

int test_tof_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;
}

px4_add_module(
	MODULE tof__px4_tof_app
	MAIN px4_tof_app
	SRCS
		px4_tof_app.c
	DEPENDS
	)

在Firmware/boards/px4/fmu-v5/default.cmake中的MODULES中添加test_tof

然后编译/下载

make px4_fmu-v5_default upload