pixhawk/px4如何获取及使用传感器数据
pixhawk/px4如何获取及使用传感器数据
第一步:读取传感器数据
上一篇博文已经介绍了如何给pixhawk/px4创建一个应用程序，现在我们在上一个应用程序的基础上使用传感器数据。
应用程序为了实现一些有用的功能，需要订阅输入和发布输出(比如电机或舵机输出的命令)。注意在这里，PX4平台真正的硬件抽象的概念在这里体现---当硬件平台或者传感器升级更新，你完全不需要和传感器驱动打交道,也不需要更新你的应用程序或者更新传感器驱动程序。
在PX4中，应用程序间发送信息的独立通道叫做“topics”，在本教程中，我们关心的话题是“多传感器间的uORB消息机制”（sensor_combinedtopic）。这些消息机制使得整个系统能够同步传感器数据。
订阅一个消息十分快速简洁：
#include
..
int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined)); “sensor_sub_fd”是一个消息句柄，能够十分有效处理新数据的到来之前的延迟等待。当前线程会休眠，直到新的传感器数据到来时，被调度器唤醒，并且在等待时不需要占用任何的CPU时间。为了实现这个功能，我们使用poll()函数即POSIX系统调用。
在消息读取中加入“poll()”机制，完整程序如下
/****************************************************************************
*
* Copyright (c) 2012-2015 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
*/
#include
#include
#include
#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[])
{
/* subscribe to sensor_combined topic */
int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));
orb_set_interval(sensor_sub_fd, 1000);
/* 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("[px4_simple_app] 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("[px4_simple_app] 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_WARN("[px4_simple_app] 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]);
}
}
}
PX4_INFO("exiting");
return 0;
} 编译应用程序：
make
注意，make命令要在Firware目录下执行，因为Makfile文件在该目录下，结果如下图所示。
第二步：测试uORB消息读取机制
最后一步，运行你的应用程序，并且切换到后台应用。注意这需要把程序重新编译上传到飞控板，拔掉sd卡，然后连接nsh控制台，运行以下命令。
px4_simple_app &你的应用程序会向串口输出当前传感器的值：
[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
我测试的结果如下图所示
它会在输出5次数据后退出。接下来会介绍如何编写一个能通过命令行控制的后台应用。
第三步：打印数据
为了能获取到计算后的数据，下一步就是“打印”这些结果。如果我们知道某一个消息是使用mavlink协议转发给地面控制站的，我们可以通过这个消息去查看结果。例如我们通过这个方法来获得高度信息的消息。
接口非常简单--初始化消息的结构体，然后广播这条消息：
#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);修改后的完整的示例代码如下：
/****************************************************************************
*
* Copyright (c) 2012-2015 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
*/
#include
#include
#include
#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[])
{
/* 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));
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("[px4_simple_app] 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("[px4_simple_app] 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_WARN("[px4_simple_app] 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 */
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, &att);
}
/* there could be more file descriptors here, in the form like:
* if (fds[1..n].revents & POLLIN) {}
*/
}
}
PX4_INFO("exiting");
return 0;
} 第四步：运行整个示例
在nsh终端运行你的应用程序：
px4_simple_app
如果打开地面站QGroundControl，你就能通过实时绘图(Tools-> Analyze)来获得实时的传感器数据。
注意事项：
１．打开nsh前需要拔掉sd卡．
２．连接nsh时不要地面站qgc运行．
３．连接地面站时需要插上sd卡．
４．运行编译命令是要在含有Makefile的．．．／Firware/目录下进行．
参考资料
First App Tutorial (Hello Sky)