PX4官方examples 发布与订阅 分析

功能：实现 订阅 sensor_combined 消息，将订阅的消息数据 通过发布主题（ vehicle_attitude）发布出去
/****************************************************************************
*

• Copyright © 2012-2019 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.

****************************************************************************/

/**

• @file px4_simple_app.c
• Minimal application example for PX4 autopilot
• @author Example User mail@example.com
  */

#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> //引入 需要订阅sensor_combined 消息头文件
#include <uORB/topics/vehicle_attitude.h> //引入 需要发布vehicle_attitude 消息头文件

__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 */ 
//功能：订阅主题（topic）
//说明：通过实例的ID索引来确定是主题的哪个实例；
//可以认为是主题id，一般是通过ORB_ID(主题名)来赋值;
int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));

/* limit the update rate to 5 Hz */
//int orb_set_interval(int handle, unsigned interval)
//功能：设置订阅的最小时间间隔；订阅消息间隔200ms一次
//说明：如果设置了，则在这间隔内发布的数据将订阅不到；需要注意的是，设置后，第一次的数据订阅还是由起初设	 //  	置的频率来获取
//参数：handle:orb_subscribe函数返回的句柄；interval:间隔时间，单位ms;
orb_set_interval(sensor_sub_fd, 200);

/* advertise attitude topic */
//生成一个来自vehicle_attitude.h的vehicle_attitude_s类型的结构体
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
//功能：公告发布者的主题；
//说明：在发布主题之前是必须的；否则订阅者虽然能订阅，但是得不到数据；
//参数：uORB元对象，可以认为是主题id，一般是通过ORB_ID(主题名)来赋值;
//	   指向一个已被初始化，发布者要发布的数据存储变量的指针；
//返回值：错误则返回ERROR;成功则返回一个可以发布主题的句柄；如果待发布的主题没有定义或声明则会返回-1，然后会将errno赋值为ENOENT;
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) */
	//添加等待订阅消息的时间，非阻塞，如果订阅n个消息，②将修改第二个参数n
	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语句fd[n]...
		if (fds[0].revents & POLLIN) {
		
			/* obtained data for the first file descriptor */
			//创建来自sensor_combined.h的sensor_combined_s结构体实例;注意结构体 *_s
			struct sensor_combined_s raw;
			
			/* copy sensors raw data into local buffer */
			//读取sensor_sub_fd的数据到raw中
			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
			*/
			//只是试验例程，无功能意义；raw数组中的数据赋值给att数组
			att.q[0] = raw.accelerometer_m_s2[0];
			att.q[1] = raw.accelerometer_m_s2[1];
			att.q[2] = raw.accelerometer_m_s2[2];
			
			//实现向vehicle_attitude消息中发布数据
			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;

}