PX4固件通过UART连接串口读取超声波，和树莓派3通信

来源：互联网发布：新加坡移动网络制式编辑：程序博客网时间：2024/04/30 14:31

添加串口读取程序

首先在Firmware/msg文件夹下添加rw_uart.msg

char[5] datastrint16 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_uart        MAIN rw_uart    COMPILE_FLAGS        -Os    SRCS                rw_uart.c    DEPENDS        platforms__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 –lwiringPisudo ./uart

这几天坑埋完了，上各种博客链接：
树莓派和pixhawk连接
struct termios结构体详解
树莓派串口发送数据
PX4原生固件添加串口读取传感器

阅读全文

0 0