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



版权声明：本文为博主[原创]文章，未经博主允许可以转载，注明博客出处：[http://blog.csdn.net/FreeApe]

目录(?)[+]

1. Pixhawk添加一个自定义传感器超声波串口方式
   1. 说明
   2. 读取数据测试
   3. 发布超声波的数据
      1. 1 定义主题和发布主题
      2. 2 测试发布的主题订阅主题
      3. 3 加入系统启动脚本

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>