Pixhawk—添加一个自定义传感器—超声波（串口方式）

Pixhawk添加一个自定义传感器超声波串口方式

Pixhawk—添加一个自定义传感器—超声波（串口方式）

1 说明

首先超声波模块是通过串口方式发送（Tx）出数据，使用的模块数据发送周期为100ms，数据格式为：

R0034 R0122 R0122 R0046 R0127 R0044 R0044 R0125 R0034 R0037 R0041 R0122 R0122 .....

则可以通过Pixhawk板上的串口来接收（Rx）数据，即将超声波的Tx接口连接到Pixhawk板上的Rx接口。
Pixhawk板上串口说明：
这里写图片描述
测试使用Pixhawk板上TELEM2接口的USART2，对应的Nuttx UART设备文件尾/dev/ttyS2：

2 读取数据测试

步骤：

在Firmware/src/modules中添加一个新的文件夹，命名为rw_uart
在rw_uart文件夹中创建module.mk文件，并输入以下内容：
- MODULE_COMMAND = rw_uart
- SRCS = rw_uart.c
在rw_uart文件夹中创建rw_uart.c文件
注册新添加的应用到NuttShell中。Firmware/makefiles/nuttx/config_px4fmu-v2_default.mk文件中添加如下内容：
- MODULES += modules/rw_uart

rw_uart.c

<span class="hljs-preprocessor">#include <stdio.h></span>
<span class="hljs-preprocessor">#include <termios.h></span>
<span class="hljs-preprocessor">#include <unistd.h></span>
<span class="hljs-preprocessor">#include <stdbool.h></span>
<span class="hljs-preprocessor">#include <errno.h></span>
<span class="hljs-preprocessor">#include <drivers/drv_hrt.h></span>

__EXPORT <span class="hljs-keyword">int</span> rw_uart_main(<span class="hljs-keyword">int</span> argc, <span class="hljs-keyword">char</span> *argv[]);

<span class="hljs-keyword">static</span> <span class="hljs-keyword">int</span> uart_init(<span class="hljs-keyword">char</span> * uart_name);
<span class="hljs-keyword">static</span> <span class="hljs-keyword">int</span> set_uart_baudrate(<span class="hljs-keyword">const</span> <span class="hljs-keyword">int</span> fd, <span class="hljs-keyword">unsigned</span> <span class="hljs-keyword">int</span> baud);

<span class="hljs-keyword">int</span> set_uart_baudrate(<span class="hljs-keyword">const</span> <span class="hljs-keyword">int</span> fd, <span class="hljs-keyword">unsigned</span> <span class="hljs-keyword">int</span> baud)
{
    <span class="hljs-keyword">int</span> speed;

    <span class="hljs-keyword">switch</span> (baud) {
        <span class="hljs-keyword">case</span> <span class="hljs-number">9600</span>:   speed = B9600;   <span class="hljs-keyword">break</span>;
        <span class="hljs-keyword">case</span> <span class="hljs-number">19200</span>:  speed = B19200;  <span class="hljs-keyword">break</span>;
        <span class="hljs-keyword">case</span> <span class="hljs-number">38400</span>:  speed = B38400;  <span class="hljs-keyword">break</span>;
        <span class="hljs-keyword">case</span> <span class="hljs-number">57600</span>:  speed = B57600;  <span class="hljs-keyword">break</span>;
        <span class="hljs-keyword">case</span> <span class="hljs-number">115200</span>: speed = B115200; <span class="hljs-keyword">break</span>;
        <span class="hljs-keyword">default</span>:
            warnx(<span class="hljs-string">"ERR: baudrate: %d\n"</span>, baud);
            <span class="hljs-keyword">return</span> -EINVAL;
    }

    <span class="hljs-keyword">struct</span> termios uart_config;

    <span class="hljs-keyword">int</span> termios_state;

    <span class="hljs-comment">/* fill the struct for the new configuration */</span>
    tcgetattr(fd, &uart_config);
    <span class="hljs-comment">/* clear ONLCR flag (which appends a CR for every LF) */</span>
    uart_config.c_oflag &= ~ONLCR;
    <span class="hljs-comment">/* no parity, one stop bit */</span>
    uart_config.c_cflag &= ~(CSTOPB | PARENB);
    <span class="hljs-comment">/* set baud rate */</span>
    <span class="hljs-keyword">if</span> ((termios_state = cfsetispeed(&uart_config, speed)) < <span class="hljs-number">0</span>) {
        warnx(<span class="hljs-string">"ERR: %d (cfsetispeed)\n"</span>, termios_state);
        <span class="hljs-keyword">return</span> <span class="hljs-keyword">false</span>;
    }

    <span class="hljs-keyword">if</span> ((termios_state = cfsetospeed(&uart_config, speed)) < <span class="hljs-number">0</span>) {
        warnx(<span class="hljs-string">"ERR: %d (cfsetospeed)\n"</span>, termios_state);
        <span class="hljs-keyword">return</span> <span class="hljs-keyword">false</span>;
    }

    <span class="hljs-keyword">if</span> ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < <span class="hljs-number">0</span>) {
        warnx(<span class="hljs-string">"ERR: %d (tcsetattr)\n"</span>, termios_state);
        <span class="hljs-keyword">return</span> <span class="hljs-keyword">false</span>;
    }

    <span class="hljs-keyword">return</span> <span class="hljs-keyword">true</span>;
}


<span class="hljs-keyword">int</span> uart_init(<span class="hljs-keyword">char</span> * uart_name)
{
    <span class="hljs-keyword">int</span> serial_fd = open(uart_name, O_RDWR | O_NOCTTY);

    <span class="hljs-keyword">if</span> (serial_fd < <span class="hljs-number">0</span>) {
        err(<span class="hljs-number">1</span>, <span class="hljs-string">"failed to open port: %s"</span>, uart_name);
        <span class="hljs-keyword">return</span> <span class="hljs-keyword">false</span>;
    }
    <span class="hljs-keyword">return</span> serial_fd;
}

<span class="hljs-keyword">int</span> rw_uart_main(<span class="hljs-keyword">int</span> argc, <span class="hljs-keyword">char</span> *argv[])
{
    <span class="hljs-keyword">char</span> data = <span class="hljs-string">'0'</span>;
    <span class="hljs-keyword">char</span> buffer[<span class="hljs-number">4</span>] = <span class="hljs-string">""</span>;
    <span class="hljs-comment">/*
     * TELEM1 : /dev/ttyS1
     * TELEM2 : /dev/ttyS2
     * GPS    : /dev/ttyS3
     * NSH    : /dev/ttyS5
     * SERIAL4: /dev/ttyS6
     * N/A    : /dev/ttyS4
     * IO DEBUG (RX only):/dev/ttyS0
     */</span>
    <span class="hljs-keyword">int</span> uart_read = uart_init(<span class="hljs-string">"/dev/ttyS2"</span>);
    <span class="hljs-keyword">if</span>(<span class="hljs-keyword">false</span> == uart_read)<span class="hljs-keyword">return</span> -<span class="hljs-number">1</span>;
    <span class="hljs-keyword">if</span>(<span class="hljs-keyword">false</span> == set_uart_baudrate(uart_read,<span class="hljs-number">9600</span>)){
        <span class="hljs-built_in">printf</span>(<span class="hljs-string">"[YCM]set_uart_baudrate is failed\n"</span>);
        <span class="hljs-keyword">return</span> -<span class="hljs-number">1</span>;
    }
    <span class="hljs-built_in">printf</span>(<span class="hljs-string">"[YCM]uart init is successful\n"</span>);

    <span class="hljs-keyword">while</span>(<span class="hljs-keyword">true</span>){
        read(uart_read,&data,<span class="hljs-number">1</span>);
        <span class="hljs-keyword">if</span>(data == <span class="hljs-string">'R'</span>){
            <span class="hljs-keyword">for</span>(<span class="hljs-keyword">int</span> i = <span class="hljs-number">0</span>;i <<span class="hljs-number">4</span>;++i){
                read(uart_read,&data,<span class="hljs-number">1</span>);
                buffer[i] = data;
                data = <span class="hljs-string">'0'</span>;
            }
            <span class="hljs-built_in">printf</span>(<span class="hljs-string">"%s\n"</span>,buffer);
        }
    }

    <span class="hljs-keyword">return</span> <span class="hljs-number">0</span>;
}

编译并刷固件
- make clean
- make upload px4fmu-v2_default
查看app
- 在NSH终端中输入help，在Builtin Apps中出现rw_uart应用。
运行rw_uart应用（前提是模块与Pixhawk连接好）
- 在NSH终端中输入rw_uart，回车，查看超声波的打印数据。

3 发布超声波的数据

在无人机运行时，首先是要将应用随系统启动时就启动起来的，且将获得的超声波数据不断的发布出去，从而让其他应用得以订阅使用。这里也使用Pixhawk里面的通用模式，即主线程，检测app命令输入，创建一个线程来不断的发布数据。

3.1 定义主题和发布主题

在modules/rw_uart文件夹下创建一个文件：rw_uart_sonar_topic.h

rw_uart_sonar_topic.h

<code class="language-c++ hljs vala has-numbering"><span class="hljs-preprocessor">#ifndef __RW_UART_SONAR_H_</span>
<span class="hljs-preprocessor">#define __RW_UART_SONAR_H_</span>

<span class="hljs-preprocessor">#include <stdint.h></span>
<span class="hljs-preprocessor">#include <uORB/uORB.h></span>

<span class="hljs-comment">/*声明主题，主题名自定义*/</span>
ORB_DECLARE(rw_uart_sonar);

<span class="hljs-comment">/* 定义要发布的数据结构体 */</span>
<span class="hljs-keyword">struct</span> rw_uart_sonar_data_s{
    <span class="hljs-keyword">char</span> datastr[<span class="hljs-number">5</span>];        <span class="hljs-comment">//原始数据</span>
    <span class="hljs-keyword">int</span> data;               <span class="hljs-comment">//解析数据，单位：mm</span>
};

<span class="hljs-preprocessor">#endif  </span></code><ul class="pre-numbering" style="display: block;"><li>1</li><li>2</li><li>3</li><li>4</li><li>5</li><li>6</li><li>7</li><li>8</li><li>9</li><li>10</li><li>11</li><li>12</li><li>13</li><li>14</li><li>15</li><li>16</li></ul><ul class="pre-numbering" style="display: block;"><li>1</li><li>2</li><li>3</li><li>4</li><li>5</li><li>6</li><li>7</li><li>8</li><li>9</li><li>10</li><li>11</li><li>12</li><li>13</li><li>14</li><li>15</li><li>16</li></ul>

rw_uart.c

<code class="language-c++ hljs cpp has-numbering"><span class="hljs-preprocessor">#include <stdio.h></span>
<span class="hljs-preprocessor">#include <termios.h></span>
<span class="hljs-preprocessor">#include <unistd.h></span>
<span class="hljs-preprocessor">#include <stdlib.h></span>
<span class="hljs-preprocessor">#include <string.h></span>
<span class="hljs-preprocessor">#include <stdbool.h></span>
<span class="hljs-preprocessor">#include <errno.h></span>
<span class="hljs-preprocessor">#include <drivers/drv_hrt.h></span>

<span class="hljs-preprocessor">#include "rw_uart_sonar_topic.h"</span>

<span class="hljs-comment">/* 定义主题 */</span>
ORB_DEFINE(rw_uart_sonar, <span class="hljs-keyword">struct</span> rw_uart_sonar_data_s);

<span class="hljs-keyword">static</span> <span class="hljs-keyword">bool</span> thread_should_exit = <span class="hljs-keyword">false</span>;
<span class="hljs-keyword">static</span> <span class="hljs-keyword">bool</span> thread_running = <span class="hljs-keyword">false</span>;
<span class="hljs-keyword">static</span> <span class="hljs-keyword">int</span> daemon_task;


__EXPORT <span class="hljs-keyword">int</span> rw_uart_main(<span class="hljs-keyword">int</span> argc, <span class="hljs-keyword">char</span> *argv[]);
<span class="hljs-keyword">int</span> rw_uart_thread_main(<span class="hljs-keyword">int</span> argc, <span class="hljs-keyword">char</span> *argv[]);

<span class="hljs-keyword">static</span> <span class="hljs-keyword">int</span> uart_init(<span class="hljs-keyword">const</span> <span class="hljs-keyword">char</span> * uart_name);
<span class="hljs-keyword">static</span> <span class="hljs-keyword">int</span> set_uart_baudrate(<span class="hljs-keyword">const</span> <span class="hljs-keyword">int</span> fd, <span class="hljs-keyword">unsigned</span> <span class="hljs-keyword">int</span> baud);
<span class="hljs-keyword">static</span> <span class="hljs-keyword">void</span> usage(<span class="hljs-keyword">const</span> <span class="hljs-keyword">char</span> *reason);



<span class="hljs-keyword">int</span> set_uart_baudrate(<span class="hljs-keyword">const</span> <span class="hljs-keyword">int</span> fd, <span class="hljs-keyword">unsigned</span> <span class="hljs-keyword">int</span> baud)
{
    <span class="hljs-keyword">int</span> speed;

    <span class="hljs-keyword">switch</span> (baud) {
        <span class="hljs-keyword">case</span> <span class="hljs-number">9600</span>:   speed = B9600;   <span class="hljs-keyword">break</span>;
        <span class="hljs-keyword">case</span> <span class="hljs-number">19200</span>:  speed = B19200;  <span class="hljs-keyword">break</span>;
        <span class="hljs-keyword">case</span> <span class="hljs-number">38400</span>:  speed = B38400;  <span class="hljs-keyword">break</span>;
        <span class="hljs-keyword">case</span> <span class="hljs-number">57600</span>:  speed = B57600;  <span class="hljs-keyword">break</span>;
        <span class="hljs-keyword">case</span> <span class="hljs-number">115200</span>: speed = B115200; <span class="hljs-keyword">break</span>;
        <span class="hljs-keyword">default</span>:
            warnx(<span class="hljs-string">"ERR: baudrate: %d\n"</span>, baud);
            <span class="hljs-keyword">return</span> -EINVAL;
    }

    <span class="hljs-keyword">struct</span> termios uart_config;

    <span class="hljs-keyword">int</span> termios_state;

    <span class="hljs-comment">/* fill the struct for the new configuration */</span>
    tcgetattr(fd, &uart_config);
    <span class="hljs-comment">/* clear ONLCR flag (which appends a CR for every LF) */</span>
    uart_config.c_oflag &= ~ONLCR;
    <span class="hljs-comment">/* no parity, one stop bit */</span>
    uart_config.c_cflag &= ~(CSTOPB | PARENB);
    <span class="hljs-comment">/* set baud rate */</span>
    <span class="hljs-keyword">if</span> ((termios_state = cfsetispeed(&uart_config, speed)) < <span class="hljs-number">0</span>) {
        warnx(<span class="hljs-string">"ERR: %d (cfsetispeed)\n"</span>, termios_state);
        <span class="hljs-keyword">return</span> <span class="hljs-keyword">false</span>;
    }

    <span class="hljs-keyword">if</span> ((termios_state = cfsetospeed(&uart_config, speed)) < <span class="hljs-number">0</span>) {
        warnx(<span class="hljs-string">"ERR: %d (cfsetospeed)\n"</span>, termios_state);
        <span class="hljs-keyword">return</span> <span class="hljs-keyword">false</span>;
    }

    <span class="hljs-keyword">if</span> ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < <span class="hljs-number">0</span>) {
        warnx(<span class="hljs-string">"ERR: %d (tcsetattr)\n"</span>, termios_state);
        <span class="hljs-keyword">return</span> <span class="hljs-keyword">false</span>;
    }

    <span class="hljs-keyword">return</span> <span class="hljs-keyword">true</span>;
}


<span class="hljs-keyword">int</span> uart_init(<span class="hljs-keyword">const</span> <span class="hljs-keyword">char</span> * uart_name)
{
    <span class="hljs-keyword">int</span> serial_fd = open(uart_name, O_RDWR | O_NOCTTY);

    <span class="hljs-keyword">if</span> (serial_fd < <span class="hljs-number">0</span>) {
        err(<span class="hljs-number">1</span>, <span class="hljs-string">"failed to open port: %s"</span>, uart_name);
        <span class="hljs-keyword">return</span> <span class="hljs-keyword">false</span>;
    }
    <span class="hljs-keyword">return</span> serial_fd;
}

<span class="hljs-keyword">static</span> <span class="hljs-keyword">void</span> usage(<span class="hljs-keyword">const</span> <span class="hljs-keyword">char</span> *reason)
{
    <span class="hljs-keyword">if</span> (reason) {
        <span class="hljs-built_in">fprintf</span>(stderr, <span class="hljs-string">"%s\n"</span>, reason);
    }

    <span class="hljs-built_in">fprintf</span>(stderr, <span class="hljs-string">"usage: position_estimator_inav {start|stop|status} [param]\n\n"</span>);
    <span class="hljs-built_in">exit</span>(<span class="hljs-number">1</span>);
}

<span class="hljs-keyword">int</span> rw_uart_main(<span class="hljs-keyword">int</span> argc, <span class="hljs-keyword">char</span> *argv[])
{
    <span class="hljs-keyword">if</span> (argc < <span class="hljs-number">2</span>) {
        usage(<span class="hljs-string">"[YCM]missing command"</span>);
    }

    <span class="hljs-keyword">if</span> (!<span class="hljs-built_in">strcmp</span>(argv[<span class="hljs-number">1</span>], <span class="hljs-string">"start"</span>)) {
        <span class="hljs-keyword">if</span> (thread_running) {
            warnx(<span class="hljs-string">"[YCM]already running\n"</span>);
            <span class="hljs-built_in">exit</span>(<span class="hljs-number">0</span>);
        }

        thread_should_exit = <span class="hljs-keyword">false</span>;
        daemon_task = px4_task_spawn_cmd(<span class="hljs-string">"rw_uart"</span>,
                         SCHED_DEFAULT,
                         SCHED_PRIORITY_MAX - <span class="hljs-number">5</span>,
                         <span class="hljs-number">2000</span>,
                         rw_uart_thread_main,
                         (argv) ? (<span class="hljs-keyword">char</span> * <span class="hljs-keyword">const</span> *)&argv[<span class="hljs-number">2</span>] : (<span class="hljs-keyword">char</span> * <span class="hljs-keyword">const</span> *)NULL);
        <span class="hljs-built_in">exit</span>(<span class="hljs-number">0</span>);
    }

    <span class="hljs-keyword">if</span> (!<span class="hljs-built_in">strcmp</span>(argv[<span class="hljs-number">1</span>], <span class="hljs-string">"stop"</span>)) {
        thread_should_exit = <span class="hljs-keyword">true</span>;
        <span class="hljs-built_in">exit</span>(<span class="hljs-number">0</span>);
    }

    <span class="hljs-keyword">if</span> (!<span class="hljs-built_in">strcmp</span>(argv[<span class="hljs-number">1</span>], <span class="hljs-string">"status"</span>)) {
        <span class="hljs-keyword">if</span> (thread_running) {
            warnx(<span class="hljs-string">"[YCM]running"</span>);

        } <span class="hljs-keyword">else</span> {
            warnx(<span class="hljs-string">"[YCM]stopped"</span>);
        }

        <span class="hljs-built_in">exit</span>(<span class="hljs-number">0</span>);
    }

    usage(<span class="hljs-string">"unrecognized command"</span>);
    <span class="hljs-built_in">exit</span>(<span class="hljs-number">1</span>);
}

<span class="hljs-keyword">int</span> rw_uart_thread_main(<span class="hljs-keyword">int</span> argc, <span class="hljs-keyword">char</span> *argv[])
{

    <span class="hljs-keyword">if</span> (argc < <span class="hljs-number">2</span>) {
        errx(<span class="hljs-number">1</span>, <span class="hljs-string">"[YCM]need a serial port name as argument"</span>);
        usage(<span class="hljs-string">"eg:"</span>);
    }

    <span class="hljs-keyword">const</span> <span class="hljs-keyword">char</span> *uart_name = argv[<span class="hljs-number">1</span>];

    warnx(<span class="hljs-string">"[YCM]opening port %s"</span>, uart_name);
    <span class="hljs-keyword">char</span> data = <span class="hljs-string">'0'</span>;
    <span class="hljs-keyword">char</span> buffer[<span class="hljs-number">5</span>] = <span class="hljs-string">""</span>;
    <span class="hljs-comment">/*
     * TELEM1 : /dev/ttyS1
     * TELEM2 : /dev/ttyS2
     * GPS    : /dev/ttyS3
     * NSH    : /dev/ttyS5
     * SERIAL4: /dev/ttyS6
     * N/A    : /dev/ttyS4
     * IO DEBUG (RX only):/dev/ttyS0
     */</span>
    <span class="hljs-keyword">int</span> uart_read = uart_init(uart_name);
    <span class="hljs-keyword">if</span>(<span class="hljs-keyword">false</span> == uart_read)<span class="hljs-keyword">return</span> -<span class="hljs-number">1</span>;
    <span class="hljs-keyword">if</span>(<span class="hljs-keyword">false</span> == set_uart_baudrate(uart_read,<span class="hljs-number">9600</span>)){
        <span class="hljs-built_in">printf</span>(<span class="hljs-string">"[YCM]set_uart_baudrate is failed\n"</span>);
        <span class="hljs-keyword">return</span> -<span class="hljs-number">1</span>;
    }
    <span class="hljs-built_in">printf</span>(<span class="hljs-string">"[YCM]uart init is successful\n"</span>);

    thread_running = <span class="hljs-keyword">true</span>;

    <span class="hljs-comment">/*初始化数据结构体 */</span>
    <span class="hljs-keyword">struct</span> rw_uart_sonar_data_s sonardata;
    <span class="hljs-built_in">memset</span>(&sonardata, <span class="hljs-number">0</span>, <span class="hljs-keyword">sizeof</span>(sonardata));
    <span class="hljs-comment">/* 公告主题 */</span>
    orb_advert_t rw_uart_sonar_pub = orb_advertise(ORB_ID(rw_uart_sonar), &sonardata);


    <span class="hljs-keyword">while</span>(!thread_should_exit){
        read(uart_read,&data,<span class="hljs-number">1</span>);
        <span class="hljs-keyword">if</span>(data == <span class="hljs-string">'R'</span>){
            <span class="hljs-keyword">for</span>(<span class="hljs-keyword">int</span> i = <span class="hljs-number">0</span>;i <<span class="hljs-number">4</span>;++i){
                read(uart_read,&data,<span class="hljs-number">1</span>);
                buffer[i] = data;
                data = <span class="hljs-string">'0'</span>;
            }
            <span class="hljs-built_in">strncpy</span>(sonardata.datastr,buffer,<span class="hljs-number">4</span>);
            sonardata.data = atoi(sonardata.datastr);
            <span class="hljs-comment">//printf("[YCM]sonar.data=%d\n",sonardata.data);</span>
            orb_publish(ORB_ID(rw_uart_sonar), rw_uart_sonar_pub, &sonardata);
        }
    }

    warnx(<span class="hljs-string">"[YCM]exiting"</span>);
    thread_running = <span class="hljs-keyword">false</span>;
    close(uart_read);

    fflush(stdout);
    <span class="hljs-keyword">return</span> <span class="hljs-number">0</span>;
}
</code><ul class="pre-numbering" style="display: block;"><li>1</li><li>2</li><li>3</li><li>4</li><li>5</li><li>6</li><li>7</li><li>8</li><li>9</li><li>10</li><li>11</li><li>12</li><li>13</li><li>14</li><li>15</li><li>16</li><li>17</li><li>18</li><li>19</li><li>20</li><li>21</li><li>22</li><li>23</li><li>24</li><li>25</li><li>26</li><li>27</li><li>28</li><li>29</li><li>30</li><li>31</li><li>32</li><li>33</li><li>34</li><li>35</li><li>36</li><li>37</li><li>38</li><li>39</li><li>40</li><li>41</li><li>42</li><li>43</li><li>44</li><li>45</li><li>46</li><li>47</li><li>48</li><li>49</li><li>50</li><li>51</li><li>52</li><li>53</li><li>54</li><li>55</li><li>56</li><li>57</li><li>58</li><li>59</li><li>60</li><li>61</li><li>62</li><li>63</li><li>64</li><li>65</li><li>66</li><li>67</li><li>68</li><li>69</li><li>70</li><li>71</li><li>72</li><li>73</li><li>74</li><li>75</li><li>76</li><li>77</li><li>78</li><li>79</li><li>80</li><li>81</li><li>82</li><li>83</li><li>84</li><li>85</li><li>86</li><li>87</li><li>88</li><li>89</li><li>90</li><li>91</li><li>92</li><li>93</li><li>94</li><li>95</li><li>96</li><li>97</li><li>98</li><li>99</li><li>100</li><li>101</li><li>102</li><li>103</li><li>104</li><li>105</li><li>106</li><li>107</li><li>108</li><li>109</li><li>110</li><li>111</li><li>112</li><li>113</li><li>114</li><li>115</li><li>116</li><li>117</li><li>118</li><li>119</li><li>120</li><li>121</li><li>122</li><li>123</li><li>124</li><li>125</li><li>126</li><li>127</li><li>128</li><li>129</li><li>130</li><li>131</li><li>132</li><li>133</li><li>134</li><li>135</li><li>136</li><li>137</li><li>138</li><li>139</li><li>140</li><li>141</li><li>142</li><li>143</li><li>144</li><li>145</li><li>146</li><li>147</li><li>148</li><li>149</li><li>150</li><li>151</li><li>152</li><li>153</li><li>154</li><li>155</li><li>156</li><li>157</li><li>158</li><li>159</li><li>160</li><li>161</li><li>162</li><li>163</li><li>164</li><li>165</li><li>166</li><li>167</li><li>168</li><li>169</li><li>170</li><li>171</li><li>172</li><li>173</li><li>174</li><li>175</li><li>176</li><li>177</li><li>178</li><li>179</li><li>180</li><li>181</li><li>182</li><li>183</li><li>184</li><li>185</li><li>186</li><li>187</li><li>188</li><li>189</li><li>190</li><li>191</li><li>192</li><li>193</li><li>194</li><li>195</li><li>196</li><li>197</li><li>198</li></ul><ul class="pre-numbering" style="display: block;"><li>1</li><li>2</li><li>3</li><li>4</li><li>5</li><li>6</li><li>7</li><li>8</li><li>9</li><li>10</li><li>11</li><li>12</li><li>13</li><li>14</li><li>15</li><li>16</li><li>17</li><li>18</li><li>19</li><li>20</li><li>21</li><li>22</li><li>23</li><li>24</li><li>25</li><li>26</li><li>27</li><li>28</li><li>29</li><li>30</li><li>31</li><li>32</li><li>33</li><li>34</li><li>35</li><li>36</li><li>37</li><li>38</li><li>39</li><li>40</li><li>41</li><li>42</li><li>43</li><li>44</li><li>45</li><li>46</li><li>47</li><li>48</li><li>49</li><li>50</li><li>51</li><li>52</li><li>53</li><li>54</li><li>55</li><li>56</li><li>57</li><li>58</li><li>59</li><li>60</li><li>61</li><li>62</li><li>63</li><li>64</li><li>65</li><li>66</li><li>67</li><li>68</li><li>69</li><li>70</li><li>71</li><li>72</li><li>73</li><li>74</li><li>75</li><li>76</li><li>77</li><li>78</li><li>79</li><li>80</li><li>81</li><li>82</li><li>83</li><li>84</li><li>85</li><li>86</li><li>87</li><li>88</li><li>89</li><li>90</li><li>91</li><li>92</li><li>93</li><li>94</li><li>95</li><li>96</li><li>97</li><li>98</li><li>99</li><li>100</li><li>101</li><li>102</li><li>103</li><li>104</li><li>105</li><li>106</li><li>107</li><li>108</li><li>109</li><li>110</li><li>111</li><li>112</li><li>113</li><li>114</li><li>115</li><li>116</li><li>117</li><li>118</li><li>119</li><li>120</li><li>121</li><li>122</li><li>123</li><li>124</li><li>125</li><li>126</li><li>127</li><li>128</li><li>129</li><li>130</li><li>131</li><li>132</li><li>133</li><li>134</li><li>135</li><li>136</li><li>137</li><li>138</li><li>139</li><li>140</li><li>141</li><li>142</li><li>143</li><li>144</li><li>145</li><li>146</li><li>147</li><li>148</li><li>149</li><li>150</li><li>151</li><li>152</li><li>153</li><li>154</li><li>155</li><li>156</li><li>157</li><li>158</li><li>159</li><li>160</li><li>161</li><li>162</li><li>163</li><li>164</li><li>165</li><li>166</li><li>167</li><li>168</li><li>1

3.2 测试发布的主题—订阅主题

测试可以随便一个启动的app中进行主题订阅，然后将订阅的数据打印出来，看是否是超声波的数据。这里测试是在固件的src/examples文件夹中的px4_simple_app应用进行测试的。

将px4_simple_app应用添加到NuttShell中。Firmware/makefiles/nuttx/config_px4fmu-v2_default.mk文件中添加如下内容：
- MODULES += examples/px4_simple_app
在px4_simple_app.c中代码内容：

<code class="language-c++ hljs cpp has-numbering"><span class="hljs-preprocessor">#include <px4_config.h></span>
<span class="hljs-preprocessor">#include <unistd.h></span>
<span class="hljs-preprocessor">#include <stdio.h></span>
<span class="hljs-preprocessor">#include <poll.h></span>
<span class="hljs-preprocessor">#include <string.h></span>

<span class="hljs-preprocessor">#include <uORB/uORB.h></span>
<span class="hljs-preprocessor">#include "rw_uart/rw_uart_sonar_topic.h"</span>

__EXPORT <span class="hljs-keyword">int</span> px4_simple_app_main(<span class="hljs-keyword">int</span> argc, <span class="hljs-keyword">char</span> *argv[]);

<span class="hljs-keyword">int</span> px4_simple_app_main(<span class="hljs-keyword">int</span> argc, <span class="hljs-keyword">char</span> *argv[])
{
    <span class="hljs-built_in">printf</span>(<span class="hljs-string">"Hello Sky!\n"</span>);

    <span class="hljs-comment">/* subscribe to rw_uart_sonar topic */</span>
    <span class="hljs-keyword">int</span> sonar_sub_fd = orb_subscribe(ORB_ID(rw_uart_sonar));
    <span class="hljs-comment">/*设置以一秒钟接收一次，并打印出数据*/</span>
    orb_set_interval(sonar_sub_fd, <span class="hljs-number">1000</span>);
    <span class="hljs-keyword">bool</span> updated;
    <span class="hljs-keyword">struct</span> rw_uart_sonar_data_s sonar;

    <span class="hljs-comment">/*接收数据方式一：start*/</span>
    <span class="hljs-comment">/*
    while(true){
        orb_check(sonar_sub_fd, &updated);

        if (updated) {
            orb_copy(ORB_ID(rw_uart_sonar), sonar_sub_fd, &sonar);
            printf("[YCM]sonar.data=%d\n",sonar.data);
        }
        //else printf("[YCM]No soanar data update\n");
    }
    */</span>
    <span class="hljs-comment">/*接收数据方式一：end*/</span>

    <span class="hljs-comment">/*接收数据方式二：start*/</span>
    <span class="hljs-comment">/* one could wait for multiple topics with this technique, just using one here */</span>
    <span class="hljs-keyword">struct</span> pollfd fds[] = {
        { .fd = sonar_sub_fd,   .events = POLLIN },
        <span class="hljs-comment">/* there could be more file descriptors here, in the form like:
         * { .fd = other_sub_fd,   .events = POLLIN },
         */</span>
    };

    <span class="hljs-keyword">int</span> error_counter = <span class="hljs-number">0</span>;

    <span class="hljs-keyword">for</span> (<span class="hljs-keyword">int</span> i = <span class="hljs-number">0</span>; i < <span class="hljs-number">5</span>; i++) {s
        <span class="hljs-comment">/* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */</span>
        <span class="hljs-keyword">int</span> poll_ret = poll(fds, <span class="hljs-number">1</span>, <span class="hljs-number">1000</span>);

        <span class="hljs-comment">/* handle the poll result */</span>
        <span class="hljs-keyword">if</span> (poll_ret == <span class="hljs-number">0</span>) {
            <span class="hljs-comment">/* this means none of our providers is giving us data */</span>
            <span class="hljs-built_in">printf</span>(<span class="hljs-string">"[px4_simple_app] Got no data within a second\n"</span>);

        } <span class="hljs-keyword">else</span> <span class="hljs-keyword">if</span> (poll_ret < <span class="hljs-number">0</span>) {
            <span class="hljs-comment">/* this is seriously bad - should be an emergency */</span>
            <span class="hljs-keyword">if</span> (error_counter < <span class="hljs-number">10</span> || error_counter % <span class="hljs-number">50</span> == <span class="hljs-number">0</span>) {
                <span class="hljs-comment">/* use a counter to prevent flooding (and slowing us down) */</span>
                <span class="hljs-built_in">printf</span>(<span class="hljs-string">"[px4_simple_app] ERROR return value from poll(): %d\n"</span>
                       , poll_ret);
            }

            error_counter++;

        } <span class="hljs-keyword">else</span> {

            <span class="hljs-keyword">if</span> (fds[<span class="hljs-number">0</span>].revents & POLLIN) {
                <span class="hljs-comment">/* obtained data for the first file descriptor */</span>
                <span class="hljs-keyword">struct</span> rw_uart_sonar_data_s sonar;
                <span class="hljs-comment">/* copy sensors raw data into local buffer */</span>
                orb_copy(ORB_ID(rw_uart_sonar), sonar_sub_fd, &sonar);
                <span class="hljs-built_in">printf</span>(<span class="hljs-string">"[px4_simple_app] Sonar data:\t%s\t%d\n"</span>,
                       sonar.datastr,
                       sonar.data);
            }

            <span class="hljs-comment">/* there could be more file descriptors here, in the form like:
             * if (fds[1..n].revents & POLLIN) {}
             */</span>
        }
    }
    <span class="hljs-comment">/*接收数据方式二：end*/</span>

    <span class="hljs-keyword">return</span> <span class="hljs-number">0</span>;
}</code><ul class="pre-numbering" style="display: block;"><li>1</li><li>2</li><li>3</li><li>4</li><li>5</li><li>6</li><li>7</li><li>8</li><li>9</li><li>10</li><li>11</li><li>12</li><li>13</li><li>14</li><li>15</li><li>16</li><li>17</li><li>18</li><li>19</li><li>20</li><li>21</li><li>22</li><li>23</li><li>24</li><li>25</li><li>26</li><li>27</li><li>28</li><li>29</li><li>30</li><li>31</li><li>32</li><li>33</li><li>34</li><li>35</li><li>36</li><li>37</li><li>38</li><li>39</li><li>40</li><li>41</li><li>42</li><li>43</li><li>44</li><li>45</li><li>46</li><li>47</li><li>48</li><li>49</li><li>50</li><li>51</li><li>52</li><li>53</li><li>54</li><li>55</li><li>56</li><li>57</li><li>58</li><li>59</li><li>60</li><li>61</li><li>62</li><li>63</li><li>64</li><li>65</li><li>66</li><li>67</li><li>68</li><li>69</li><li>70</li><li>71</li><li>72</li><li>73</li><li>74</li><li>75</li><li>76</li><li>77</li><li>78</li><li>79</li><li>80</li><li>81</li><li>82</li><li>83</li><li>84</li><li>85</li><li>86</li><li>87</li></ul><ul class="pre-numbering" style="display: block;"><li>1</li><li>2</li><li>3</li><li>4</li><li>5</li><li>6</li><li>7</li><li>8</li><li>9</li><li>10</li><li>11</li><li>12</li><li>13</li><li>14</li><li>15</li><li>16</li><li>17</li><li>18</li><li>19</li><li>20</li><li>21</li><li>22</li><li>23</li><li>24</li><li>25</li><li>26</li><li>27</li><li>28</li><li>29</li><li>30</li><li>31</li><li>32</li><li>33</li><li>34</li><li>35</li><li>36</li><li>37</li><li>38</li><li>39</li><li>40</li><li>41</li><li>42</li><li>43</li><li>44</li><li>45</li><li>46</li><li>47</li><li>48</li><li>49</li><li>50</li><li>51</li><li>52</li><li>53</li><li>54</li><li>55</li><li>56</li><li>57</li><li>58</li><li>59</li><li>60</li><li>61</li><li>62</li><li>63</li><li>64</li><li>65</li><li>66</li><li>67</li><li>68</li><li>69</li><li>70</li><li>71</li><li>72</li><li>73</li><li>74</li><li>75</li><li>76</li><li>77</li><li>78</li><li>79</li><li>80</li><li>81</li><li>82</li><li>83</li><li>84</li><li>85</li><li>86</li><li>87</li></ul>

编译并刷固件
- make upload px4fmu-v2_default
在NSH中测试(已加入自启动脚本中)
- rw_uart start /dev/ttyS2
- px4_simple_app

这里写图片描述

3.3 加入系统启动脚本

可以加入到光流的自定义启动脚本中：/fs/microsd/etc/extras.txt。这样随着系统的自启动，rw_uart就会默认启动了。

<code class="language-makefile hljs  has-numbering"><span class="hljs-comment"># start sonar</span>
rw_uart start /dev/ttyS2</code><ul class="pre-numbering" style="display: block;"><li>1</li><li>2</li></ul><ul class="pre-numbering" style="display: block;"><li>1</li><li>2</li></ul>