pixhawk/px4如何获取及使用传感器数据

pixhawk/px4如何获取及使用传感器数据

第一步:读取传感器数据

上一篇博文已经介绍了如何给pixhawk/px4创建一个应用程序，现在我们在上一个应用程序的基础上使用传感器数据。应用程序为了实现一些有用的功能，需要订阅输入和发布输出(比如电机或舵机输出的命令)。注意在这里，PX4平台真正的硬件抽象的概念在这里体现---当硬件平台或者传感器升级更新，你完全不需要和传感器驱动打交道,也不需要更新你的应用程序或者更新传感器驱动程序。在PX4中，应用程序间发送信息的独立通道叫做“topics”，在本教程中，我们关心的话题是“多传感器间的uORB消息机制”（sensor_combinedtopic）。这些消息机制使得整个系统能够同步传感器数据。订阅一个消息十分快速简洁：

#include<uORB/topics/sensor_combined.h>

..

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 <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 <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[])
{
/* 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

[/code]

注意，

make

命令要在

Firware

目录下执行，因为

Makfile

文件在该目录下，结果如下图所示。

 [/code]

第二步：测试uORB消息读取机制

最后一步，运行你的应用程序，并且切换到后台应用。注意这需要把程序重新编译上传到飞控板，拔掉sd卡，然后连接nsh控制台，运行以下命令。

px4_simple_app　&

你的应用程序会向串口输出当前传感器的值：

[px4_simple_app]Accelerometer: 0.04830.08210.0332

[/code]

[px4_simple_app]Accelerometer: 0.04860.08200.0336

[/code]

[px4_simple_app]Accelerometer: 0.04870.08190.0327

[/code]

[px4_simple_app]Accelerometer: 0.04820.08180.0323

[/code]

[px4_simple_app]Accelerometer: 0.04820.08270.0331

[/code]

[px4_simple_app]Accelerometer: 0.04890.08040.0328

[/code]

我测试的结果如下图所示

 [/code]它会在输出5次数据后退出。接下来会介绍如何编写一个能通过命令行控制的后台应用。

第三步：打印数据

　　　为了能获取到计算后的数据，下一步就是“打印”这些结果。如果我们知道某一个消息是使用mavlink协议转发给地面控制站的，我们可以通过这个消息去查看结果。例如我们通过这个方法来获得高度信息的消息。接口非常简单--初始化消息的结构体，然后广播这条消息：

#include<uORB/topics/vehicle_attitude.h>

[/code]

..

[/code]

/*advertise attitude topic */

[/code]

structvehicle_attitude_s att;

[/code]

memset(&att,0, sizeof(att));

[/code]

orb_advert_tatt_pub_fd = orb_advertise(ORB_ID(vehicle_attitude), &att);

[/code]在主循环中，当消息准备好时，打印这条消息。

orb_publish(ORB_ID(vehicle_attitude),att_pub_fd, &att);

[/code]修改后的完整的示例代码如下：

/****************************************************************************** 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 <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 <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[]){/* 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

[/code]　如果打开地面站QGroundControl，你就能通过实时绘图(Tools-> Analyze)来获得实时的传感器数据。

注意事项：

１．打开nsh前需要拔掉sd卡．２．连接nsh时不要地面站qgc运行．３．连接地面站时需要插上sd卡．４．运行编译命令是要在含有Makefile的．．．／Firware/目录下进行．参考资料First App Tutorial (Hello Sky)