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__gaoweijintask
    MAIN gaoweijintask
    STACK_MAIN 200
    COMPILE_FLAGS
    SRCS
        gaoweijintask.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_list
    modules/gaoweijintask
    )

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

执行编译命令

make px4fmu-v2_gaoweijin

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

效果测试

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

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

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

替换为

# Start USB shell if no microSD present, MAVLink else
    if [ $LOG_FILE == /dev/null ]
    then
        # Try to get an USB console
        nshterm /dev/ttyACM0 &
    else
        mavlink start -r 800000 -d /dev/ttyACM0 -m config -x
    fi

然后在进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并回车，得到：