Falling_jasmine/ASER-NAV
1
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

导航部分效果最好的版本，主分支

Browse Source
This commit is contained in:
nitiantuhao
2026-04-13 23:30:22 +08:00
commit 350fd830f4
1679 changed files with 1464081 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

175
App/laser/Laser Manager.md Normal file
View File

@@ -0,0 +1,175 @@
激光雷达管理模块 (Laser Manager) 🚀
===========================
本模块专为 **STM32H7 + FreeRTOS** 环境打造，用于同时驱动和解析 4 路单点激光雷达（2 路连续流 STP-23L，2 路文本流 ATK-MS53L1M）。
采用 **DMA 环形缓冲 + 串口空闲中断 (IDLE) + RTOS 事件驱动** 架构，彻底消灭单字节中断引起的 CPU 拥堵，是适用于机器人底盘避障与路径规划的“企业级”底层驱动。
✨ 核心特性
------
1. **零中断阻塞**：所有数据均由硬件 DMA 搬运，CPU 仅在数据接收过半/完成或触发 IDLE 空闲时被唤醒。
2. **免 Cache 物理隔离**：基于 MPU 划分专用 `.dma_buffer` 特区，彻底杜绝 Cortex-M7 的 Cache 一致性导致的数据乱码或 USB 掉线问题。
3. **极速解析算法**：
* **STP 协议**：采用 O(1) 环形指针流式提取，**0 内存搬运 (`memmove`)**。
* **ATK 协议**：采用 O(1) 字符特征匹配，彻底抛弃耗时的 `strstr` 和 `strtol` 标准库函数。
4. **高质量数据输出**：内置 **滑动中值滤波 (Median Filter)**，有效剔除玻璃反光、灰尘引起的突发测距尖峰，提供平滑稳定的控制级数据。
5. **安全可靠**：
* 采用 `taskENTER_CRITICAL()` 保护快照读取，杜绝多线程数据撕裂。
* 严格区分中断上下文 (`FROM_ISR`) 与任务上下文的临界区调用。
* 内置 `HAL_UART_ErrorCallback`，在遭受电磁干扰触发 ORE/FE 错误时自动重启 DMA，永久防死机。
* * *
⚙️ 移植与配置指南
----------
### 第一步：STM32CubeMX 硬件配置
1. **MPU 设置 (极其重要)**
* 进入 `Cortex-M7` -> `MPU Control`
* Enable MPU
* **Region 1**: Base Address = **`0x30000000`**, Size = **`32KB`**
* Access Permission: **`ALL ACCESS PERMITTED`**
* Cacheable: **`Disable`** (彻底关闭该区域的 D-Cache)
2. **串口与 DMA 设置**
* **STP 雷达** (如 USART2, UART4 - 230400bps)：添加 RX DMA，模式选 **`Circular`** (数据宽度 Byte)。优先级建议设为 **`High`**。
* **ATK 雷达** (如 USART3, USART6 - 115200bps)：添加 RX DMA，模式选 **`Circular`**。优先级设为 **`Medium`**。
* **NVIC**：必须开启这 4 个串口的**全局中断 (Global Interrupt)**。
### 第二步：链接脚本 (.ld) 配置
打开工程的链接脚本（如 `STM32H743XX_FLASH.ld`），在 `RAM_D2` 区域添加 `.dma_buffer` 段定义：
代码段
/* 给雷达 DMA 专门划分的免 Cache 区域 */
.dma_buffer (NOLOAD) :
{
. = ALIGN(32);
*(.dma_buffer)
*(.dma_buffer*)
. = ALIGN(32);
} >RAM_D2
### 第三步：中断回调接管
确保在 `usart.c` 或 `main.c` 中 **删除或注释掉** 旧的串口接收回调，因为 `laser_manager.c` 底部已经重写了以下函数并接管了整个链路：
* `HAL_UART_RxCpltCallback`
* `HAL_UART_RxHalfCpltCallback`
* `HAL_UARTEx_RxEventCallback` (用于处理 IDLE)
* `HAL_UART_ErrorCallback` (用于自动恢复错误)
* * *
💻 API 使用说明
-----------
### 1. 数据结构概览
C
typedef struct {
uint16_t distance_mm; // 经过中值滤波处理后平滑的距离值 (推荐用于避障控制)
uint16_t raw_distance_mm; // 传感器吐出的原始跳动距离值 (仅供调试观察)
uint8_t valid; // 数据是否有效 (1:有效, 0:无效)
uint8_t online; // 雷达是否在线 (未超时)
uint8_t fault_code; // 故障码 (0x00:正常, 0x80:掉线超时)
uint32_t update_tick_ms; // 最后一次数据刷新的系统 tick
} laser_simple_data_t;
### 2. 初始化与任务启动
在 FreeRTOS 调度器启动前（或统一的应用初始化任务中）调用初始化接口。它会自动配置并开启 DMA，同时创建一个高优先级的解析守护任务。
C
#include "laser/laser_manager.h"
// 在 main.c 或 app_tasks.c 初始化阶段调用
LASER_SIMPLE_Init();
### 3. 数据轮询与读取 (应用层)
在你的应用周期任务中（如 `100ms` 测试任务或控制任务），调用 `Poll` 并获取快照指针：
C
void AppTasks_RunLaserTestTask_Impl(void *argument)
{
const laser_simple_snapshot_t *snap;
uint32_t print_divider = 0;
LASER_SIMPLE_Init();
for(;;)
{
// 1. 触发状态机巡检 (更新在线状态/超时)
LASER_SIMPLE_Poll(HAL_GetTick());
// 2. 获取绝对安全、无撕裂的全局数据快照
snap = LASER_SIMPLE_GetSnapshot();
// 3. 降频打印 (例如每 500ms 打印一次)
if (++print_divider >= 5)
{
print_divider = 0;
printf("F_STP [Filter:%4umm, Raw:%4umm] Valid:%d Fault:%02X\r\n",
snap->ch[LASER_CH_FRONT_STP].distance_mm,
snap->ch[LASER_CH_FRONT_STP].raw_distance_mm,
snap->ch[LASER_CH_FRONT_STP].valid,
snap->ch[LASER_CH_FRONT_STP].fault_code);
}
osDelay(100); // 维持高频巡检节拍 (切勿大于雷达设定的 Timeout 时间)
}
}
### 4. 便捷测距接口
如果你的小车只需要知道“前方/后方离障碍物最近是多少”，可以直接调用辅助接口：
C
// 获取前/后两路雷达中，最近且有效的平滑距离值 (毫米)
uint16_t front_min_dist = LASER_SIMPLE_GetFrontNearest();
uint16_t rear_min_dist = LASER_SIMPLE_GetRearNearest();
* * *
❓ 常见问题 (Troubleshooting)
------------------------
**Q: 烧录程序后，USB 虚拟串口无法识别 (或电脑提示无法识别的 USB 设备)？**
> **A:** 请检查 MPU 配置是否生效，以及链接脚本中是否正确加入了 `.dma_buffer`。如果 DMA 数组被编译器分配到了带有 D-Cache 的内存中，USB 外设会因为内存一致性冲突而死机。
**Q: 雷达数据一直是 0，且终端不停输出 FaultCode `0x80`？**
> **A:** `0x80` 代表超时掉线。请检查：
>
> 1. 雷达硬件连线（RX/TX 是否接反）。
>
> 2. `CubeMX` 中 4 个串口是否正确开启了**全局中断**（未开启中断将无法唤醒解析任务）。
>
> 3. 波特率是否匹配（STP: `230400`，ATK: `115200`）。
**Q: 想要调整中值滤波的平滑程度？**
> **A:** 在 `laser_manager.c` 顶部修改 `#define FILTER_WIN_SZ 3U`。改为 `5U` 会更平滑，但会增加几毫秒的系统响应延迟。通常 `3` 是最佳平衡点。

317
App/laser/laser_manager.c Normal file
View File

@@ -0,0 +1,317 @@
#include "laser_manager.h"
#include <string.h>
#include "cmsis_os2.h"
#include "FreeRTOS.h"
#include "task.h"
/* --- 配置区 --- */
#define STP_TIMEOUT_MS 200U
#define ATK_TIMEOUT_MS 500U
#define FILTER_WIN_SZ 3U // 中值滤波窗口大小（推荐 3 或 5）
#define FAULT_NONE 0x00
#define FAULT_STP_CHECKSUM 0x41
#define FAULT_STP_FRAME 0x42
#define FAULT_TIMEOUT 0x80
/* MPU 无 Cache 段宏定义 */
#if defined(__GNUC__)
#define LASER_DMA_RAM __attribute__((section(".dma_buffer"))) __attribute__((aligned(32)))
#else
#define LASER_DMA_RAM
#endif
/* 缓冲区大小 */
#define STP_DMA_SZ 512U
#define ATK_DMA_SZ 256U
/* 分配在无 Cache 区域的 DMA 内存 */
static LASER_DMA_RAM uint8_t g_stp_front_dma[STP_DMA_SZ];
static LASER_DMA_RAM uint8_t g_atk_front_dma[ATK_DMA_SZ];
static LASER_DMA_RAM uint8_t g_stp_rear_dma[STP_DMA_SZ];
static LASER_DMA_RAM uint8_t g_atk_rear_dma[ATK_DMA_SZ];
/* 数据节点与快照 */
typedef struct {
UART_HandleTypeDef *huart;
uint8_t *dma_buf;
uint16_t dma_sz;
uint16_t read_ptr;
uint32_t last_tick;
// 滤波队列缓存
uint16_t dist_history[FILTER_WIN_SZ];
uint8_t hist_idx;
uint8_t hist_cnt;
laser_simple_data_t data;
} laser_node_t;
static laser_node_t g_nodes[LASER_CH_MAX] = {
[LASER_CH_FRONT_STP] = { &huart2, g_stp_front_dma, STP_DMA_SZ, 0, 0, {0}, 0, 0, {0} },
[LASER_CH_FRONT_ATK] = { &huart3, g_atk_front_dma, ATK_DMA_SZ, 0, 0, {0}, 0, 0, {0} },
[LASER_CH_REAR_STP] = { &huart4, g_stp_rear_dma, STP_DMA_SZ, 0, 0, {0}, 0, 0, {0} },
[LASER_CH_REAR_ATK] = { &huart6, g_atk_rear_dma, ATK_DMA_SZ, 0, 0, {0}, 0, 0, {0} }
};
static laser_simple_snapshot_t g_snapshot;
static osThreadId_t laserTaskHandle;
/* --- 辅助宏与函数 --- */
#define READ_RING(buf, sz, pos, offset) (buf[((pos) + (offset)) % (sz)])
static uint16_t le16_read(const uint8_t *p) { return (uint16_t)(p[0] | (p[1] << 8)); }
static uint16_t apply_median_filter(laser_node_t *n, uint16_t new_val) {
n->dist_history[n->hist_idx] = new_val;
n->hist_idx = (n->hist_idx + 1) % FILTER_WIN_SZ;
if (n->hist_cnt < FILTER_WIN_SZ) n->hist_cnt++;
uint16_t temp[FILTER_WIN_SZ];
for (int i = 0; i < n->hist_cnt; i++) temp[i] = n->dist_history[i];
for (int i = 0; i < n->hist_cnt - 1; i++) {
for (int j = i + 1; j < n->hist_cnt; j++) {
if (temp[i] > temp[j]) {
uint16_t t = temp[i]; temp[i] = temp[j]; temp[j] = t;
}
}
}
return temp[n->hist_cnt / 2];
}
/* --- STP 环形无 memmove 解析 (保持不变) --- */
static void process_stp(laser_channel_t ch) {
laser_node_t *n = &g_nodes[ch];
uint16_t write_ptr = n->dma_sz - __HAL_DMA_GET_COUNTER(n->huart->hdmarx);
uint16_t available = (write_ptr - n->read_ptr + n->dma_sz) % n->dma_sz;
while (available >= 11U) {
uint16_t head = 0xFFFF;
for (uint16_t i = 0; i <= available - 4; i++) {
if (READ_RING(n->dma_buf, n->dma_sz, n->read_ptr, i) == 0xAA &&
READ_RING(n->dma_buf, n->dma_sz, n->read_ptr, i+1) == 0xAA &&
READ_RING(n->dma_buf, n->dma_sz, n->read_ptr, i+2) == 0xAA &&
READ_RING(n->dma_buf, n->dma_sz, n->read_ptr, i+3) == 0xAA) {
head = i; break;
}
}
if (head == 0xFFFF) {
n->read_ptr = (n->read_ptr + available - 3) % n->dma_sz;
break;
}
n->read_ptr = (n->read_ptr + head) % n->dma_sz;
available -= head;
if (available < 11) break;
uint8_t data_len = READ_RING(n->dma_buf, n->dma_sz, n->read_ptr, 8);
uint16_t frame_len = 10 + data_len + 1;
if (data_len > 200) { n->read_ptr = (n->read_ptr + 1) % n->dma_sz; available--; continue; }
if (available < frame_len) break;
uint8_t frame[256];
uint32_t sum = 0;
for (uint16_t i = 0; i < frame_len; i++) {
frame[i] = READ_RING(n->dma_buf, n->dma_sz, n->read_ptr, i);
if (i >= 4 && i < frame_len - 1) sum += frame[i];
}
if ((sum & 0xFF) == frame[frame_len - 1] && frame[5] == 0x02 && data_len == 0xB8) {
uint32_t p = 10;
uint16_t best = 0xFFFF;
for (int i = 0; i < 12; i++) {
int16_t dist = le16_read(&frame[p]); p += 8;
uint8_t conf = frame[p]; p += 7;
if (dist >= 70 && dist <= 7500 && conf >= 1 && dist < best) best = dist;
}
taskENTER_CRITICAL();
if (best != 0xFFFF) {
n->data.raw_distance_mm = best;
n->data.distance_mm = apply_median_filter(n, best);
n->data.valid = 1;
n->data.fault_code = 0;
} else {
n->data.valid = 0;
}
n->last_tick = HAL_GetTick();
taskEXIT_CRITICAL();
}
n->read_ptr = (n->read_ptr + frame_len) % n->dma_sz;
available -= frame_len;
}
}
/* --- 【全新】ATK 环形滑动窗口解析 (无视断帧与粘包) --- */
static void process_atk(laser_channel_t ch) {
laser_node_t *n = &g_nodes[ch];
uint16_t write_ptr = n->dma_sz - __HAL_DMA_GET_COUNTER(n->huart->hdmarx);
uint16_t available = (write_ptr - n->read_ptr + n->dma_sz) % n->dma_sz;
// ATK 哪怕报故障，最小长度也有类似 "State:1\r\n" 约 9 字节
while (available >= 5) {
char c0 = (char)READ_RING(n->dma_buf, n->dma_sz, n->read_ptr, 0);
char c1 = (char)READ_RING(n->dma_buf, n->dma_sz, n->read_ptr, 1);
// 匹配帧头 "d:" 或 "St" (State的开头)
if ((c0 == 'd' && c1 == ':') || (c0 == 'S' && c1 == 't')) {
// 寻找帧尾 '\n'
uint16_t lf_pos = 0xFFFF;
for (uint16_t i = 0; i < available; i++) {
if (READ_RING(n->dma_buf, n->dma_sz, n->read_ptr, i) == '\n') {
lf_pos = i;
break;
}
}
// 如果没找到换行符，说明这帧 DMA 还没搬运完，立刻退出等下次任务周期
if (lf_pos == 0xFFFF) {
break;
}
uint16_t frame_len = lf_pos + 1;
char line[64] = {0};
for (uint16_t i = 0; i < frame_len && i < sizeof(line) - 1; i++) {
line[i] = READ_RING(n->dma_buf, n->dma_sz, n->read_ptr, i);
}
taskENTER_CRITICAL();
if (line[0] == 'S' && line[1] == 't') {
int state_val = line[6] - '0';
if (state_val >= 0 && state_val <= 9) {
n->data.fault_code = state_val;
if (state_val != 0) n->data.valid = 0;
n->last_tick = HAL_GetTick();
}
}
else if (line[0] == 'd' && line[1] == ':') {
int dist_val = 0;
const char *p = &line[2];
while (*p == ' ') p++;
while (*p >= '0' && *p <= '9') {
dist_val = dist_val * 10 + (*p - '0');
p++;
}
if (dist_val > 0 && n->data.fault_code == 0) {
n->data.raw_distance_mm = dist_val;
n->data.distance_mm = apply_median_filter(n, dist_val);
n->data.valid = 1;
} else {
n->data.valid = 0;
}
n->last_tick = HAL_GetTick();
}
taskEXIT_CRITICAL();
// 成功解析一帧，滑动读取指针
n->read_ptr = (n->read_ptr + frame_len) % n->dma_sz;
available -= frame_len;
} else {
// 没找到帧头，说明有垃圾数据，把指针往后挪 1 字节继续找
n->read_ptr = (n->read_ptr + 1) % n->dma_sz;
available--;
}
}
}
/* --- 核心 RTOS 任务 (全轮询模式) --- */
static void LaserTask(void *arg) {
(void)arg;
for(;;) {
// 【架构升级】：不再死等中断标志，而是以 10ms 的心跳主动去 DMA 缓冲区提货。
// 50Hz 等于 20ms 来一帧，10ms 巡视一次性能富裕且绝不漏包。
osDelay(10);
uint32_t now = HAL_GetTick();
// 纯 DMA 内存操作，极快
process_stp(LASER_CH_FRONT_STP);
process_stp(LASER_CH_REAR_STP);
process_atk(LASER_CH_FRONT_ATK);
process_atk(LASER_CH_REAR_ATK);
// 统一心跳/超时/快照检查
for (int i = 0; i < LASER_CH_MAX; i++) {
uint32_t timeout = (i == LASER_CH_FRONT_STP || i == LASER_CH_REAR_STP) ? STP_TIMEOUT_MS : ATK_TIMEOUT_MS;
taskENTER_CRITICAL();
if (now - g_nodes[i].last_tick > timeout) {
g_nodes[i].data.online = 0;
g_nodes[i].data.valid = 0;
g_nodes[i].data.fault_code = FAULT_TIMEOUT;
g_nodes[i].hist_cnt = 0; // 掉线时清空历史滤波数组
} else {
g_nodes[i].data.online = 1;
g_nodes[i].data.update_tick_ms = g_nodes[i].last_tick;
}
g_snapshot.ch[i] = g_nodes[i].data;
taskEXIT_CRITICAL();
}
}
}
/* --- API 接口 --- */
void LASER_SIMPLE_Init(void) {
memset(&g_snapshot, 0, sizeof(g_snapshot));
// 【重要变更】：全员使用普通循环 DMA，彻底抛弃 IDLE 接收
HAL_UART_Receive_DMA(g_nodes[LASER_CH_FRONT_STP].huart, g_nodes[LASER_CH_FRONT_STP].dma_buf, STP_DMA_SZ);
HAL_UART_Receive_DMA(g_nodes[LASER_CH_REAR_STP].huart, g_nodes[LASER_CH_REAR_STP].dma_buf, STP_DMA_SZ);
HAL_UART_Receive_DMA(g_nodes[LASER_CH_FRONT_ATK].huart, g_nodes[LASER_CH_FRONT_ATK].dma_buf, ATK_DMA_SZ);
HAL_UART_Receive_DMA(g_nodes[LASER_CH_REAR_ATK].huart, g_nodes[LASER_CH_REAR_ATK].dma_buf, ATK_DMA_SZ);
osThreadAttr_t attr = { .name = "LaserTsk", .stack_size = 1024 * 4, .priority = osPriorityAboveNormal };
laserTaskHandle = osThreadNew(LaserTask, NULL, &attr);
}
void LASER_SIMPLE_Poll(uint32_t tick_ms) {
(void)tick_ms;
}
const laser_simple_snapshot_t *LASER_SIMPLE_GetSnapshot(void) {
return &g_snapshot;
}
static uint16_t nearest_of_two(const laser_simple_data_t *a, const laser_simple_data_t *b) {
if (a->online && a->valid && b->online && b->valid) {
return (a->distance_mm < b->distance_mm) ? a->distance_mm : b->distance_mm;
}
if (a->online && a->valid) return a->distance_mm;
if (b->online && b->valid) return b->distance_mm;
return 0U;
}
uint16_t LASER_SIMPLE_GetFrontNearest(void) {
return nearest_of_two(&g_snapshot.ch[LASER_CH_FRONT_STP], &g_snapshot.ch[LASER_CH_FRONT_ATK]);
}
uint16_t LASER_SIMPLE_GetRearNearest(void) {
return nearest_of_two(&g_snapshot.ch[LASER_CH_REAR_STP], &g_snapshot.ch[LASER_CH_REAR_ATK]);
}
/* --- 中断桥接 (已大幅瘦身，仅保留错误恢复) --- */
// 因为改为轮询模式，不再需要依赖这几个正常接收的中断了。保留空函数防止外部调用报错。
void LASER_UART_RxCpltCallback(UART_HandleTypeDef *huart) { (void)huart; }
void LASER_UART_RxHalfCpltCallback(UART_HandleTypeDef *huart) { (void)huart; }
void LASER_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size) { (void)huart; (void)Size; }
// 错误自动恢复保留，这是底层硬件 ORE 报错时重启 DMA 必需的
void LASER_UART_ErrorRecovery(UART_HandleTypeDef *huart) {
for (int i=0; i<LASER_CH_MAX; i++) {
if (g_nodes[i].huart == huart) {
HAL_UART_DMAStop(huart);
// 统一恢复为普通 DMA 接收
if (i == LASER_CH_FRONT_STP || i == LASER_CH_REAR_STP)
HAL_UART_Receive_DMA(huart, g_nodes[i].dma_buf, STP_DMA_SZ);
else
HAL_UART_Receive_DMA(huart, g_nodes[i].dma_buf, ATK_DMA_SZ);
}
}
}

53
App/laser/laser_manager.h Normal file
View File

@@ -0,0 +1,53 @@
#ifndef __LASER_SIMPLE_H
#define __LASER_SIMPLE_H
#include <stdint.h>
#include "usart.h"
#ifdef __cplusplus
extern "C" {
#endif
typedef enum {
LASER_CH_FRONT_STP = 0,
LASER_CH_FRONT_ATK,
LASER_CH_REAR_STP,
LASER_CH_REAR_ATK,
LASER_CH_MAX
} laser_channel_t;
typedef struct {
uint16_t distance_mm; // [新增] 经过中值滤波处理后平滑的距离值 (推荐用于避障控制)
uint16_t raw_distance_mm; // [新增] 传感器吐出的原始距离值 (供调试或特殊算法读取)
uint8_t valid; // 数据是否有效
uint8_t online; // 雷达是否在线(未超时)
uint8_t fault_code; // 故障码 (0为正常，0x80为超时)
uint32_t update_tick_ms; // 最后一次数据刷新的系统节拍
} laser_simple_data_t;
typedef struct {
laser_simple_data_t ch[LASER_CH_MAX];
} laser_simple_snapshot_t;
/* 初始化并启动 DMA 及解析任务 */
void LASER_SIMPLE_Init(void);
/* 周期调用 (实际为空，保留接口兼容) */
void LASER_SIMPLE_Poll(uint32_t tick_ms);
/* 获取统一快照 (无数据撕裂风险) */
const laser_simple_snapshot_t *LASER_SIMPLE_GetSnapshot(void);
/* 获取前后最小有效距离 (使用滤波后的值) */
uint16_t LASER_SIMPLE_GetFrontNearest(void);
uint16_t LASER_SIMPLE_GetRearNearest(void);
/* --- 新增：暴露给全局回调分发器的专用接口 --- */
void LASER_UART_RxCpltCallback(UART_HandleTypeDef *huart);
void LASER_UART_RxHalfCpltCallback(UART_HandleTypeDef *huart);
void LASER_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size);
void LASER_UART_ErrorRecovery(UART_HandleTypeDef *huart);
#ifdef __cplusplus
}
#endif
#endif