Pixhawk—添加一个自定义传感器—超声波（串口方式）
版权声明：本文为博主[原创]文章，未经博主允许可以转载，注明博客出处：[http://blog.csdn.net/FreeApe]
目录(?)[+]
- Pixhawk添加一个自定义传感器超声波串口方式
- 说明
- 读取数据测试
- 发布超声波的数据
- 1 定义主题和发布主题
- 2 测试发布的主题订阅主题
- 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应用。
- 在NSH终端中输入
- 运行rw_uart应用（前提是模块与Pixhawk连接好）
- 在NSH终端中输入
rw_uart，回车，查看超声波的打印数据。
- 在NSH终端中输入
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>