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

1.0

Browse Source
This commit is contained in:
nitiantuhao
2026-03-31 23:30:33 +08:00
commit 760043c8e7
1615 changed files with 1406836 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

183
App/IMU/hwt101.c Normal file
View File

@@ -0,0 +1,183 @@
#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;
}
}