PX4（PIXHAWK）源码开发人员文档（二）——Hello Sky（续）

Step 5: 订阅传感器数据

为了做有用的事情，应用需要订阅subscribe输入并发布publish输出 (e.g. 电机 或伺服指令). PX4平台真正的硬件抽象（true hardware abstraction ）开始有用——不需要与传感器驱动以任何方式相互作用，并且电路板或者传感器升级后不需要升级。

PX4中应用间独立的消息通道被称作topics. 在这里，感兴趣的是 sensor_combined topic, 其调用整个系统的同步传感器数据。

订阅到topic非常快和干净：

#include 
..
int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));

sensor_sub_fd 是一个文件描述符，非常高效的执行一块等待新的数据的数据块。当前的进程进入休眠，当有新的数据可用时，由调度程序自动地唤醒，在等待的时候，不占用任何CPU周期。为实现这点，使用poll()，调用 POSIX system 。

将poll() 添加到subscription看上去像：

#include 
#include 
..
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 
 */
 
#include 
#include 
#include 
#include 
 
#include 
#include 
 
__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 
..
/* 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 
#include 
#include 
#include 
 
#include 
#include 
#include 
 
__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]