Pixhawk---通过串口方式添加一个自定义传感器(超声波为例)
2015-08-21 17:17
92 查看
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
相关文章推荐
- radiusd cisco限速问题
- jQuery动画与特效
- spring framework各个版本下载网址
- win10怎么新建网络映射驱动器?win10映射驱动器的使用方法
- iOS 项目的目录结构
- 升级Win10 Build 10525激活方面遇到的一些问题解答汇总
- C fopen,fseek,fprintf,feof,ftell,rewind
- ICTCLAS分词关键技术
- Mac OSX 安装Docker
- Android之单选按钮,复选框使用方法汇总
- POJ 3273 Monthly Expense (二分,最小化最大值)
- 已知二叉树的先序遍历和中序遍历,如何求后序遍历
- 文字上添加删除线
- 折半插入排序
- 换个地方,说点儿真话,谈点儿人生中的循环定理
- jQuery操作dom事件
- H5版定点投篮游戏(1)--物理模型抽象
- Delphi 代码创建控件与事件动态绑定
- FFmpeg介绍
- 搭建ftp