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中。
ardupilot/mk/PX4/px4_common.mk文件中添加如下内容：
- MODULES += modules/rw_uart
- rw_uart.c
#include#include #include #include #include #include
#include#include #include #include
__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 px4fmu-v2_upload
-
查看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
3.2 测试发布的主题—订阅主题
测试可以随便一个启动的app中进行主题订阅，然后将订阅的数据打印出来，看是否是超声波的数据。这里测试是在固件的src/examples文件夹中的px4_simple_app应用进行测试的。
- 将
px4_simple_app应用添加到NuttShell中。文件中添加如下内容：
- MODULES += examples/px4_simple_app
- 在
px4_simple_app.c中代码内容：
rw_uart_sonar_topic.h
#ifndef __RW_UART_SONAR_H_
#define __RW_UART_SONAR_H_
#include
#include
/*声明主题，主题名自定义*/
ORB_DECLARE(rw_uart_sonar);
/* 定义要发布的数据结构体 */
struct rw_uart_sonar_data_s{
char datastr[5]; //原始数据
int data; //解析数据，单位：mm
};
#endif
rw_uart.c
#include
#include
#include
#include
#include
#include
#include
#include
#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
#include
#include
#include
#include
#include
#include
#include
#include "rw_uart/rw_uart_app_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 sensor_combined topic */
//int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));
int sonar_sub_fd = orb_subscribe(ORB_ID(rw_uart_sonar));
//orb_set_interval(sensor_sub_fd, 1000);
orb_set_interval(sonar_sub_fd, 1000);
bool updated;
/* advertise attitude topic */
//struct vehicle_attitude_s att;
//struct rw_uart_sonar_dara_s sonar;
//memset(&att, 0, sizeof(att));
//orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
/* one could wait for multiple topics with this technique, just using one here */
//struct pollfd fds[] = {
//{ .fd = sensor_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++) {
/* 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 sensor_combined_s raw;
/* copy sensors raw data into local buffer */
/* orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw);
printf("[px4_simple_app] Accelerometer:\t%8.4f\t%8.4f\t%8.4f\n",
(double)raw.accelerometer_m_s2[0],
(double)raw.accelerometer_m_s2[1],
(double)raw.accelerometer_m_s2[2]);
*/
/* set att and publish this information for other apps */
/*att.roll = raw.accelerometer_m_s2[0];
att.pitch = raw.accelerometer_m_s2[1];
att.yaw = raw.accelerometer_m_s2[2];
orb_publish(ORB_ID(vehicle_attitude), att_pub, &att);
}*/
/* there could be more file descriptors here, in the form like:
* if (fds[1..n].revents & POLLIN) {}
*/
// }
// }
//return 0;
//}
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++) {
/* 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 px4fmu-v2_upload
-
在NSH中测试(已加入自启动脚本中)
- rw_uart start /dev/ttyS2
- px4_simple_app
3.3 加入系统启动脚本
可以加入到光流的自定义启动脚本中：/fs/microsd/etc/extras.txt。这样随着系统的自启动，rw_uart就会默认启动了。
rw_uart start /de/ttys2