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);