Hello Sky你的第一个PX4应用程序

Hello Sky你的第一个PX4应用程序

Comments 0 Comment

课程介绍

本节课主要在 PX4 的框架下学习以下内容：

  • 如何快速开发一个简单的 PX4 模块。
  • 如何编写一个模块查询 IMU 数据。
  • uORB 订阅发布机制的简单使用。

课程准备

在这堂课之前，我们需要准备以下内容:

  • PX4 模拟器环境或兼容 PX4 的飞行控制器硬件一套。
  • 基于 WSL2 -Ubuntu 环境下的一套完整的 PX4 开发工具链。
  • 完整的 PX4 V1.14.3 版本的源码，包含相应的子模块。
  • 删除 PX4_AUTOPILOT/src/examples/px4_simple_app 文件夹。

Hello World

在我们学习任何一门开发语言初期，通常是从输出一句 Hello World! 开始的。在这节课程中，我们也将会创建一个最简单的飞控应用，通过飞控输出字符串 Hello World! 到地面站终端；正所谓万丈高楼平地起，那么下面将开启我们飞控的第一行代码，这其中包括一个 c 文件和 cmake 的一些定义（其中：cmake 的作用是告知主控芯片如何构建程序，我们可以先不用关心 cmake 的其他特性，先就按照课程一步一步做下去，后续有兴趣的同学可以自行拓展学习）。

添加代码

  1. 创建应用程序的文件夹：PX4-Autopilot/src/examples/px4_simple_app

  2. 在文件夹下创建 c 文件，并命名为 px4_simple_app.c

  3. 拷贝如下的默认版权信息，放在上述文件代码的最前处，这里大概包含了以下内容：

    a. 版权声明：PX4 源码通常在文件开头包含版权声明，表明该软件的版权归属于 PX4 开发团队。

    b. 许可证：PX4 主要采用 BSD 3-clause 许可证。这是一种宽松的开源许可证，允许用户自由使用、修改和分发代码，只要保留原始版权声明。

    c. 免责声明：声明中通常包含免责条款，表明软件按”原样”提供，不提供任何明示或暗示的保证。

    d. 贡献者协议：可能会提到贡献者协议，说明向 PX4 项目提交代码的开发者需要同意的条款。

    e. 其他信息：有时还会包含项目网站链接、主要维护者信息等。

/****************************************************************************
 *
 *   Copyright (c) 2012-2019 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.
 *
 ****************************************************************************/
  1. 然后，标注好开发者信息，也就是你的名字和邮箱，这方便后续开发者遇到问题，方便询问等。
/**
 * @file px4_simple_app.c
 * Minimal application example for PX4 autopilot
 *
 * @author Example User <mail@example.com>
 */

#include <px4_platform_common/log.h>

__EXPORT int px4_simple_app_main(int argc, char *argv[]);

int px4_simple_app_main(int argc, char *argv[])
{
 PX4_INFO("Hello World!");
 return OK;
}

其中：__EXPORT int px4_simple_app_main(int argc, char *argv[]) 函数的具体功能如下：

  • 程序入口：作为 PX4 飞行控制软件中的一个应用程序的主入口点。
  • 参数接收：int argc: 表示命令行参数的数量，包括程序名称在内的参数总数。char *argv[]: 是一个字符指针数组，存储了所有命令行参数的具体值，其中argv[0]通常为程序名称。
  • 执行流程：在函数内部，可能进行以下操作： a. 初始化应用程序所需的资源或模块。 b. 解析传入的命令行参数，根据不同的参数执行相应的功能或配置。 c. 启动应用程序的主要功能，如数据采集、传感器读取、控制逻辑执行等。
  • 返回值：函数返回一个 int 类型的值，通常用于指示程序执行的状态或结果，例如：0 表示成功，非 0 值表示错误代码。
  • 主函数必须以 \_main\_ 的方式命名，并且使用关键字 __EXPORT 导出。

在程序的第 14 行中，PX4_INFO() 相当于我们以前学习 C 语言中的 printf() 函数，需要使用的时候需要包含头文件px4_platform_common/log.h。此外还有以下几种形式：

打印等级 使用场景
PX4_INFO 用于输出正常的信息
PX4_ERR 用于输出程序错误时的信息
PX4_DEBUG 用于调试阶段的一些输出

通过上述方式的输出信息，都会记录在飞控的 ULog 文件中，我们也可以通过一些工具（如：Flight Review, flightplot, PlotJuggler）来查看。

添加CMakeList

创建 cmake 文件，并命名为 CMakeLists.txt

############################################################################
#
#   Copyright (c) 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.
#
############################################################################
px4_add_module(
 MODULE examples__px4_simple_app
 MAIN px4_simple_app
 SRCS
  px4_simple_app.c
 DEPENDS
 )

px4_add_module() 方法根据模块描述构建静态库：

  • MODULE：模块的唯一名称（用于区分其它不同的模块），需要和文件名一致，按照惯例，模块名称的前缀是父目录。
  • MAIN：模块功能的入口点，相当于模块的main函数。它向操作系统 NuttX 注册命令，以便可以从 PX4 shell 或SITL 控制台中调用它。

添加Kconfig（可选）

创建一个名为 Kconfig 的文件，并定义要命名的符号（请参阅 Kconfig 命名约定）并复制粘贴以下文本:

menuconfig EXAMPLES_PX4_SIMPLE_APP
 bool "px4_simple_app"
 default n
 ---help---
  Enable support for px4_simple_app

在这里，我们将简单的介绍一下什么是 Kconfig ? 它的作用是什么 ?

在 PX4 中 Kconfig 是一个配置系统，它源自 Linux 内核的配置机制。它在 PX4 固件中扮演着重要的角色。以下是 Kconfig的主要作用和特点：

  • 配置管理：Kconfig 允许开发者定义和管理各种配置选项，用于控制编译过程中哪些功能或模块被包含或不包含。

  • 模块化构建：通过 Kconfig，可以实现 PX4 固件的模块化编译，使得不同的硬件平台或应用场景可以选择性地包含各自所需的功能。

  • 依赖管理：Kconfig 可以定义配置选项之间的依赖关系，确保相互依赖的功能被正确启用或禁用。

  • 用户界面：Kconfig 通常与 menuconfig 等工具配合使用，提供图形化界面，方便用户选择和配置各种选项。

  • 条件编译：基于 Kconfig 的选项，可以在代码中使用条件编译指令，实现灵活的代码组织。

  • 减小固件大小：通过选择性地包含功能，可以有效减小最终固件的大小，适应不同的硬件资源限制。

  • 跨平台支持：帮助 PX4 在不同的硬件平台上实现更好的可移植性和适配性。

因此，开发者合理的使用 Kconfig，可以更灵活地管理复杂的固件配置；同时也使得最终用户能够根据自己的需求定制 PX4 固件，这对于支持多样化的无人机和机器人平台非常重要。

有兴趣的同学可以通过以下途径获取 Kconfig 更多的相关内容：

  1. PX4 官方文档：PX4 开发者指南中包含 Kconfig 相关的信息。网址：https://docs.px4.io/main/en/

  2. PX4 GitHub 仓库：查看 PX4 源码中的 Kconfig 文件可以直接了解其使用方式。网址：https://github.com/PX4/PX4-Autopilot

  3. Kconfig 语言文档：虽然不是 PX4 特有，但了解 Kconfig 语言本身很有帮助。网址：https://www.kernel.org/doc/Documentation/kbuild/kconfig-language.txt

  4. PX4 开发者论坛：关于 Kconfig 使用的讨论。网址：https://discuss.px4.io/

  5. Linux Kconfig 文档：由于 PX4 的 Kconfig 源自 Linux，了解 Linux 的 Kconfig 也很有帮助。网址：https://www.kernel.org/doc/Documentation/kbuild/kconfig.txt

编译代码

应用程序现在已经完成。为了运行它，首先需要确保将它加入到对应的飞控板的配置文件中（也就是 *.px4board 文件）。

下面，我们介绍几种常用的配置文件：

  • PX4 SITL (Simulator): PX4-Autopilot/boards/px4/sitl/default.px4board
  • Pixhawk v1/2: PX4-Autopilot/boards/px4/fmu-v2/default.px4board
  • Pixracer (px4/fmu-v4): PX4-Autopilot/boards/px4/fmu-v4/default

其中：PX4-Autopilot/boards/px4/sitl/default.px4board的内容如下：

sitl_default_px4board 图 1.1 px4_v6c default.px4board

这些 *.px4board 文件可以在 PX4-Autopilot/boards 路径下找到。可以看出这里面每行都是以 CONFIG_ 开头，后面是模块名称，后面是 y/n，y 表示启用，n 表示不启用。因此，为了编译我们刚才写的代码，我们就需要将添加的 px4_simple_app 模块设置为 y。

我们在 PX4-Autopilot 文件夹下打开终端，输入指令：

make px4_fmu-v6c_default boardconfig 如果您使用的是雷迅 v5 版本的飞控，仅需要将上述的 px4_fmu-v6c 改为 px4_fmu-v5 即可。

然后我们将看到如下的界面：

v6c_boardconfig 图 1.2 v6c_boardconfig

将 px4_simple_app 模块选中（按空格键选中）并保存退出，点击键盘上的 S 键进行配置保存。

save_cfg 图 1.3 px4_v6c 保存配置

将 px4_simple_app 模块选中并保存退出。再次查看 px4_v6c 的 default.px4board 文件，可以看到最后一行将我们编写的程序配置为 y 了：

CONFIG_SYSTEMCMDS_TOP=y
CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
CONFIG_SYSTEMCMDS_TUNE_CONTROL=y
CONFIG_SYSTEMCMDS_UORB=y
CONFIG_SYSTEMCMDS_VER=y
CONFIG_SYSTEMCMDS_WORK_QUEUE=y
CONFIG_EXAMPLES_PX4_SIMPLE_APP=y

然后，编译代码，这里针对不同的情况，例举相应的编译指令：

board 编译指令
Gazebo Simulator make px4_sitl gz_standard_vtol
Pixhawk v6c px4_fmu-v6c_default
Pixhawk v5 make make px4_fmu-v5_default

编译好的固件的路径在：PX4_Autopilot/build/px4_fmu-v6c_default

firmware_path 图 1.4 固件路径

这里我们可以看到几种不同格式的固件，它们的区别如下：

后缀名 介绍 使用场景
.px4 这是 PX4 的主要固件文件，通常用于飞行控制器的固件更新，包含了完整的飞行控制软件。 适用于一般用户进行固件更新。
.bin 二进制文件格式，通常用于存储固件的二进制数据，某些硬件可能需要这种格式进行更新。 适用于特定硬件或开发者使用。
.elf 可执行和可链接格式，主要用于开发和调试。它包含了调试信息。 适合开发者在调试过程中使用。

测试代码

我们通过上述步骤已经编译好了我们的 px4_simple_app 模块，现在我们通过硬件来实操一下。

  1. 首先，将编译好的固件烧录到飞控中。可以使用如下指令：

make px4_fmu-v6c_default

将我们编译好的固件烧录到飞控中。当烧录完成，您将看到如下界面：

load_fimware 图 1.5 烧录固件
  1. 连接控制台，我们需要通过串口线或者 USB 线连接控制台，按下 Enter 键进入终端。在终端中，我们可以输入help 指令，然后敲击回车，可以看到如下界面：

hello_help

图 1.6 终端帮助指令

注意，px4_simple_app 现在已经是可用命令了。我们输入 px4_simple_app，然后回车:

hello_world

图 1.7 输出 Hello World

至此，应用程序现在已经正确地注册到了系统中，并且可以正常使用。此外，您还可以尝试在 SITL 环境下，尝试上述的步骤，脱离硬件来实现一次测试。

uORB初体验

现在，我们可以更进一步，获取传感器的数据，以 SensorCombined 主题为例。该主题中存放的是 IMU 的数据。

PX4 硬件抽象的好处在这里发挥了作用！不需要以任何方式与传感器驱动程序进行交互，如果飞控板或传感器更新，也不需要修改您的应用程序。也就是说我们在这里获取传感器的数据，不需要修改传感器那边的代码。

订阅一个主题也很简单，在文件的头部引入需要订阅数据的头文件：

/**
 * @file px4_simple_app.c
 * Minimal application example for PX4 autopilot
 *
 * @author Example User <mail@example.com>
 */

#include <px4_platform_common/log.h>
#include <uORB/topics/sensor_combined.h>

int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));

__EXPORT int px4_simple_app_main(int argc, char *argv[]);

int px4_simple_app_main(int argc, char *argv[])
{
 PX4_INFO("Hello World!");
 return OK;
}

其中：sensor_sub_fd 是一个主题句柄，可用于执行阻塞等待新数据。当前线程进入睡眠状态，一旦有新数据可用，调度器就会自动唤醒它，在等待期间不会消耗任何 CPU 时间。为此，我们使用 poll() 函数调用。这里介绍一下 px4_poll 函数：

#define px4_poll  _GLOBAL poll
int poll(struct pollfd *fds, nfds_t nfds, int timeout);

_GLOBAL poll 表示全局作用域下的 poll 函数。可以看出其实 px4_poll() 函数也就是 posix 中的 poll() 函数：

参数说明：

  • fds：传入传出参数，指向 struct pollfd 类型数组的首元素，每个数组元素指定一个描述符以及对其关心的状态，关于这个结构体的说明在本小节后面阐述。

  • nfds：指明 fds 指向的数组元素个数。

  • timeout：该参数指定 poll 阻塞等待文件描述符就绪的毫秒数。这个参数有三种可能：

    1. timeout 设置为负数：一直阻塞等待，直到有描述符准备就绪；

    2. timeout 设置为 0：不等待，检查描述符后直接返回。

    3. timeout 设置为正数：阻塞等待timeout设置的毫秒数，期间有描述符准备就绪就返回，没有就等到时间结束返回。

  • 返回值：成功返回已准备就绪的描述符个数；超时返回0；失败返回-1。

具体的代码如下：

/****************************************************************************
 *
 *   Copyright (c) 2012-2019 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 <poll.h>
#include <px4_platform_common/log.h>
#include <px4_platform_common/posix.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[])
{
 PX4_INFO("Hello World!");

 int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));

    /* 该方式可以等待多主题的数据，此处只等待一组主题的数据 */
 px4_pollfd_struct_t fds[] = {
  { .fd = sensor_sub_fd,   .events = POLLIN },
 };

 int error_counter = 0;

 while (true) {
        /* 每隔 1s 看看传感器主题的数据是否更新 */
  int poll_ret = px4_poll(fds, 1, 1000);

  /* 处理轮训结果 */
  if (poll_ret == 0) {
   /* 过了 1s 都没收到数据 */
   PX4_ERR("Got no data within a second");

  } else if (poll_ret < 0) {
   /* 未能成功返回已准备就绪的描述符个数 */
   if (error_counter < 10 || error_counter % 50 == 0) {
    /* 降低打印频率，防止打印的太快 */
    PX4_ERR("ERROR return value from poll(): %d", poll_ret);
   }

   error_counter++;

  } else {
   if (fds[0].revents & POLLIN) {
    /* 为第一个文件描述符获得的数据 */
    struct sensor_combined_s raw;
    /* 将传感器原始数据复制到本地缓冲区 */
    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]);
   }
  }

  return OK;
 }
}

再次编译代码并烧录进飞控。

订阅数据

最后一步是通过在 nsh shell 中输入以下命令，启动任务:

px4_simple_app

将会显示以下数据：

sub_acc

图 1.8 订阅加速度数据

现在您可以模仿上述教程，尝试订阅 SensorGyro 主题中的数据，也就是陀螺仪的数据，并通过终端查看数据。

发布数据

在飞控中，有很多模块，有模块需要订阅上述的 SensorCombined 主题，所以也一定有对应的模块在发布 SensorCombined 主题。

我们在这里不使用 SensorCombined 主题来作为发布数据的实操，采用 VehicleAttitude 主题来作为发布数据。是因为Mavlink 会把该主题的数据下发给地面站，更方便我们查阅。如果学了后续高阶的 Mavlink 的相关知识，您也可以把SensorCombined 主题下发给地面站显示。

这个案例很简单，代码如下：

/****************************************************************************
 *
 *   Copyright (c) 2012-2019 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_platform_common/px4_config.h>
#include <px4_platform_common/tasks.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/log.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 World!");

 /* 订阅 sensor_combined 主题 */
 int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));
 /* 限制更新频率为 5Hz */
 orb_set_interval(sensor_sub_fd, 200);

 /* 公告 vehicle_attitude 主题 */
 struct vehicle_attitude_s att;
 memset(&att, 0, sizeof(att));
 orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);

 /* 可以订阅一个主题多组数据，这里只订阅一组；举例，有3个imu发布 sensor_accel 主题，那么这个数据就有三组 */
 px4_pollfd_struct_t fds[] = {
  { .fd = sensor_sub_fd,   .events = POLLIN },
  /* 如果要订阅多组，可以这样写：
   * { .fd = other_sub_fd,   .events = POLLIN },
   */
 };

 int error_counter = 0;

 for (int i = 0; i < 5; i++) {
  /* 每隔 1s 看看传感器主题的数据是否更新 */
  int poll_ret = px4_poll(fds, 1, 1000);

  /* 处理轮训结果 */
  if (poll_ret == 0) {
   /* 过了 1s 都没收到数据 */
   PX4_ERR("Got no data within a second");

  } else if (poll_ret < 0) {
   /* 未能成功返回已准备就绪的描述符个数 */
   if (error_counter < 10 || error_counter % 50 == 0) {
    /* 降低打印频率，防止打印的太快 */
    PX4_ERR("ERROR return value from poll(): %d", poll_ret);
   }

   error_counter++;

  } else {

   if (fds[0].revents & POLLIN) {
    /* 获取该主题的第一个文件描述符 */
    struct sensor_combined_s raw;
    /* 将传感器原始数据复制到本地缓冲区 */
    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]);

    /* 将加速度的数据赋值给姿态，没有任何意义，这里只是演示作用 */
    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);
   }

   /* 如果要处理该主题的其它数据，可以这样写:
    * if (fds[1..n].revents & POLLIN) {}
    */
  }
 }

 PX4_INFO("exiting");

 return 0;
}

该函数 px4_simple_app_main 主要功能如下：

  1. 初始化与订阅：
    • 打印 “Hello World!” 信息。
    • 订阅 sensor_combined 主题，用于接收传感器数据。
    • 设置传感器数据更新频率为 5 Hz。
  2. 发布态度信息：
    • 初始化 vehicle_attitude 结构体，并创建一个发布者 att_pub。
    • 将 vehicle_attitude 的初始值发布出去。
  3. 轮询传感器数据：
    • 使用 px4_poll 轮询 sensor_combined 数据，共执行 5 次循环。
    • 每次轮询等待 1 秒钟。
  4. 处理轮询结果：
    • 无数据：如果在 1 秒内没有收到数据，打印错误信息。
    • 错误返回：如果 px4_poll 返回负值，记录错误次数并打印错误信息。
    • 收到数据：如果收到数据，执行以下操作：
      • 复制传感器数据到本地缓冲区。
      • 打印加速度计数据。
      • 将加速度计数据作为四元数存储到 vehicle_attitude 结构体，并发布出去。
  5. 退出程序：
    • 循环结束后，打印退出信息并返回 0。

同样的，重复以上的make和烧录的步骤，在终端中运行 px4_simple_app，您也可以在 mavlink_inspector 中看到数据。

pub_data1 图 1.9 查看数据 pub_data2 图 1.10 查看姿态数据

总结

本节课涵盖了开发基本 PX4 自动驾驶应用程序所需的一切。此外，uORB的消息定义通常在PX4的源代码中，具体路径为 msg 文件夹下。每个消息都有相应的 .msg 文件，里面定义了字段及其类型。

发表回复

您的邮箱地址不会被公开。 必填项已用 * 标注