学习px4的第一个程序,这个例子作为官方给出解释的例程,对于新手上手来说,是很好的范例。
接下来我对照程序源码(已经经过自己的修改和添加注释)进行讲解
1.PX4_INFO是标准的LOG函数,相当于C语言中的printf,你直接使用C语言也是可以的,但是更加推荐使用LOG函数,LOG函数主要包括PX4_INFO, PX4_WARE,PX4_ERR,PX4_DEBUG,也就是我们平时遇到的输入,警告,错误和调试。他们代表不同级别日志,错误和警告会写入飞控的日志中去,养成习惯方便以后阅读日志。
2.UORD是PX4的核心,具体解释很容易理解,相当于两个人无法直接沟通,只能依靠一个媒介来交流,topic相当于决定这个媒介的使用限制,只有这个主题下的信息可以留在这个媒介上,媒介对某个具体信息有固定的存放位置,你需要修改它就必须删除之前的信息,两个人拿到信息怎么处理我们不管,我们只需要了解这两个人时如何互通有无的,这就是UORD机制了。
(topic可以在Firmware/msg里查看到所有的可使用的,如果你想自定义,需要在Firmware/msg的CMakeLists里进行注册,具体格式仿照已有的主题就可以啦)
3 用UORD具体详细细节:
a :发送信息:公告(advertise)+发布(publish)
注意点:公告只能有一次,毕竟货多不值钱,你一件事说个无数次,大家反而嫌你啰嗦,系统也会报错哦,所以最好把公告写入构造函数
b :接受信息:订阅(subscribe)+发布(publish)
接受消息的主要点在于你怎么知道消息更新了?毕竟老是看过时报纸是很瓜的行为。。。
这里给出两种方式:1.PX4_poll(阻塞等待)意思是我什么都不做,就盯着你,你变了我立刻就能知道并进行处理,就好比人家欠了你钱你好不容易堵到他,还不得死死盯着?但是电脑等的时候不需要占用CPU
2.check(检查)意思就是我想起来了我就去看一下更新没有,想不起来就算了。。。佛系检测法,一般用做优先级不高的事
4.例程里还用到了限制读取数据频率的函数,平时我们不会用,如果你不想他更新那么快或者资源有限的时候用
orb_set_interval(sensor_sub_fd, 200);
//限制每200ms内只复制一次
5.看到一个大佬的博客。建议我们平时new一个例子,最后delete,是一个好习惯,把公告订阅写在构造函数,取消写在析构函数,建议学习
最后贴出我的代码,自己修改了一下风格,加了一些注释
/****************************************************************************
*
* Copyright (c) 2012-2016 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_config.h>
#include <px4_tasks.h>
#include <px4_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");
/*××××××××××××××××××××××××××××××××××××××××××××××××××××××A topic+频率××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××*/
/* subscribe to sensor_combined topic */
//topic是sensor_combined 订阅 topic目录在Firmware/msg中查看
int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));
/* limit the update rate to 5 Hz */
//限制数据读取频率
orb_set_interval(sensor_sub_fd,200);
/*×××××××××××××××××××××××××××××××××××××××××××××××××××××××B 公告××××××××××××××××××××××××××××发送=adverse(公告)+publish(发布)××××××××××××××××××××××××××××××××××*/
/* 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;
/*×××××××××××××××××××××××××××××××××××××××××××××××××××××××C 检测更新 阻塞等待poll××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××*/
for(int i=0; i<5; i++ )
{
int poll_ret = px4_poll(fds,1,1000);//检测时间de间隔
if(poll_ret==0)
{
PX4_ERR("Get no data whinin 1s");
}
else if(poll_ret<0)
{
if(error_counter<10||error_counter%50==0)
{
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("Acclerometer:\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];
/* there could be more file descriptors here, in the form like:
* if (fds[1..n].revents & POLLIN) {}
*/
orb_publish(ORB_ID(vehicle_attitude), att_pub, &att);
}
}
}
PX4_INFO("exiting");
return 0;
}
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)