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

#include <stdio.h>
#include <termios.h>
#include <unistd.h>
#include <stdbool.h>
#include <errno.h>
#include <drivers/drv_hrt.h>__EXPORT int rw_uart_main(int argc, char *argv[]);static int uart_init(char * uart_name);
static int set_uart_baudrate(const int fd, unsigned int baud);int set_uart_baudrate(const int fd, unsigned int baud)
{int speed;switch (baud) {case 9600:   speed = B9600;   break;case 19200:  speed = B19200;  break;case 38400:  speed = B38400;  break;case 57600:  speed = B57600;  break;case 115200: speed = B115200; break;default:warnx("ERR: baudrate: %d\n", baud);return -EINVAL;}struct termios uart_config;int termios_state;/* fill the struct for the new configuration */tcgetattr(fd, &uart_config);/* clear ONLCR flag (which appends a CR for every LF) */uart_config.c_oflag &= ~ONLCR;/* no parity, one stop bit */uart_config.c_cflag &= ~(CSTOPB | PARENB);/* set baud rate */if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {warnx("ERR: %d (cfsetispeed)\n", termios_state);return false;}if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {warnx("ERR: %d (cfsetospeed)\n", termios_state);return false;}if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {warnx("ERR: %d (tcsetattr)\n", termios_state);return false;}return true;
}int uart_init(char * uart_name)
{int serial_fd = open(uart_name, O_RDWR | O_NOCTTY);if (serial_fd < 0) {err(1, "failed to open port: %s", uart_name);return false;}return serial_fd;
}int rw_uart_main(int argc, char *argv[])
{char data = '0';char buffer[4] = "";/** 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*/int uart_read = uart_init("/dev/ttyS2");if(false == uart_read)return -1;if(false == set_uart_baudrate(uart_read,9600)){printf("[YCM]set_uart_baudrate is failed\n");return -1;}printf("[YCM]uart init is successful\n");while(true){read(uart_read,&data,1);if(data == 'R'){for(int i = 0;i <4;++i){read(uart_read,&data,1);buffer[i] = data;data = '0';}printf("%s\n",buffer);}}return 0;
}

编译并刷固件
- 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

#ifndef __RW_UART_SONAR_H_
#define __RW_UART_SONAR_H_#include <stdint.h>
#include <uORB/uORB.h>/*声明主题，主题名自定义*/
ORB_DECLARE(rw_uart_sonar);/* 定义要发布的数据结构体 */
struct rw_uart_sonar_data_s{char datastr[5];        //原始数据int data;               //解析数据，单位：mm
};#endif

rw_uart.c

#include <stdio.h>
#include <termios.h>
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <errno.h>
#include <drivers/drv_hrt.h>#include "rw_uart_sonar_topic.h"/* 定义主题 */
ORB_DEFINE(rw_uart_sonar, struct rw_uart_sonar_data_s);static bool thread_should_exit = false;
static bool thread_running = false;
static int daemon_task;__EXPORT int rw_uart_main(int argc, char *argv[]);
int rw_uart_thread_main(int argc, char *argv[]);static int uart_init(const char * uart_name);
static int set_uart_baudrate(const int fd, unsigned int baud);
static void usage(const char *reason);int set_uart_baudrate(const int fd, unsigned int baud)
{int speed;switch (baud) {case 9600:   speed = B9600;   break;case 19200:  speed = B19200;  break;case 38400:  speed = B38400;  break;case 57600:  speed = B57600;  break;case 115200: speed = B115200; break;default:warnx("ERR: baudrate: %d\n", baud);return -EINVAL;}struct termios uart_config;int termios_state;/* fill the struct for the new configuration */tcgetattr(fd, &uart_config);/* clear ONLCR flag (which appends a CR for every LF) */uart_config.c_oflag &= ~ONLCR;/* no parity, one stop bit */uart_config.c_cflag &= ~(CSTOPB | PARENB);/* set baud rate */if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {warnx("ERR: %d (cfsetispeed)\n", termios_state);return false;}if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {warnx("ERR: %d (cfsetospeed)\n", termios_state);return false;}if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {warnx("ERR: %d (tcsetattr)\n", termios_state);return false;}return true;
}int uart_init(const char * uart_name)
{int serial_fd = open(uart_name, O_RDWR | O_NOCTTY);if (serial_fd < 0) {err(1, "failed to open port: %s", uart_name);return false;}return serial_fd;
}static void usage(const char *reason)
{if (reason) {fprintf(stderr, "%s\n", reason);}fprintf(stderr, "usage: position_estimator_inav {start|stop|status} [param]\n\n");exit(1);
}int rw_uart_main(int argc, char *argv[])
{if (argc < 2) {usage("[YCM]missing command");}if (!strcmp(argv[1], "start")) {if (thread_running) {warnx("[YCM]already running\n");exit(0);}thread_should_exit = false;daemon_task = px4_task_spawn_cmd("rw_uart",SCHED_DEFAULT,SCHED_PRIORITY_MAX - 5,2000,rw_uart_thread_main,(argv) ? (char * const *)&argv[2] : (char * const *)NULL);exit(0);}if (!strcmp(argv[1], "stop")) {thread_should_exit = true;exit(0);}if (!strcmp(argv[1], "status")) {if (thread_running) {warnx("[YCM]running");} else {warnx("[YCM]stopped");}exit(0);}usage("unrecognized command");exit(1);
}int rw_uart_thread_main(int argc, char *argv[])
{if (argc < 2) {errx(1, "[YCM]need a serial port name as argument");usage("eg:");}const char *uart_name = argv[1];warnx("[YCM]opening port %s", uart_name);char data = '0';char buffer[5] = "";/** 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*/int uart_read = uart_init(uart_name);if(false == uart_read)return -1;if(false == set_uart_baudrate(uart_read,9600)){printf("[YCM]set_uart_baudrate is failed\n");return -1;}printf("[YCM]uart init is successful\n");thread_running = true;/*初始化数据结构体 */struct rw_uart_sonar_data_s sonardata;memset(&sonardata, 0, sizeof(sonardata));/* 公告主题 */orb_advert_t rw_uart_sonar_pub = orb_advertise(ORB_ID(rw_uart_sonar), &sonardata);while(!thread_should_exit){read(uart_read,&data,1);if(data == 'R'){for(int i = 0;i <4;++i){read(uart_read,&data,1);buffer[i] = data;data = '0';}strncpy(sonardata.datastr,buffer,4);sonardata.data = atoi(sonardata.datastr);//printf("[YCM]sonar.data=%d\n",sonardata.data);orb_publish(ORB_ID(rw_uart_sonar), rw_uart_sonar_pub, &sonardata);}}warnx("[YCM]exiting");thread_running = false;close(uart_read);fflush(stdout);return 0;
}

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中代码内容：

#include <px4_config.h>
#include <unistd.h>
#include <stdio.h>
#include <poll.h>
#include <string.h>#include <uORB/uORB.h>
#include "rw_uart/rw_uart_sonar_topic.h"__EXPORT int px4_simple_app_main(int argc, char *argv[]);int px4_simple_app_main(int argc, char *argv[])
{printf("Hello Sky!\n");/* subscribe to rw_uart_sonar topic */int sonar_sub_fd = orb_subscribe(ORB_ID(rw_uart_sonar));/*设置以一秒钟接收一次，并打印出数据*/orb_set_interval(sonar_sub_fd, 1000);bool updated;struct rw_uart_sonar_data_s sonar;/*接收数据方式一：start*//*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");}*//*接收数据方式一：end*//*接收数据方式二：start*//* one could wait for multiple topics with this technique, just using one here */struct pollfd fds[] = {{ .fd = sonar_sub_fd,   .events = POLLIN },/* there could be more file descriptors here, in the form like:* { .fd = other_sub_fd,   .events = POLLIN },*/};int error_counter = 0;for (int i = 0; i < 5; i++) {s/* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */int poll_ret = poll(fds, 1, 1000);/* handle the poll result */if (poll_ret == 0) {/* this means none of our providers is giving us data */printf("[px4_simple_app] Got no data within a second\n");} else if (poll_ret < 0) {/* this is seriously bad - should be an emergency */if (error_counter < 10 || error_counter % 50 == 0) {/* use a counter to prevent flooding (and slowing us down) */printf("[px4_simple_app] ERROR return value from poll(): %d\n", poll_ret);}error_counter++;} else {if (fds[0].revents & POLLIN) {/* obtained data for the first file descriptor */struct rw_uart_sonar_data_s sonar;/* copy sensors raw data into local buffer */orb_copy(ORB_ID(rw_uart_sonar), sonar_sub_fd, &sonar);printf("[px4_simple_app] Sonar data:\t%s\t%d\n",sonar.datastr,sonar.data);}/* there could be more file descriptors here, in the form like:* if (fds[1..n].revents & POLLIN) {}*/}}/*接收数据方式二：end*/return 0;
}

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

3.3 加入系统启动脚本

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

# start sonar
rw_uart start /dev/ttyS2