PX4代码学习系列博客(5)——在px4中添加自己的模块

怎么在px4中添加自己的模块

在“px4固件目录结构和代码风格”这一节，曾经说过NuttX是一个实时的嵌入式系统，上面可以像windows那样运行程序。那既然是应用程序，那我们应该也能写一些可以在NuttX上运行的程序。这一节介绍怎么在px工程中添加自己的模块。

说明：本文用到的固件版本是1.6.5dev。操作系统是Ubuntu16.04。

程序修改具体步骤

凡是程序入门的教程，都是经典的“hello world”，所以我们新建一个可以输出“hello world”的模块。为了复习上一节的uORB通信，我们可以在“hello world”后把一些传感器的数据输出来。

首先在Firmware\src\modules下新建文件夹，起名gaoweijintask。在下面新建文件gaoweijintask.c和CMakeLists.txt。
其中CMakeLists.txt中的内容：

px4_add_module(MODULE modules__gaoweijintaskMAIN gaoweijintaskSTACK_MAIN 200COMPILE_FLAGSSRCSgaoweijintask.c)

第二行代表这个模块在目录modules\gaoweijintask下，注意中间是两个下划线。

第三行代表这个模块的主函数是test，在代码中是gaoweijintask_main。

第五行代表这个模块用到的源文件。

gaoweijintask.c中的内容：

#include <stdio.h>__EXPORT int gaoweijintask_main(int argc, char *argv[]);int gaoweijintask_main(int argc, char *argv[])
{printf("Hello World!\n");return 1;
}

这个时候还需要在编译的配置中添加这个模块，在Firmware\cmake\configs下新建我们的配置文件nuttx_px4fmu-v2_gaoweijin.cmake，其中内容如下：

include(configs/nuttx_px4fmu-v2_default)list(APPEND config_module_listmodules/gaoweijintask)

这代表在默认的配置下加入新模块gaoweijintask，而且这个模块的路径是modules/gaoweijintask。

执行编译命令

make px4fmu-v2_gaoweijin

等待编译完成后下载到pixhawk中。

效果测试

为了测试我们的程序，我们需要用到nuttx系统的shell，nsh。

目前的px4固件版本，用putty查看nsh控制台时，会出现乱码。网上有说要拔掉SD卡，但是依然乱码。解决办法是:
将 /Firmware/ROMFS/px4fmu_common/init.d/rcS 文件中的

# Start MAVLinkmavlink start -r 800000 -d /dev/ttyACM0 -m config -x

替换为

# Start USB shell if no microSD present, MAVLink elseif [ $LOG_FILE == /dev/null ]then# Try to get an USB consolenshterm /dev/ttyACM0 &elsemavlink start -r 800000 -d /dev/ttyACM0 -m config -xfi

然后在进nsh的时候拔掉sd卡。进nsh我们需要用到一个工具，putty。

Ubuntu安装putty命令

sudo apt-get install putty

打开putty，选择串口/dev/ttyACM0,波特率选择57600。
输入help回车，查看可用的命令，会发现有一条gaoweijintask，这就是我们新添加的模块。

输入gaoweijintask start 并回车，显示Hello World!。这说明我们的模块添加成功了。

添加新模块实现输出加速度计的值

接下来实现一个简单的应用，通过nsh把加速度计的值打印出来。利用上一节的获取uORB消息的方法，获取加速度计的值。替换gaoweijintask.c中的内容为：

#include <nuttx/config.h>
#include <unistd.h>
#include <stdio.h>
#include <poll.h>#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>__EXPORT int gaoweijintask_main(int argc, char *argv[]);int gaoweijintask_main(int argc, char *argv[])
{printf("Hello World!\n");/*订阅sensor_combined 主题*/int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));/*一个应用可以等待多个主题，在这里只等待一个主题*/struct pollfd fds[] = {{ .fd = sensor_sub_fd,   .events = POLLIN },/* 这里可以添加更多的文件描述符；* { .fd = other_sub_fd,   .events = POLLIN },*/};int error_counter = 0;while (true) {/*poll函数调用阻塞的时间为1s*/int poll_ret = poll(fds, 1, 1000);/*处理poll返回的结果 */if (poll_ret == 0) {/* 这表示时间溢出了，在1s内没有获取到发布者的数据 */printf("[gaoweijin_task] Got no data within a second\n");} else if (poll_ret < 0) {/* 出现问题 */if (error_counter < 10 || error_counter % 50 == 0) {/* use a counter to prevent flooding (and slowing us down) */printf("[gaoweijin_task] ERROR return value from poll(): %d\n", poll_ret);}error_counter++;} else {if (fds[0].revents & POLLIN) {/*从文件描述符中获取订阅的数据*/struct sensor_combined_s raw;/* copy sensors raw data into local buffer */orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw);printf("[gaoweijin_task] 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]);}/* 如果有更多的文件描述符，可以这样：* if (fds[1..n].revents & POLLIN) {}*/}}return 0;
}

在putty中输入gaoweijintask并回车，得到：