功能:实现 订阅 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:
-
- Redistributions of source code must retain the above copyright
- notice, this list of conditions and the following disclaimer.
-
- 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.
-
- 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;
}
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)