Step 5: 订阅传感器数据
为了做有用的事情，应用需要订阅subscribe输入并发布publish输出 (e.g. 电机 或伺服指令). PX4平台真正的硬件抽象（true hardware abstraction ）开始有用——不需要与传感器驱动以任何方式相互作用，并且电路板或者传感器升级后不需要升级。
PX4中应用间独立的消息通道被称作topics. 在这里，感兴趣的是 sensor_combined topic, 其调用整个系统的同步传感器数据。
订阅到topic非常快和干净：
#include <uORB/topics/sensor_combined.h> .. int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));
sensor_sub_fd 是一个文件描述符，非常高效的执行一块等待新的数据的数据块。当前的进程进入休眠，当有新的数据可用时，由调度程序自动地唤醒，在等待的时候，不占用任何CPU周期。为实现这点，使用poll()，调用 POSIX system 。
将poll() 添加到subscription看上去像：
#include <poll.h> #include <uORB/topics/sensor_combined.h> .. int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined)); /* one could wait for multiple topics with this technique, just using one here */ struct pollfd fds[] = { { .fd = sensor_sub_fd, .events = POLLIN }, }; while (true) { /* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */ int poll_ret = poll(fds, 1, 1000); .. 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); printf("[px4_simple_app] Accelerometer:\t%8.4f\t%8.4f\t%8.4f\n", (double)raw.accelerometer_m_s2[0], (double)raw.accelerometer_m_s2[1], (double)raw.accelerometer_m_s2[2]); } }
px4_simple_app 的完整代码如下：
/** * @file px4_simple_app.c * Minimal application example for PX4 autopilot * * @author Example User <mail@example.com> */ #include <nuttx/config.h> #include <unistd.h> #include <stdio.h> #include <poll.h> #include <uORB/uORB.h> #include <uORB/topics/sensor_combined.h> __EXPORT int px4_simple_app_main(int argc, char *argv[]); int px4_simple_app_main(int argc, char *argv[]) { printf("Hello Sky!\n"); /* subscribe to sensor_combined topic */ int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined)); /* one could wait for multiple topics with this technique, just using one here */ struct pollfd 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; while (true) { /* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */ int poll_ret = poll(fds, 1, 1000); /* handle the poll result */ if (poll_ret == 0) { /* this means none of our providers is giving us data */ printf("[px4_simple_app] Got no data within a second\n"); } 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) */ printf("[px4_simple_app] ERROR return value from poll(): %d\n" , 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); printf("[px4_simple_app] Accelerometer:\t%8.4f\t%8.4f\t%8.4f\n", (double)raw.accelerometer_m_s2[0], (double)raw.accelerometer_m_s2[1], (double)raw.accelerometer_m_s2[2]); } /* there could be more file descriptors here, in the form like: * if (fds[1..n].revents & POLLIN) {} */ } } return 0; }
编译app，通过下面的指令：
make
测试uORB订阅
运行更新后的程序。UORB app 可能已经在运行，但是重新运行一次也没关系，所以执行:
uorb start
目标请求中间人已经被激活，可以启动传感器：
sh /etc/init.d/rc.sensors
最后一步是启动应用，但是为什么语法不同？
px4_simple_app &
不同是，uorb和传感器应用作为后台程序运行Daemon(computing)。 这就允许在不失去对NuttShell控制的情况下，启动/停止其。假如忘了在行末输入& ，应用将会将现在传感器的值铺满显示屏：
[px4_simple_app] Accelerometer: 0.0483 0.0821 0.0332 [px4_simple_app] Accelerometer: 0.0486 0.0820 0.0336 [px4_simple_app] Accelerometer: 0.0487 0.0819 0.0327 [px4_simple_app] Accelerometer: 0.0482 0.0818 0.0323 [px4_simple_app] Accelerometer: 0.0482 0.0827 0.0331 [px4_simple_app] Accelerometer: 0.0489 0.0804 0.0328
由于不是后台程序，没有办法将其停止，只能让其运行，或者通过发起下面的命令重启自驾仪：
reboot
或者按压PX4FMU上的reset button.如何将应用变为后台程序可以参考this tutorial 。
Step 6: 速率受限的订阅
上一步最后一步是屏幕上铺满传感器数据。通常并不需要数据以最高速率刷新，试图保持最高速率，通常会使整个系统变慢。
API接口非常简单:
orb_set_interval(int handle, unsigned interval);
限制topic到1 Hz, 只需添加：
orb_set_interval(sensor_sub_fd, 1000);
到示例代码这行的下面(在line 55附近):
/* subscribe to sensor_combined topic */ int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));
屏幕将会以1Hz的速率方便的打印传感器数据。
Step 7: 发布数据
为了使用计算得到的输出，下一步是发布结果。假如我们使用了一个topic，mavlink app通过该topic转发到地面站，我们甚至可以查看结果。出于这个目的，来拦截姿态 topic。
接口非常简单，初始化topic结构，并通告（advertise)topic。
#include <uORB/topics/vehicle_attitude.h> .. /* advertise attitude topic */ struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); orb_advert_t att_pub_fd = orb_advertise(ORB_ID(vehicle_attitude), &att);
在主循环中，当其准备好后，发布信息：
orb_publish(ORB_ID(vehicle_attitude), att_pub_fd, &att);
修改后的完整示例代码为：
/**
* @file px4_simple_app.c
* Minimal application example for PX4 autopilot
*/
#include <nuttx/config.h>
#include <unistd.h>
#include <stdio.h>
#include <poll.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[])
{
printf("Hello Sky!\n");
/* subscribe to sensor_combined topic */
int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));
orb_set_interval(sensor_sub_fd, 1000);
/* advertise attitude topic */
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
int att_pub_fd = orb_advertise(ORB_ID(vehicle_attitude), &att);
/* one could wait for multiple topics with this technique, just using one here */
struct pollfd 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;
while (true) {
/* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */
int poll_ret = poll(fds, 1, 1000);
/* handle the poll result */
if (poll_ret == 0) {
/* this means none of our providers is giving us data */
printf("[px4_simple_app] Got no data within a second\n");
} 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) */
printf("[px4_simple_app] ERROR return value from poll(): %d\n"
, 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);
printf("[px4_simple_app] Accelerometer:\t%8.4f\t%8.4f\t%8.4f\n",
(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 */
att.roll = raw.accelerometer_m_s2[0];
att.pitch = raw.accelerometer_m_s2[1];
att.yaw = raw.accelerometer_m_s2[2];
orb_publish(ORB_ID(vehicle_attitude), att_pub_fd, &att);
}
/* there could be more file descriptors here, in the form like:
* if (fds[1..n].revents & POLLIN) {}
*/
}
}
return 0;
}运行最终的程序
需要一些基础应用:
uorb start sh /etc/init.d/rc.sensors mavlink start -d /dev/ttyS1 -b 115200
最终运行应用:
px4_simple_app &
假如启动了QGroundControl或者Mission Planner, 可以检查在实时绘图中显示的传感器数据(Main Menu: Main Widget →Realtime Plot)，这些数据反映了进程正在发送的数据。
Wrap-Up
这个教程涉及到了所有需要在PX4自驾仪应用上进行增量开发所需的内容。需要知道的是，在availablehere 这里有完整的topics列表，而且头文件都被写好，并作为参考。
该示例应用在 src/examples/px4_simple_app可用. 在app配置中，注释掉Firmware/makefiles/nuttx/config_px4fmu-v2_default.mk即可。
版权声明：本文为博主[翻译]文章，未经博主允许可以转载，注明博客出处：[http://blog.csdn.net/lkk05]
2万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
