317 lines
12 KiB
C
317 lines
12 KiB
C
#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);
|
||
}
|
||
}
|
||
} |