183 lines
6.6 KiB
C
183 lines
6.6 KiB
C
#include "hwt101.h"
|
||
|
||
#include <stdbool.h>
|
||
#include "cmsis_os2.h"
|
||
#include "FreeRTOS.h"
|
||
#include "stm32h7xx_hal_uart.h"
|
||
#include "task.h"
|
||
|
||
/* 寄存器地址 */
|
||
#define REG_SAVE 0x00
|
||
#define REG_RRATE 0x03
|
||
#define REG_BAUD 0x04
|
||
#define REG_KEY 0x69
|
||
#define REG_CALIYAW 0x76
|
||
|
||
/* D2 域特区分配 */
|
||
__attribute__((section(".dma_buffer"))) __attribute__((aligned(32)))
|
||
static uint8_t s_dma_buf[HWT101_DMA_BUF_SIZE];
|
||
|
||
/* 内部静态变量 */
|
||
static UART_HandleTypeDef *s_huart = NULL;
|
||
static HWT101_Data_t s_data = {0};
|
||
static uint16_t s_read_ptr = 0;
|
||
static uint8_t s_frame[11];
|
||
static uint8_t s_frame_idx = 0;
|
||
|
||
/* yaw unwrap 状态：将 [-180, +180) 的原始 yaw 转为连续角度 */
|
||
static float s_yaw_prev = 0.0f; // 上一次原始 yaw (deg)
|
||
static float s_yaw_continuous = 0.0f; // 累计连续角度 (deg)
|
||
static bool s_yaw_initialized = false; // 首次标记
|
||
|
||
void HWT101_Init(UART_HandleTypeDef *huart) {
|
||
if (huart == NULL) return;
|
||
s_huart = huart;
|
||
s_read_ptr = 0;
|
||
|
||
// 【终极修复 1】：强制使用 32 位指针，将这块 (NOLOAD) 内存全部写 0。
|
||
// 必须是 32 位写入！这会彻底激活 RAM_D2 的硬件 ECC，消灭上电时的随机乱码，防止 DMA 单字节写入引发总线错误。
|
||
uint32_t *p32 = (uint32_t *)s_dma_buf;
|
||
for (uint32_t i = 0; i < (HWT101_DMA_BUF_SIZE / 4); i++) {
|
||
p32[i] = 0;
|
||
}
|
||
|
||
// ⚠️ 【终极修复 2】：因为 .dma_buffer 所在的 0x30000000 已被 MPU 配置为 Device/Non-Cacheable 内存，
|
||
// 绝对不能调用 SCB_InvalidateDCache_by_Addr，否则 Cortex-M7 会判定越权操作直接进入 HardFault 死机！
|
||
// (已删除原有的 SCB_InvalidateDCache_by_Addr 代码)
|
||
|
||
HAL_UART_Receive_DMA(s_huart, s_dma_buf, HWT101_DMA_BUF_SIZE);
|
||
}
|
||
|
||
void HWT101_Process(void) {
|
||
if (s_huart == NULL) return;
|
||
|
||
// 获取当前 DMA 写指针
|
||
uint16_t write_ptr = HWT101_DMA_BUF_SIZE - (uint16_t)__HAL_DMA_GET_COUNTER(s_huart->hdmarx);
|
||
|
||
// ⚠️ 【终极修复 3】：删除原有的刷 Cache 代码。
|
||
// 依靠硬件 MPU 免 Cache 机制，CPU 读到的必定是 DMA 搬运过来的最新数据，不会有数据一致性问题。
|
||
|
||
while (s_read_ptr != write_ptr) {
|
||
uint8_t byte = s_dma_buf[s_read_ptr];
|
||
s_read_ptr = (s_read_ptr + 1) % HWT101_DMA_BUF_SIZE;
|
||
|
||
// 状态机加速：如果不是帧头，直接跳过，不用进入后续逻辑
|
||
if (s_frame_idx == 0 && byte != 0x55) continue;
|
||
|
||
s_frame[s_frame_idx++] = byte;
|
||
if (s_frame_idx >= 11) {
|
||
uint8_t sum = 0;
|
||
for (int i = 0; i < 10; i++) sum += s_frame[i];
|
||
|
||
if (sum == s_frame[10]) {
|
||
// 更新全局变量时使用临界区保护，防止数据撕裂
|
||
taskENTER_CRITICAL();
|
||
if (s_frame[1] == 0x52) {
|
||
int16_t wz_raw = (int16_t)((s_frame[7] << 8) | s_frame[6]);
|
||
s_data.wz = (float)wz_raw / 32768.0f * 2000.0f;
|
||
} else if (s_frame[1] == 0x53) {
|
||
int16_t yaw_raw = (int16_t)((s_frame[7] << 8) | s_frame[6]);
|
||
float yaw_deg = (float)yaw_raw / 32768.0f * 180.0f;
|
||
|
||
/* 保存原始 yaw ([-180, +180) 度) */
|
||
s_data.yaw = yaw_deg;
|
||
|
||
/* ---- yaw unwrap: 消除 ±180° 跳变，生成连续角度 ---- */
|
||
if (!s_yaw_initialized) {
|
||
s_yaw_continuous = yaw_deg;
|
||
s_yaw_prev = yaw_deg;
|
||
s_yaw_initialized = true;
|
||
} else {
|
||
float diff = yaw_deg - s_yaw_prev;
|
||
if (diff > 180.0f) diff -= 360.0f;
|
||
if (diff < -180.0f) diff += 360.0f;
|
||
s_yaw_continuous += diff;
|
||
s_yaw_prev = yaw_deg;
|
||
}
|
||
s_data.yaw_continuous = s_yaw_continuous;
|
||
|
||
s_data.last_update = HAL_GetTick();
|
||
s_data.online = 1;
|
||
}
|
||
taskEXIT_CRITICAL();
|
||
}
|
||
s_frame_idx = 0;
|
||
}
|
||
}
|
||
|
||
// 超时检测
|
||
if (HAL_GetTick() - s_data.last_update > 200) {
|
||
s_data.online = 0;
|
||
}
|
||
}
|
||
|
||
// 安全的数据获取接口
|
||
void HWT101_GetData(HWT101_Data_t *out_data) {
|
||
if (out_data == NULL) return;
|
||
// 关中断拷贝，绝对防止浮点数撕裂
|
||
taskENTER_CRITICAL();
|
||
*out_data = s_data;
|
||
taskEXIT_CRITICAL();
|
||
}
|
||
|
||
/* ==========================================
|
||
* 传感器配置函数 (与原版保持一致)
|
||
* ========================================== */
|
||
static HAL_StatusTypeDef HWT101_Unlock(void) {
|
||
uint8_t cmd[5] = {0xFF, 0xAA, REG_KEY, 0x88, 0xB5};
|
||
return HAL_UART_Transmit(s_huart, cmd, 5, 20);
|
||
}
|
||
|
||
static HAL_StatusTypeDef HWT101_Save(void) {
|
||
uint8_t cmd[5] = {0xFF, 0xAA, REG_SAVE, 0x00, 0x00};
|
||
HAL_StatusTypeDef res = HAL_UART_Transmit(s_huart, cmd, 5, 20);
|
||
osDelay(200);
|
||
return res;
|
||
}
|
||
|
||
static HAL_StatusTypeDef HWT101_WriteReg(uint8_t reg, int16_t value) {
|
||
if (HWT101_Unlock() != HAL_OK) return HAL_ERROR;
|
||
osDelay(20);
|
||
uint8_t cmd[5] = {0xFF, 0xAA, reg, (uint8_t)(value & 0xFF), (uint8_t)(value >> 8)};
|
||
return HAL_UART_Transmit(s_huart, cmd, 5, 20);
|
||
}
|
||
|
||
HAL_StatusTypeDef HWT101_Config(HWT101_Rate_t rate, HWT101_Baud_t baud) {
|
||
if (HWT101_WriteReg(REG_RRATE, (int16_t)rate) != HAL_OK) return HAL_ERROR;
|
||
osDelay(100);
|
||
if (HWT101_WriteReg(REG_BAUD, (int16_t)baud) != HAL_OK) return HAL_ERROR;
|
||
osDelay(100);
|
||
return HWT101_Save();
|
||
}
|
||
|
||
HAL_StatusTypeDef HWT101_ZeroYaw(void) {
|
||
if (HWT101_WriteReg(REG_CALIYAW, 0x0000) != HAL_OK) return HAL_ERROR;
|
||
osDelay(500);
|
||
|
||
/* 重置 unwrap 状态，下次收到帧时重新初始化连续角度 */
|
||
taskENTER_CRITICAL();
|
||
s_yaw_initialized = false;
|
||
s_yaw_continuous = 0.0f;
|
||
s_yaw_prev = 0.0f;
|
||
s_data.yaw_continuous = 0.0f;
|
||
taskEXIT_CRITICAL();
|
||
|
||
return HWT101_Save();
|
||
}
|
||
|
||
void HWT101_ErrorRecovery(UART_HandleTypeDef *huart) {
|
||
if (s_huart != NULL && huart == s_huart) {
|
||
HAL_UART_DMAStop(s_huart);
|
||
|
||
// 暴力清除 ORE/NE/FE/PE 错误标志，防止无限死循环中断风暴
|
||
__HAL_UART_CLEAR_FLAG(s_huart, UART_CLEAR_OREF | UART_CLEAR_NEF | UART_CLEAR_PEF | UART_CLEAR_FEF);
|
||
// 读取一次 RDR 寄存器，把里面卡住的垃圾字节倒掉
|
||
volatile uint32_t dump = s_huart->Instance->RDR;
|
||
(void)dump;
|
||
|
||
// 干净地重新启动 DMA
|
||
HAL_UART_Receive_DMA(s_huart, s_dma_buf, HWT101_DMA_BUF_SIZE);
|
||
s_read_ptr = 0;
|
||
s_frame_idx = 0;
|
||
}
|
||
} |