本文主要是介绍PX4固件通过UART连接串口读取超声波，和树莓派3通信（似乎讲了怎么添加驱动程序，添加自定义msg，还有uORB消息订阅，佩服，感觉做了我想做的！），希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！
转载自：https://blog.csdn.net/qq_28773183/article/details/78137434
PX4固件通过UART连接串口读取超声波，和树莓派3通信
勤奋比天赋更重要 2017-09-29 21:12:08 2686 收藏 12
分类专栏： PX4研究笔记
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/qq_28773183/article/details/78137434
版权
PX4研究笔记 专栏收录该内容
13 篇文章 0 订阅
订阅专栏
添加串口读取程序
首先在Firmware/msg文件夹下添加rw_uart.msg
char[5] datastr
int16 data
#TOPICS rw_uart记住在这一文件夹下的CMakeLists.txt下注册这个msg，添加rw_uart.msg即可。
上面的文件make之后会自动产生rw_uart.h头文件，里面会有结构体rw_uart_s,存取了我们刚才定义的data和datastr。然后在Firmware/src/module文件夹下新建文件夹rw_uart，在里面添加CMakeLists.txt和rw_uart.c。
CMakeLists.txt内容如下：
set(MODULE_CFLAGS)
px4_add_module(MODULE modules__rw_uartMAIN rw_uartCOMPILE_FLAGS-OsSRCSrw_uart.cDEPENDSplatforms__common)读取程序rw_uart.c如下：
#include <px4_config.h>
#include <px4_tasks.h>
#include <px4_posix.h>
#include <unistd.h>
#include <stdio.h>
#include <poll.h>
#include <string.h>
#include <uORB/uORB.h>
#include <stdio.h>
#include <termios.h>
#include <unistd.h>
#include <stdbool.h>
#include <errno.h>
#include <drivers/drv_hrt.h>
#include <string.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <fcntl.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <uORB/topics/rw_uart.h>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;/**
/*termios 函数族提供了一个常规的终端接口，用于控制非同步通信端口。 这个结构包含了至少下列成员：
/*tcflag_t c_iflag; /* 输入模式 */
/*tcflag_t c_oflag; /* 输出模式 */
/*tcflag_t c_cflag; /* 控制模式 */
/*tcflag_t c_lflag; /* 本地模式 */
/*cc_t c_cc[NCCS]; /* 控制字符 */
*/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("[FC]missing command");}if (!strcmp(argv[1], "start")) {if (thread_running) {warnx("[FC]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("[FC]running");} else {warnx("[FC]stopped");}exit(0);}usage("unrecognized command");exit(1);
}int rw_uart_thread_main(int argc, char *argv[])
{if (argc < 2) {errx(1, "[FC]need a serial port name as argument");usage("eg:");}const char *uart_name = argv[1];warnx("[FC]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("[FC]set_uart_baudrate is failed\n");return -1;}printf("[FC]uart init is successful\n");thread_running = true;/*初始化数据结构体 */struct rw_uart_s sonardata;memset(&sonardata, 0, sizeof(sonardata));/* 公告主题 */orb_advert_t rw_uart_pub = orb_advertise(ORB_ID(rw_uart), &sonardata);while(!thread_should_exit){read(uart_read,&data,1);if(data == 'R'){//这个地方是模拟树莓派发送的R1100数据，在超声波处注释掉这个if条件语句，直接读取for(int i = 0;i <4;++i){read(uart_read,&data,1);buffer[i] = data;//data = '0';}// printf("%s\n",buffer);strncpy(sonardata.datastr,buffer,4);sonardata.data = atoi(sonardata.datastr);// printf("[YCM]sonar.data=%s\n",sonardata.datastr);orb_publish(ORB_ID(rw_uart), rw_uart_pub, &sonardata);}}warnx("[FC]exiting");thread_running = false;close(uart_read);fflush(stdout);return 0;
}超声波和px4的硬件连接
首先说一下，这里用的是pixhawk的TELEM2口，TELEM1接口给的是数传。TELEM1和TELEM2接口是一样的，分布如下：
对于UART主要是TX,RX,VCC,GND这四个接口，这里面我用的是HC-SRO4超声波传感器，这个传感器给的是触发信号，并没有直接给出距离，需要自己通过计时器来获得距离，这个找度娘就可以了，这里验证的是能读到数据。TELEM2和超声波的连接方式：VCC-VCC，TX-RX，RX-TX，GND-GND，这里我的超声波的连接是：TX-Trigger，RX-Echo。最后的结果如下：
这里我使用的是数据订阅的方式——uORB通信机制
在Firmware/src/examples/px4_simple_app文件夹下，修改px4_simple_app.c如下：
/****************************************************************************** Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.** Redistribution and use in source and binary forms, with or without* modification, are permitted provided that the following conditions* are met:** 1. Redistributions of source code must retain the above copyright* notice, this list of conditions and the following disclaimer.* 2. Redistributions in binary form must reproduce the above copyright* notice, this list of conditions and the following disclaimer in* the documentation and/or other materials provided with the* distribution.* 3. Neither the name PX4 nor the names of its contributors may be* used to endorse or promote products derived from this software* without specific prior written permission.** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE* POSSIBILITY OF SUCH DAMAGE.*****************************************************************************//*** @file px4_simple_app.c* Minimal application example for PX4 autopilot** @author Example User <mail@example.com>*/#include <px4_config.h>
#include <px4_tasks.h>
#include <px4_posix.h>
#include <unistd.h>
#include <stdio.h>
#include <poll.h>
#include <string.h>
#include <math.h>#include <uORB/uORB.h>
#include <uORB/topics/rw_uart.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));/*设置以一秒钟接收一次，并打印出数据*/orb_set_interval(sonar_sub_fd, 1000);// bool updated;struct rw_uart_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++) {/* 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_s sonar;/* copy sensors raw data into local buffer */orb_copy(ORB_ID(rw_uart), 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;
}树莓派3和pixhawk通信
树莓派和pixhawk的连接如下:
这里切记不要把VCC和GND的GPIO口接反了，很容易把树莓派的CPU 烧了（我就烧了一次），如果都供电了，建议就不要连接两者之间的VCC。树莓派的发送程序如下：
#include <stdio.h>
#include <wiringPi.h>
#include <wiringSerial.h>
int main()
{
int fd;
char data[5]=“R1100”;
int flag=1;
if(wiringPiSetup()<0)return 1;
if((fd=serialOpen("/dev/ttyAMA0",9600))<0) return 1;
printf("serial test start ...\n");
serialPrintf(fd,"Hello world!\n");
while(flag) { serialPrintf(fd,data);//向串口设备发送data数据 delay(300);while(serialDataAvail(fd)) { printf("->%3d\n",serialGetchar(fd));flag=0; fflush(stdout); } }
serialFlush(fd);
serialClose(fd);
return 0;} 首先需要说一下，这个串口发送程序需要安装wiringPi库，编译运行命令如下：
gcc –Wall uart.c –o uart –lwiringPi
sudo ./uart
这几天坑埋完了，上各种博客链接：
树莓派和pixhawk连接
struct termios结构体详解
树莓派串口发送数据
PX4原生固件添加串口读取传感器
这篇关于PX4固件通过UART连接串口读取超声波，和树莓派3通信（似乎讲了怎么添加驱动程序，添加自定义msg，还有uORB消息订阅，佩服，感觉做了我想做的！）的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！
