Pixhawk原生固件PX4之串口读取信息

最新推荐文章于 2024-06-07 16:48:18 发布

FantasyJXF

最新推荐文章于 2024-06-07 16:48:18 发布

阅读量1.4w

点赞数 10

分类专栏： Pix学习笔记文章标签： Pixhawk PX4 串口数据传感器

本文链接：https://blog.csdn.net/oqqenvy12/article/details/57131022

版权

Pix学习笔记专栏收录该内容

34 篇文章 47 订阅

订阅专栏

欢迎交流~ 个人 Gitter 交流平台，点击直达：

这篇博客纯粹出于对FreeApe这位先行者贡献的复现，也是本人一直想要进行的一项操作。在此还是做一下记录。时代在改变，代码在更新，有些坑先填一填。

说明

本意是采用串口的方式添加一个自定义传感器——超声波。

已知的是超声波模块是通过串口方式发送（Tx）出数据，使用的模块数据发送周期为100ms，数据格式为：

R0034 R0122 R0122 R0046 R0127 R0044 R0044 R0125 R0034 R0037 R0041 R0122 R0122 .....

则可以通过Pixhawk板上的串口来接收（Rx）数据，即将超声波的Tx接口连接到Pixhawk板上的Rx接口。这个想法就目前来说是非常好并且实施相对最简单的，毕竟PX4源码框架很大，十分复杂，在其原有的基础上改代码需要对这套系统有着很深的理解，所有初学者确实可以采用这种“协同”的方式连入到Pixhawk，可能用一个外部运算能力强的处理器算出姿态或位置或视觉数据经过串口发送到Pixhawk中，怎么把这些数据融合进原生算法那就另说了。

OK，继续主题，Pixhawk板上串口说明如下：

NuttX UART	Pixhawk UART
/dev/ttyS0	IO DEBUG(RX ONLY)
/dev/ttyS1	TELEM1(USART2)
/dev/ttyS2	TELEM2(USART3)
/dev/ttyS3	GPS(UART4)
/dev/ttyS4	N/A(UART5, IO link)
/dev/ttyS5	SERIAL5(UART7,NSH Console Only)
/dev/ttyS6	SERIAL4(UART8)

测试使用Pixhawk板上TELEM2接口的USART2，对应的Nuttx UART设备文件尾/dev/ttyS2：

对于的各接口线序可以从这里查看，其中Pixhawk接口最左边为Vcc,最右边是GND。

TELEM1, TELEM2

Pin	Signal	Volt
1 (red)	VCC	+5V
2 (blk)	TX (OUT)	+3.3V
3 (blk)	RX (IN)	+3.3V
4 (blk)	CTS (IN)	+3.3V
5 (blk)	RTS (OUT)	+3.3V
6 (blk)	GND	GND

GPS

Pin	Signal	Volt
1 (red)	VCC	+5V
2 (blk)	TX (OUT)	+3.3V
3 (blk)	RX (IN)	+3.3V
4 (blk)	CAN2 TX	+3.3V
5 (blk)	CAN2 RX	+3.3V
6 (blk)	GND	GND

SERIAL 4/5

Pin	Signal	Volt
1 (red)	VCC	+5V
2 (blk)	TX (#4)	+3.3V
3 (blk)	RX (#4)	+3.3V
4 (blk)	TX (#5)	+3.3V
5 (blk)	RX (#5)	+3.3V
6 (blk)	GND	GND

读取数据测试

步骤：

1. 在Firmware/src/modules中添加一个新的文件夹，命名为rw_uart，并创建C文件rw_uart.c

/* 
 * 串口读取函数
 * rw_uart.c 
 */
#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>

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

    /* 以新的配置填充结构体 */
    /* 设置某个选项，那么就使用"|="运算，
     * 如果关闭某个选项就使用"&="和"~"运算
     * */
    tcgetattr(fd, &uart_config); // 获取终端参数

    /* clear ONLCR flag (which appends a CR for every LF) */
    uart_config.c_oflag &= ~ONLCR;// 将NL转换成CR(回车)-NL后输出。

    /* 无偶校验，一个停止位 */
    uart_config.c_cflag &= ~(CSTOPB | PARENB);// CSTOPB 使用两个停止位，PARENB 表示偶校验

     /* 设置波特率 */
    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;
    }
    // 设置与终端相关的参数，TCSANOW 立即改变参数
    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);
    /*Linux中，万物皆文件，打开串口设备和打开普通文件一样，使用的是open（）系统调用*/
    // 选项 O_NOCTTY 表示不能把本串口当成控制终端，否则用户的键盘输入信息将影响程序的执行
    if (serial_fd < 0) {
        err(1, "failed to open port: %s", uart_name);
        return false;
    }
//    printf("Open the %s\n",serial_fd);
    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("[JXF]set_uart_baudrate is failed\n");
        return -1;
    }
    printf("[JXF]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;
}

具体的实现过程请查看irmware/src/drivers/hott/comms.cpp，里面是关于串口的设置，设计到NuttX这个类Unix操作系统的文件系统了，小白有点难啃。

2. 在rw_uart文件夹中创建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
    )
# vim: set noet ft=cmake fenc=utf-8 ff=unix :

3. 注册新添加的应用到NuttShell中。Firmware/cmake/configs/nuttx_px4fmu-v2_default.cmake文件中添加如下内容：

modules/rw_uart

4. 编译并上传固件

make px4fmu-v2_default upload

5. 读取测试

查看app
- 在NSH终端中输入help，在Builtin Apps中出现rw_uart应用。
运行rw_uart应用（前提是模块与Pixhawk连接好）
设置好发送装置
这里我使用的是秉火STM32开发板的USART1给Pixhawk的TELEM2发送对应的字符串。根据代码，数据的格式要求很严格，不然很可能得不到数据，博主开始设置让串口疯狂的发Rxxxx，结果速度太快是不行的，后来给加了一个延时函数，更新频率设置为20Hz，有时候还是会有乱码出现。各位进行设置的时候需要注意波特率对应，数据格式按要求来。

int main(void)
{   

    /* USARTx config 9600 8-N-1 */
    USARTx_Config();    

    /* 配置SysTick为每10us中断一次 */
    SysTick_Init();

    printf("R1000 ");

  for(;;){
        int i = 1000;
        for(i=1001;i<2000;i++)
        {
            printf("R%d ",i);
            Delay_us(5000); // 5000*10us = 50ms
        }
    }
}