RT-Thread-基于RT-Thread+RA6M4的测速仪实验平台信息整合系统RT-Thread问答社区

基于RT-Thread+RA6M4的测速仪实验平台信息整合系统

发布于 2022-07-29 17:43:46 浏览：1039 订阅该版

[TOC]

#简介

##应用背景

在现代自动化产业与信息化产业迅猛发展下，导航技术手段广泛应用于车辆、舰船、卫星等各种民用及军用平台上，而速度作为导航系统中比较重要的一个参数，其精度对系统的整体性能有重大影响。我们自主研发的新型激光多普勒测速仪可以为纯惯性导航系统提供高精度的速度值，修正纯惯性导航的累积误差，从而提高传统惯性导航系统的稳定性和准确性。作为新型惯性导航系统里的关键性器件，激光多普勒测速仪的稳定性和准确性要求很高，所以对于每套系统的测试任务很重。传统的跑车实验需要跑很远的里程来减小测试误差带来的影响。本实验平台信息整合系统就是对这种激光多普勒测速仪，及其他相关传感器的信息进行采集整合、时基对齐，自主控制车辆行进路线，减小时间不对齐、跑车路线不一致等因素带来的测试误差。替代以往的跑车实验，提高测试与标定效率。

##测试系统外观

![测试车平台截图.png](https://oss-club.rt-thread.org/uploads/20220730/a3c3197e9985589b698111d9c9ca1f52.png.webp "测试车平台截图.png")

##已实现功能

此信息整合系统基于RA6M4和RT-Thread实现了对激光多普勒测速仪的测试与标定，多种传感器数据的采集整合和上传，以及控制实验平台车辆的自主巡航，使其按照规定路线行驶等功能。

#详细说明

##整体系统框架

系统的整体框架如下图所示：

![系统框图.png](https://oss-club.rt-thread.org/uploads/20220730/ea7115c53e38a24ff0637b236d79c002.png.webp "系统框图.png")

##RT-Thread使用情况概述

RT-Thread 使用了PIN设备、UART设备、CAN设备、HWTIMER设备、SPI设备等。使用了软件定时器用于控制指令下发。线程间同步和通信使用了信号量，事件集和消息队列等。还使用了DFS虚拟文件系统。

###RT-Thread代码贡献

- 适配了HWTIMER设备驱动到RA6M4硬件平台。
- 补全了串口驱动对SerialV2设备框架的非阻塞收发适配代码。

##软硬件框架说明

本系统软硬件整体框架如下图所示

![软硬件框架图.png](https://oss-club.rt-thread.org/uploads/20220731/05d06aac9442e553124987c0cf6aa209.png "软硬件框架图.png")

在测速仪、试验车等数据解析线程中实现串口、CAN等接口的数据接收和解析，并通过发送线程整合后统一发送至上位机。图像处理单元用于对摄像头实时采集的图像进行分析，指导PID控制单元对试验车进行控制。虚拟文件系统用于保存系统参数。

##软件模块说明

本系统整体框架比较大，由于时间关系，部分功能尚在开发过程中，已经基本实现了激光多普勒测速仪的速度标定功能，且对RA6M4的硬件定时器和串口非阻塞发送驱动代码做了一些完善工作。标定原理：选取一段固定长度的标准距离，在路段的开头和末端放置激光触发器标志测速的开始和结束，对行驶过程中的测速仪速度数据进行积分得到里程数据，再与标准距离相比较即可。

###目前已经实现的功能如下

####激光触发中断和硬件定时器底层驱动

激光触发外部中断的功能是为了提高本系统的速度标定精度。因为测速仪的速度数据是以固定频率上发，所以标定开始时刻到下一帧测速仪速度数据到来之间有个微小时间段，此段时间计算需要用到外部触发和定时器计时。
为提高计时精度最好使用硬件定时器外设，RA6M4并没有硬件定时器外设的驱动，所以自己补充完善。因为时间关系，目前只将驱动文件简单放在用户文件夹下使用，还没有进行PR。有需要的小伙伴可以直接去文章末尾给出的代码下载链接获得相关代码。这里有一个小问题，RT-thread的硬件定时器驱动框架中默认了可以自由配置芯片定时器的计数频率，而RA6M4的定时器外设预分频值只能设置几档，所以有时候并不能配置出用户想要的频率，所以这里的处理方法是在驱动代码中检查，如果设置的频率不能配置到，则配置最接近的频率并修改用户传入的变量值。
**外部中断使用**
对RA6M4进行配置：

![外部中断底层配置.png](https://oss-club.rt-thread.org/uploads/20220729/95967da869018789676335ab68edf209.png.webp "外部中断底层配置.png")

![外部中断底层配置2.png](https://oss-club.rt-thread.org/uploads/20220729/7beaaf7d8c0a00a535c3d73ec16d6e8c.png "外部中断底层配置2.png")

![外部中断底层配置3.png](https://oss-club.rt-thread.org/uploads/20220729/f4118486abd167b878c8f0f9e782f05a.png.webp "外部中断底层配置3.png")

新增了外部中断14，引脚设置为P505：
```c
	......
    rt_uint32_t ex_trigger_pin1 = rt_pin_get(EX_TRIGGER_PIN1);
    rt_err_t err = rt_pin_attach_irq(ex_trigger_pin1, PIN_IRQ_MODE_RISING, ex_irq14_callback, RT_NULL);
    err = rt_pin_irq_enable(ex_trigger_pin1, PIN_IRQ_ENABLE);
    if(RT_EOK != err)
    {
        LOG_W("EX_TRIGGER ERR!");
    }
	......
```

```c
void ex_irq14_callback(void *args)
{
    rt_uint32_t tim_us;
    rt_hwtimerval_t t;

ex_trigger_flag = !ex_trigger_flag;
    rt_pin_write(LED1_PIN, !ex_trigger_flag);
    rt_device_read(hw_timer0, 0, &t, sizeof(t));//读取当前时间
    tim_us = t.sec * 1000000 + t.usec;
    rt_memcpy(irq_to_pc_buf+4, &tim_us, 4);
    irq_to_pc_buf[8] = 0X01;//外部触发  有效
    rt_mq_send(pc_send_msg, irq_to_pc_buf, sizeof(irq_to_pc_buf));
}
```
在主函数中将自定义的中断回调函数绑定到激光触发器的外部中断，回调函数中读取硬件定时器的时间值，通过消息队列发送给数据整合发送线程。

**硬件定时器使用**
对RA6M4配置:

![硬件定时器配置1.png](https://oss-club.rt-thread.org/uploads/20220729/019cd75a252a30ea3e43c7b767497b4d.png.webp "硬件定时器配置1.png")

新增r_gpt,使用通道0，周期模式，修改中断回调函数名称（修改名称是为了和驱动文件中的相关代码对应起来）。其他默认。
在"rtconfig.h"文件中定义`BSP_USING_HWTIMER`和`BSP_USING_TIM0`，打开硬件定时器。
自己完善的硬件定时器驱动文件有"drv_hwtimer.c"文件，"drv_hwtimer.h"文件和"hwtimer_config.h"三个文件。
主要实现了以下函数:

![硬件定时器相关函数.png](https://oss-club.rt-thread.org/uploads/20220729/5b72c0170e090c5f73fd8aa4e67c88df.png "硬件定时器相关函数.png")

这里暂时只写了tim0-tim3四个32位定时器，需要更多定时器外设可以参考代码自行补充。三个驱动文件加入工程中后可以直接被RT-Thread的驱动框架调用。

####串口底层驱动
本系统搭载了多款传感器，很多传感器的通信都是使用串口。RA6M4的串口驱动适配了serialV2版本驱动框架，但是并没有实现串口非阻塞发送，缺少了一个函数，所以自己完善了使用中断发送的函数。在drv_usart_v2.c文件中，添加了`rt_uart_transmit`函数供驱动RTT框架调用，同时在驱动文件的回调函数中增加发送完成相关代码。

```c
rt_size_t rt_uart_transmit(struct rt_serial_device       *serial,
                             rt_uint8_t             *buf,
                             rt_size_t               size,
                             rt_uint32_t             tx_flag)
{
    struct ra_uart *uart;
    struct rt_serial_tx_fifo *tx_fifo;
    rt_uint8_t data;
    if (size>0 && (tx_flag == RT_SERIAL_TX_NON_BLOCKING))
    {
        RT_ASSERT(serial != RT_NULL);
        uart = rt_container_of(serial, struct ra_uart, serial);
        RT_ASSERT(uart != RT_NULL);
        sci_uart_instance_ctrl_t *p_ctrl = (sci_uart_instance_ctrl_t *)uart->config->p_api_ctrl;
        tx_fifo = (struct rt_serial_tx_fifo *)serial->serial_tx;
        RT_ASSERT(tx_fifo != RT_NULL);

rt_ringbuffer_getchar(&(tx_fifo->rb),&data);

if(p_ctrl->p_reg->SCR_b.TIE != 1)
        {
            p_ctrl->p_reg->SCR_b.TIE = 1;
        }
        if(p_ctrl->p_reg->SCR_b.TE != 1)
        {
            p_ctrl->p_reg->SCR_b.TE = 1;
        }
        p_ctrl->p_reg->TDR = data;
        return 1;
    }
    return 0;
}

#ifdef BSP_USING_UART1
void user_uart1_callback(uart_callback_args_t *p_args)
{
    rt_interrupt_enter();

struct rt_serial_device *serial = &uart_obj[UART1_INDEX].serial;
    RT_ASSERT(serial != RT_NULL);
    if (UART_EVENT_RX_CHAR == p_args->event)
    {
        struct rt_serial_rx_fifo *rx_fifo;
        rx_fifo = (struct rt_serial_rx_fifo *) serial->serial_rx;
        RT_ASSERT(rx_fifo != RT_NULL);

rt_ringbuffer_putchar(&(rx_fifo->rb), (rt_uint8_t)p_args->data);

rt_hw_serial_isr(serial, RT_SERIAL_EVENT_RX_IND);
    }
    else if (UART_EVENT_TX_DATA_EMPTY == p_args->event)
    {
        rt_hw_serial_isr(serial, RT_SERIAL_EVENT_TX_DONE);
    }
    rt_interrupt_leave();
}
#endif
```

**串口使用**

RA6M4配置：

![串口硬件配置.png](https://oss-club.rt-thread.org/uploads/20220729/5070640450d0256b78842e2d5125be31.png.webp "串口硬件配置.png")

注意串口回调函数名和串口名及通道号保持一致。这里没有使用FIFO和DTC功能，串口引脚可以按自己需求更改。
RT-Thread配置：

![串口软件配置.png](https://oss-club.rt-thread.org/uploads/20220729/0ddaca0589fb8d44c76eb35499814b7f.png.webp "串口软件配置.png")

打开串口1，因为我们使用了非阻塞收发，可以添加收发缓冲区，这里缓冲区大小设置为512。注意如果使用RA6M4时没有完善非阻塞发送函数，则调用发送函数会报错，因为最后的发送函数指针为空。这样都配置完成以后就可以使用串口设备了。

####CAN接口底层驱动

试验车的数据收发使用了CAN接口。RA6M4的BSP支持了CAN外设。
RA6M4配置：

![CAN硬件配置.png](https://oss-club.rt-thread.org/uploads/20220729/1243481788113686001a47d9fc2eb7ad.png.webp "CAN硬件配置.png")

新增can0设备，通道0，扩展ID模式，依然注意回调函数名称。

![CAN硬件配置2.png](https://oss-club.rt-thread.org/uploads/20220729/e5da0b2ccf4472620c5351469f027fa9.png "CAN硬件配置2.png")

发送和接收邮箱这里根据自己的需要配置，接收的话Mailbox Type修改为接收邮箱，注意邮箱掩码，这里Mask配置成0，对报文ID不进行过滤，全部接收。

在RT-Thread中打开CAN0设备：

![CAN软件配置.png](https://oss-club.rt-thread.org/uploads/20220729/b6ab2fdd26a19231d69eb334b20c086c.png.webp "CAN软件配置.png")

配置完成后，can设备就可以开始使用了。

####DFS虚拟文件系统的使用

在系统运行过程中用到的一些参数，需要保存在FLASH芯片中，免去了每次上电都要重新设置参数的麻烦。这里使用了DFS虚拟文件系统，操作文件系统更加方便。存储芯片使用的SPI串行FLASH，W25Q64。
RA6M4配置：

新增SPI0设备，这里都使用默认配置

![SPI硬件配置.png](https://oss-club.rt-thread.org/uploads/20220729/7ca468fa494940383294cdc67efae647.png.webp "SPI硬件配置.png")

在片选引脚这里选择空，不做配置，后面使用设备的时候再关联引脚。

![SPI硬件配置2.png](https://oss-club.rt-thread.org/uploads/20220729/752f6e2bb95d6fb5000916f257923ea8.png.webp "SPI硬件配置2.png")

在RT-Thread中打开SPI、SFUD、DFS、Fatfs。相关配置如下：

![DFS软件配置.png](https://oss-club.rt-thread.org/uploads/20220729/990d3fc22411c82607d03f1fac4c41c8.png.webp "DFS软件配置.png")

![DFS软件配置2.png](https://oss-club.rt-thread.org/uploads/20220729/59c2330f0461bf814d7dc53e60ce6375.png.webp "DFS软件配置2.png")

![DFS软件配置3.png](https://oss-club.rt-thread.org/uploads/20220729/432626c43d78960fba5924a7dfd114e1.png.webp "DFS软件配置3.png")

![DFS软件配置4.png](https://oss-club.rt-thread.org/uploads/20220729/2c176f2e92fd6f873fd457d10d6a8ee2.png.webp "DFS软件配置4.png")

配置好以后将spi设备挂载到SPI总线上（注意SPI定义片选引脚），把flash挂载到虚拟文件系统，底层需要使用SFUD层驱动。并将初始化函数导出到组件初始化阶段执行。

```c
static int rt_hw_spi_flash_mount()//W25Q64 自动初始化
{
    rt_spi_bus_attach_device(&spi0_w25q64_dev, "spi01", "spi0", (void *)SPI0_CS);//spi01设备（W25Q64BV）挂载到SPI0总线上
    if (RT_NULL == rt_sfud_flash_probe("W25Q64BV", "spi01"))
    {
        return -RT_ERROR;
    }
}
INIT_COMPONENT_EXPORT(rt_hw_spi_flash_mount);
```

设备挂载成功后会打印如下信息：

![DFS挂载成功.png](https://oss-club.rt-thread.org/uploads/20220729/20e887015c1414b9d470fe419f5a1963.png "DFS挂载成功.png")

最后在使用虚拟文件系统之前，需要用户分别调用挂载和格式化文件系统两个函数，当然，格式化只需要一次，如果Flash已经被格式化为Fatfs(elm)就不需要再格式化了。

```c
int spi_flash_mount(void)
{
    int sta = dfs_mount("W25Q64BV", "/W25Q64BV", "elm", 0, 0);
    return sta;
}

int flash_mkfs(void)
{
    int sta = dfs_mkfs("elm", "W25Q64BV");//文件系统格式化
    return sta;
}
```

至此，虚拟文件系统配置完毕。可以通过标准的`open`,`close`等文件操作接口读写文件了。

####图像处理+PID控制车辆行进方向和速度

为避免人为操作导致的各种误差，加入了基于QT+OpenCV的图像处理单元，处理车载摄像头得到的实时行进画面，提取跑道中的线条。计算车辆偏差，发送给RA6M4主控制器。RA6M4主控制器接收到偏差值后，输入给PID计算单元，输出需要的车辆方向和车辆速度修正量。通过CAN接口发送给车辆底盘，使试验车可以沿着固定路线自主行驶。避免每次测试路线不同带来的测试误差。目前图像处理单元暂时在PC端实现，最后验证通过后会部署到树莓派中，增加测试的灵活性。而图像处理算法，目前也暂时只用了简单的二值化、图像模糊、膨胀等算法去提取跑道中的线条。后续可以尝试更多的算法提取跑道特征，规划行进路线以适应更丰富的测试场地。

###预期添加的功能

- 加入激光雷达传感器，实现自主避障。
- 加入激光测距仪，防止碰撞小台阶或跌落。
- 加入MEMS惯组，实现激光多普勒测速仪与MEMS IMU的惯性导航。
- 加入双目摄像头和ROS系统，实现更完善的导航与控制功能。
- 在此平台基础上，做更多的探索性实验任务。

#测试结果分析与展示

##测速仪标定与测试数据分析

这里不牵扯测速仪内部的原理，只通过数据说明标定过程。如下给出测量10组数据的结果，测量的实际里程为40米：

|  组别 | SA  | SB |
| ------------ | ------------ | ------------ |
| 20220727_01 | 3.316296015661402 | 6.162251264910002 |
| 20220727_02 | 3.3154613366042965 | 6.16169697001403 |
| 20220727_03 | 3.314371372397694 | 6.159370331549038 |
| 20220727_04 | 3.3143124228979035 | 6.159282975468019 |
| 20220727_05 | 3.315801688055289 | 6.160877277180023 |
| 20220727_06 | 3.3143059925735976 | 6.160005544556053 |
| 20220727_07 | 3.315930766045205 | 6.162273335656948 |
| 20220727_08 | 3.314265039655395 | 6.1601151204389905 |
| 20220727_09 | 3.314308291224091 | 6.160049210109284 |
| 20220727_10 | 3.315458794528004 | 6.162336033749999 |
 
 统计得到SA的平均值为：3.3150511719642877 标准差为：0.0007716238025112405
 SB的平均值为：6.160825806363239 标准差为：0.0011597144044451022
 
![screenshot_image.png](https://oss-club.rt-thread.org/uploads/20220730/b4dbe1060b3fa8728d4212de8c71b8ad.png.webp)

根据40米的准确里程值，和上面得到的SA和SB的平均值可计算内部比例因子，写入测速仪系统，再次测量10组数据如下：

| 组别 | 里程 |
| ----- | ----- |
| 20220728_01 | 40.00316236755311 |
| 20220728_02 | 40.01066236762687 |
| 20220728_03 | 39.991667367594145 |
| 20220728_04 | 40.01071236759425 |
| 20220728_05 | 40.00151786759403 |
| 20220728_06 | 39.9813534175939 |
| 20220728_07 | 39.991172522593755 |
| 20220728_08 | 39.982973538093605 |
| 20220728_09 | 40.002954655143436 |
| 20220728_10 | 39.99193388389824 |

统计分析如下，平均值为：39.99681103552854，标准差为：0.009988970047437904

![screenshot_image.png](https://oss-club.rt-thread.org/uploads/20220729/aa864cf87fc74add31dcacd7cf238e52.png.webp)

测量精度如下：

![screenshot_image.png](https://oss-club.rt-thread.org/uploads/20220729/9ef9c70ee847afcd7b1994531fc78a8d.png.webp)

##触发信号时间补偿问题

如下图所示是其中一组测试数据的A，B路速度图表。其中红色的是A路速度，蓝色的是B路速度，用绿色菱形标记的点为触发信号点。
![screenshot_image.png](https://oss-club.rt-thread.org/uploads/20220730/cef3a5831ee10e1f3e5d816581f381d7.png.webp)

**注：一下分析的都是原始速度和原始速度积分的原始里程值，并不是真正的对地水平速度和里程。只为分析时间补偿问题**
以红色的A路速度为例。放大图表看起始点，触发点前一个速度时间点为：251578231us，触发时间点为：251581927us, 触发后一个速度时间点为：251588241us。两帧测速仪数据帧间隔为10.01ms。触发点到下一个速度点时间间隔为：6.314ms。触发点前一个速度点速度为：0.0795242m/s。所以触发点到后一个速度值之间车辆走过了0.0005021157988m。

![screenshot_image.png](https://oss-club.rt-thread.org/uploads/20220730/e335299349d119095a8bf00f6f9ee23d.png)

下图是终点触发点，触发点前一个速度时间点为：294943725us，触发时间点为：294951019us,触发后一个速度时间点为：294953735us。两帧测速仪数据帧间隔也为10.01ms。触发点前一个速度点到触发点时间间隔为：7.294ms。触发点前一个速度点速度为：0.0769629m/s。所以前一个速度点到触发点之间车辆走过了0.0005613673926m。

![screenshot_image.png](https://oss-club.rt-thread.org/uploads/20220730/b53acf72050b91348a77282afe790c4a.png.webp)

这组数据未做补偿时两个触发点之间所有速度积分里程值为：3.315232532470002m。前后需补偿0.0010634831914m。补偿后里程值为：3.316296015661402m。这组数据需补偿值对结果精度的影响有0.032%。
经过上面的分析可知。触发时间需要补偿的时间最大为两帧速度数据的时间，既20ms。如此，很容易理解，测试距离越近，测试时车速越快对测试精度的影响越大。就目前的测试距离和测试车速来看，最大需补偿里程为平均车速*0.02s约为0.001539258m。最大精度影响大概为0.0464%。和测速仪自身的里程精度基本处于同一量级。而且，后面为了提高测试效率，肯定还会提高测试车速或减小测试距离。所以，补偿数据帧和触发点间的时间还是很有必要的。
另外，从上面数据，可以看到，RA6M4主控时间对齐的测速仪数据，两帧时间间隔都是10.01ms。和测速仪的100HZ数据输出率略有差异。而且，查看多帧时间间隔，会发现，这个值也不稳定，存在波动。主要原因有两个，一个是程序执行差异导致的短暂时间差异，这个误差基本都是随机分布，影响不大。另一个是RA6M4主控板并没有焊接外部时钟晶振，使用的是内部RC振荡器。时钟稳定度并没有很高。这个问题会导致帧间隔时间的慢飘。甚至测试时环境温度不同会有比较大的差异。测速仪内部使用的是4ppm的温补晶振，时钟稳定性相对高很多。所以这里其它速度帧之间的里程积分用的时间就是10ms。也就是用的测速仪内部的时基。只有触发点之间的补偿用了RA6M4的时基。等待后面自己重新做板，给RA6M4也配一个高精度的温补晶振，这里的积分时间就都可以用主控板的时基了。

这组数据的LDV数据帧间隔，数据量很少，倒是看不到慢飘问题，可以看到随机波动：
![screenshot_image.png](https://oss-club.rt-thread.org/uploads/20220730/c5d67037d092e0a65dd6e14e07086e4f.png.webp)

##RA6M4主控制器控制车辆精度分析
如下是从一组测试数据中提取的图像处理单元给出的误差`pid_err_in`和PID处理单元给出的车辆调整角度`pid_angle_out`值。其中红色曲线为`pid_err_in`，用的左侧Y轴。摄像头横向视野范围大概为±10cm，`pid_err_in`的取值范围为[-1,1]，所以Y轴单位大概为10cm。可以看到，整个过程，红线波动大概在±0.1以内，也就是车辆大概在±1cm的范围内左右摆动。蓝色为`pid_angle_out`，用的右侧Y轴。单位为度。车辆角度的调整量大概在正负1度以内。目前看来，至少要比跑车或者用遥控控制车辆精确的多。后面还可以继续优化算法，提升控制精度和测试场地适应能力。

![screenshot_image.png](https://oss-club.rt-thread.org/uploads/20220729/c156b3504ed5061cfd7b10284f7f65e7.png.webp)

##探索性测试结果分析举例

如下是用FR-07 Pro的裸车姿态数据做的航迹推算结果。实验为环绕学校的一个小运动场转了一圈。粗略看，效果还可以。但由于裸车姿态数据的航向角是通过转向电机的编码器测量得来的，车辆速度是通过驱动轮的编码器测量得来的，很容易受轮胎打滑，轮胎直径变化等因素的影响。所以结果稳定性很差。
等待此平台的测试与标定功能完善完毕后，会继续做更多的探索性实验，计划会加入MEMS惯组甚至激光陀螺惯组，差分GPS，UWB以及测速仪等传感器，进行数据融合和航迹推算，分析不同配合得到的惯性导航性能差异。从而针对不同应用场合，可以选取适合的系统搭配。

![screenshot_image.png](https://oss-club.rt-thread.org/uploads/20220730/c203834142d059613f353b51097c1faa.png.webp)

[演示视频 https://www.bilibili.com/video/BV1Rt4y1G7uB/?vd_source=143082a01c512169c40eb84df2924c9d](https://www.bilibili.com/video/BV1Rt4y1G7uB/?vd_source=143082a01c512169c40eb84df2924c9d "演示视频")

[代码地址：https://gitee.com/chengjili/ra6m4-rtthread-ldvtest](https://gitee.com/chengjili/ra6m4-rtthread-ldvtest "代码地址：https://gitee.com/chengjili/ra6m4-rtthread-ldvtest")

文章的最后，感谢大家阅读此文章，也感谢瑞萨电子和RTThread联合举办的这次开发设计大赛。祝大家工作顺利，学习愉快！