uORB发布订阅实例

PX4SITL仿真——uORB实例

飞控串口读取外部传感器数据：飞控开启一个进程读取外部传感器数据，发布一个uORB主题；另一个进程订阅前一个进程发布的主题，订阅到的主题通过mavlink消息发送到地面站（QGroundControl）。

1 新增一个自定义uORB主题

在源码 Firmware/msg 文件夹下是飞控所有的uORB主题

可以看到vehicle_global_position.msg是全球位置，vehicle_attitude.msg是姿态消息。姿态的成员变量包括横滚 roll, 俯仰 pitch,偏航 yaw, 横滚速度，俯仰速度等相关姿态的数据。相关的加速度计，磁力计，陀螺仪经过算法滤波整合之后会发布姿态数
据，而姿态控制进程会订阅这个 vehicle_attitude.msg 主题。

1.1 自定义主题

在msg文件夹下添加一个具体的消息：
topic_name.msg,文件里的结构体成员自己定义：

然后修改msg/CMakeList.txt ：将自定义的 topic_name.msg 名字写入到该文件中。在编译源码的时候会自动写好头文件。之后make 就可以在Firmware/uORB/topics下看到我们自定义的消息头文件了。

1.2 订阅和发布示例

1、Create a new directory Firmware/src/examples/px4_simple_app.
2、Create a new C file in that directory named px4_simple_app.c:
示例px4_simple_app.c内容如下：

/**
 * @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!");

    /* subscribe to sensor_combined topic */
    int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));
    /* limit the update rate to 5 Hz */
    orb_set_interval(sensor_sub_fd, 200);

    /* 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;

    for (int i = 0; i < 5; i++) {
        /* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */
        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 (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("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
                */
                att.q[0] = raw.accelerometer_m_s2[0];
                att.q[1] = raw.accelerometer_m_s2[1];
                att.q[2] = raw.accelerometer_m_s2[2];

                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;
}

3、Create and open a new cmake definition file named CMakeLists.txt. Copy in the text below:

px4_add_module(
    MODULE examples__px4_simple_app
    MAIN px4_simple_app
    STACK_MAIN 2000
    SRCS
        px4_simple_app.c
    DEPENDS
    )

1.4 编译订阅发布进程

PX4 SITL (Simulator): Firmware/boards/px4/sitl/default.cmake
在default.cmake中添加模块示例，此处的EXAMPLE对应创建的examples/px4_simple_app模块的位置。

编译命令：
make px4 gazebo

1.5 使用进程

uORB进行消息的订阅与发布示例参考：https://dev.px4.io/en/apps/hello_sky.html#subscribing-to-sensor-data