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

483
App/Can/snc_can_app.c Normal file
View File

@@ -0,0 +1,483 @@
#include "snc_can_app.h"
#include <string.h>
#include <math.h>
#include "FreeRTOS.h"
#include "task.h"
/* ========================= 外部句柄 ========================= */
extern FDCAN_HandleTypeDef hfdcan1;
/* ========================= 在线判定参数 ========================= */
#define SNC_STATUS_TIMEOUT_MS 200U
#define SNC_ACTUAL_RPM_TIMEOUT_MS 200U
#define SNC_TARGET_RPM_TIMEOUT_MS 200U
#define SNC_COMM_DIAG_TIMEOUT_MS 300U
/* ========================= FDCAN 过滤器索引 ========================= */
#define SNC_FILTER_INDEX_STATUS 0U
#define SNC_FILTER_INDEX_ACTUAL_RPM 1U
#define SNC_FILTER_INDEX_TARGET_RPM 2U
#define SNC_FILTER_INDEX_COMM_DIAG 3U
#define SNC_FILTER_INDEX_ODOM 4U
SNC_CAN_AppContext_t g_snc_can_app;
/* ========================= CRC8-SAE J1850 查表 ========================= */
static const uint8_t s_crc8_j1850_table[256] =
{
0x00U, 0x1DU, 0x3AU, 0x27U, 0x74U, 0x69U, 0x4EU, 0x53U, 0xE8U, 0xF5U, 0xD2U, 0xCFU, 0x9CU, 0x81U, 0xA6U, 0xBBU,
0xCDU, 0xD0U, 0xF7U, 0xEAU, 0xB9U, 0xA4U, 0x83U, 0x9EU, 0x25U, 0x38U, 0x1FU, 0x02U, 0x51U, 0x4CU, 0x6BU, 0x76U,
0x87U, 0x9AU, 0xBDU, 0xA0U, 0xF3U, 0xEEU, 0xC9U, 0xD4U, 0x6FU, 0x72U, 0x55U, 0x48U, 0x1BU, 0x06U, 0x21U, 0x3CU,
0x4AU, 0x57U, 0x70U, 0x6DU, 0x3EU, 0x23U, 0x04U, 0x19U, 0xA2U, 0xBFU, 0x98U, 0x85U, 0xD6U, 0xCBU, 0xECU, 0xF1U,
0x13U, 0x0EU, 0x29U, 0x34U, 0x67U, 0x7AU, 0x5DU, 0x40U, 0xFBU, 0xE6U, 0xC1U, 0xDCU, 0x8FU, 0x92U, 0xB5U, 0xA8U,
0xDEU, 0xC3U, 0xE4U, 0xF9U, 0xAAU, 0xB7U, 0x90U, 0x8DU, 0x36U, 0x2BU, 0x0CU, 0x11U, 0x42U, 0x5FU, 0x78U, 0x65U,
0x94U, 0x89U, 0xAEU, 0xB3U, 0xE0U, 0xFDU, 0xDAU, 0xC7U, 0x7CU, 0x61U, 0x46U, 0x5BU, 0x08U, 0x15U, 0x32U, 0x2FU,
0x59U, 0x44U, 0x63U, 0x7EU, 0x2DU, 0x30U, 0x17U, 0x0AU, 0xB1U, 0xACU, 0x8BU, 0x96U, 0xC5U, 0xD8U, 0xFFU, 0xE2U,
0x26U, 0x3BU, 0x1CU, 0x01U, 0x52U, 0x4FU, 0x68U, 0x75U, 0xCEU, 0xD3U, 0xF4U, 0xE9U, 0xBAU, 0xA7U, 0x80U, 0x9DU,
0xEBU, 0xF6U, 0xD1U, 0xCCU, 0x9FU, 0x82U, 0xA5U, 0xB8U, 0x03U, 0x1EU, 0x39U, 0x24U, 0x77U, 0x6AU, 0x4DU, 0x50U,
0xA1U, 0xBCU, 0x9BU, 0x86U, 0xD5U, 0xC8U, 0xEFU, 0xF2U, 0x49U, 0x54U, 0x73U, 0x6EU, 0x3DU, 0x20U, 0x07U, 0x1AU,
0x6CU, 0x71U, 0x56U, 0x4BU, 0x18U, 0x05U, 0x22U, 0x3FU, 0x84U, 0x99U, 0xBEU, 0xA3U, 0xF0U, 0xEDU, 0xCAU, 0xD7U,
0x35U, 0x28U, 0x0FU, 0x12U, 0x41U, 0x5CU, 0x7BU, 0x66U, 0xDDU, 0xC0U, 0xE7U, 0xFAU, 0xA9U, 0xB4U, 0x93U, 0x8EU,
0xF8U, 0xE5U, 0xC2U, 0xDFU, 0x8CU, 0x91U, 0xB6U, 0xABU, 0x10U, 0x0DU, 0x2AU, 0x37U, 0x64U, 0x79U, 0x5EU, 0x43U,
0xB2U, 0xAFU, 0x88U, 0x95U, 0xC6U, 0xDBU, 0xFCU, 0xE1U, 0x5AU, 0x47U, 0x60U, 0x7DU, 0x2EU, 0x33U, 0x14U, 0x09U,
0x7FU, 0x62U, 0x45U, 0x58U, 0x0BU, 0x16U, 0x31U, 0x2CU, 0x97U, 0x8AU, 0xADU, 0xB0U, 0xE3U, 0xFEU, 0xD9U, 0xC4U
};
static inline int16_t snc_read_i16_le(uint8_t lo, uint8_t hi)
{
return (int16_t)((uint16_t)lo | ((uint16_t)hi << 8));
}
static inline void snc_write_i16_le(uint8_t *dst, int16_t val)
{
dst[0] = (uint8_t)(val & 0xFF);
dst[1] = (uint8_t)((uint16_t)val >> 8);
}
static int16_t snc_saturate_float_to_i16(float x)
{
if (x > 32767.0f) return 32767;
if (x < -32768.0f) return -32768;
// 手动实现四舍五入，避免调用 lroundf 库函数
return (int16_t)(x >= 0.0f ? (x + 0.5f) : (x - 0.5f));
}
uint8_t SNC_CAN_Crc8J1850(const uint8_t *data, uint16_t len)
{
uint8_t crc = 0xFFU;
while (len-- > 0U)
{
crc = s_crc8_j1850_table[(uint8_t)(crc ^ *data++)];
}
return (uint8_t)(crc ^ 0xFFU);
}
static uint32_t snc_fdcan_dlc_from_bytes(uint8_t dlc_bytes)
{
switch (dlc_bytes)
{
case 0: return FDCAN_DLC_BYTES_0;
case 1: return FDCAN_DLC_BYTES_1;
case 2: return FDCAN_DLC_BYTES_2;
case 3: return FDCAN_DLC_BYTES_3;
case 4: return FDCAN_DLC_BYTES_4;
case 5: return FDCAN_DLC_BYTES_5;
case 6: return FDCAN_DLC_BYTES_6;
case 7: return FDCAN_DLC_BYTES_7;
case 8: return FDCAN_DLC_BYTES_8;
default: return FDCAN_DLC_BYTES_8;
}
}
static HAL_StatusTypeDef snc_fdcan_add_tx_std(uint16_t std_id, const uint8_t *data, uint8_t dlc_bytes)
{
FDCAN_TxHeaderTypeDef txHeader;
memset(&txHeader, 0, sizeof(txHeader));
txHeader.Identifier = std_id;
txHeader.IdType = FDCAN_STANDARD_ID;
txHeader.TxFrameType = FDCAN_DATA_FRAME;
txHeader.DataLength = snc_fdcan_dlc_from_bytes(dlc_bytes);
txHeader.ErrorStateIndicator = FDCAN_ESI_ACTIVE;
txHeader.BitRateSwitch = FDCAN_BRS_OFF;
txHeader.FDFormat = FDCAN_CLASSIC_CAN;
txHeader.TxEventFifoControl = FDCAN_NO_TX_EVENTS;
txHeader.MessageMarker = 0U;
/* 先判断 TX FIFO/Queue 是否有空位，避免异常状态下继续硬塞 */
if (HAL_FDCAN_GetTxFifoFreeLevel(&hfdcan1) == 0U)
{
return HAL_BUSY;
}
return HAL_FDCAN_AddMessageToTxFifoQ(&hfdcan1, &txHeader, (uint8_t *)data);
}
static void snc_fdcan_config_filter(uint32_t index, uint16_t std_id)
{
FDCAN_FilterTypeDef sFilter;
sFilter.IdType = FDCAN_STANDARD_ID;
sFilter.FilterIndex = index;
sFilter.FilterType = FDCAN_FILTER_MASK;
sFilter.FilterConfig = FDCAN_FILTER_TO_RXFIFO0;
sFilter.FilterID1 = std_id;
sFilter.FilterID2 = 0x7FFU; /* 完全匹配 11-bit ID */
if (HAL_FDCAN_ConfigFilter(&hfdcan1, &sFilter) != HAL_OK)
{
Error_Handler();
}
}
static void snc_parse_status(const uint8_t *d)
{
g_snc_can_app.status.state = d[0];
g_snc_can_app.status.health = d[1];
g_snc_can_app.status.diag_bits =
((uint32_t)d[2]) |
((uint32_t)d[3] << 8) |
((uint32_t)d[4] << 16) |
((uint32_t)d[5] << 24);
g_snc_can_app.status.cmd_age_10ms = d[6];
g_snc_can_app.status.status_counter = d[7];
g_snc_can_app.status.last_update_ms = HAL_GetTick();
g_snc_can_app.status.online = true;
}
static void snc_parse_actual_rpm(const uint8_t *d)
{
g_snc_can_app.actual_rpm.fl = snc_read_i16_le(d[0], d[1]);
g_snc_can_app.actual_rpm.rl = snc_read_i16_le(d[2], d[3]);
g_snc_can_app.actual_rpm.fr = snc_read_i16_le(d[4], d[5]);
g_snc_can_app.actual_rpm.rr = snc_read_i16_le(d[6], d[7]);
g_snc_can_app.actual_rpm.last_update_ms = HAL_GetTick();
}
static void snc_parse_target_rpm(const uint8_t *d)
{
g_snc_can_app.target_rpm.fl = snc_read_i16_le(d[0], d[1]);
g_snc_can_app.target_rpm.rl = snc_read_i16_le(d[2], d[3]);
g_snc_can_app.target_rpm.fr = snc_read_i16_le(d[4], d[5]);
g_snc_can_app.target_rpm.rr = snc_read_i16_le(d[6], d[7]);
g_snc_can_app.target_rpm.last_update_ms = HAL_GetTick();
}
static void snc_parse_comm_diag(const uint8_t *d)
{
g_snc_can_app.comm_diag.valid_cmd_total_lsb = d[0];
g_snc_can_app.comm_diag.crc_error_total_lsb = d[1];
g_snc_can_app.comm_diag.counter_reject_total_lsb = d[2];
g_snc_can_app.comm_diag.can_tx_drop_total_lsb = d[3];
g_snc_can_app.comm_diag.busoff_total_lsb = d[4];
g_snc_can_app.comm_diag.rx_overrun_total_lsb = d[5];
g_snc_can_app.comm_diag.last_accepted_counter = d[6];
g_snc_can_app.comm_diag.consecutive_counter_errors = (uint8_t)((d[7] >> 4) & 0x0F);
g_snc_can_app.comm_diag.consecutive_crc_errors = (uint8_t)(d[7] & 0x0F);
g_snc_can_app.comm_diag.last_update_ms = HAL_GetTick();
}
static void snc_parse_odom_delta(const uint8_t *d)
{
uint32_t now = HAL_GetTick();
uint32_t prev_update_ms = g_snc_can_app.odom_delta.last_update_ms;
/* 保留最近一帧快照（兼容性，调试用） */
g_snc_can_app.odom_delta.fl_delta = snc_read_i16_le(d[0], d[1]);
g_snc_can_app.odom_delta.rl_delta = snc_read_i16_le(d[2], d[3]);
g_snc_can_app.odom_delta.fr_delta = snc_read_i16_le(d[4], d[5]);
g_snc_can_app.odom_delta.rr_delta = snc_read_i16_le(d[6], d[7]);
g_snc_can_app.odom_delta.last_update_ms = now;
/* ---- 累加到待消费缓冲区，解决漏积分问题 ---- */
SNC_OdomDeltaAccum_t *acc = &g_snc_can_app.odom_accum;
acc->fl_accum += (int32_t)snc_read_i16_le(d[0], d[1]);
acc->rl_accum += (int32_t)snc_read_i16_le(d[2], d[3]);
acc->fr_accum += (int32_t)snc_read_i16_le(d[4], d[5]);
acc->rr_accum += (int32_t)snc_read_i16_le(d[6], d[7]);
if (acc->frame_count == 0U) {
acc->first_frame_ms = now;
}
acc->last_frame_ms = now;
/*
* 累加增量真实覆盖的时间窗。
* 每个 0x200 帧表示“自上一帧以来”的编码器增量，
* 因此应累加相邻帧之间的时间差，而不是简单用 last-first。
*/
if (prev_update_ms != 0U) {
uint32_t frame_dt_ms = now - prev_update_ms;
if (frame_dt_ms <= 1000U) {
acc->span_ms += frame_dt_ms;
}
}
if (acc->frame_count < 255U) {
acc->frame_count++;
}
}
void SNC_CAN_AppInit(void)
{
memset(&g_snc_can_app, 0, sizeof(g_snc_can_app));
/* 配置 5 个标准滤波器，只接收协议相关 ID */
snc_fdcan_config_filter(SNC_FILTER_INDEX_STATUS, SNC_CAN_ID_STATUS);
snc_fdcan_config_filter(SNC_FILTER_INDEX_ACTUAL_RPM, SNC_CAN_ID_ACTUAL_RPM);
snc_fdcan_config_filter(SNC_FILTER_INDEX_TARGET_RPM, SNC_CAN_ID_TARGET_RPM);
snc_fdcan_config_filter(SNC_FILTER_INDEX_COMM_DIAG, SNC_CAN_ID_COMM_DIAG);
snc_fdcan_config_filter(SNC_FILTER_INDEX_ODOM, SNC_CAN_ID_ODOM);
/* 其余标准帧/扩展帧全部拒收 */
if (HAL_FDCAN_ConfigGlobalFilter(&hfdcan1,
FDCAN_REJECT,
FDCAN_REJECT,
FDCAN_REJECT_REMOTE,
FDCAN_REJECT_REMOTE) != HAL_OK)
{
Error_Handler();
}
if (HAL_FDCAN_Start(&hfdcan1) != HAL_OK)
{
Error_Handler();
}
if (HAL_FDCAN_ActivateNotification(&hfdcan1,
FDCAN_IT_RX_FIFO0_NEW_MESSAGE,
0U) != HAL_OK)
{
Error_Handler();
}
}
HAL_StatusTypeDef SNC_CAN_SendHeartbeat(void)
{
uint8_t data[0];
HAL_StatusTypeDef ret = snc_fdcan_add_tx_std(SNC_CAN_ID_HEARTBEAT, data, 0U);
if (ret == HAL_OK)
{
g_snc_can_app.tx_total++;
}
else
{
g_snc_can_app.tx_fail_total++;
}
return ret;
}
HAL_StatusTypeDef SNC_CAN_SendCmdVel(float vx_mps, float wz_radps, uint8_t ctrl_flags)
{
uint8_t data[8];
int16_t vx_i16 = snc_saturate_float_to_i16(vx_mps * 1000.0f);
int16_t wz_i16 = snc_saturate_float_to_i16(wz_radps * 1000.0f);
HAL_StatusTypeDef ret;
g_snc_can_app.tx_counter = (uint8_t)(g_snc_can_app.tx_counter + 1U);
g_snc_can_app.tx_ctrl_flags = ctrl_flags;
snc_write_i16_le(&data[0], vx_i16);
snc_write_i16_le(&data[2], wz_i16);
data[4] = ctrl_flags;
data[5] = 0U;
data[6] = g_snc_can_app.tx_counter;
data[7] = SNC_CAN_Crc8J1850(data, 7U);
ret = snc_fdcan_add_tx_std(SNC_CAN_ID_CMD_VEL, data, 8U);
if (ret == HAL_OK)
{
g_snc_can_app.tx_total++;
}
else
{
g_snc_can_app.tx_fail_total++;
}
return ret;
}
void SNC_CAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t rxFifo0ITs)
{
FDCAN_RxHeaderTypeDef rxHeader;
uint8_t data[8];
if (hfdcan != &hfdcan1)
{
return;
}
if ((rxFifo0ITs & FDCAN_IT_RX_FIFO0_NEW_MESSAGE) == 0U)
{
return;
}
while (HAL_FDCAN_GetRxFifoFillLevel(&hfdcan1, FDCAN_RX_FIFO0) > 0U)
{
if (HAL_FDCAN_GetRxMessage(&hfdcan1, FDCAN_RX_FIFO0, &rxHeader, data) != HAL_OK)
{
break;
}
if ((rxHeader.IdType != FDCAN_STANDARD_ID) ||
(rxHeader.RxFrameType != FDCAN_DATA_FRAME))
{
continue;
}
g_snc_can_app.rx_total++;
switch (rxHeader.Identifier)
{
case SNC_CAN_ID_STATUS:
if (rxHeader.DataLength == FDCAN_DLC_BYTES_8)
{
snc_parse_status(data);
}
break;
case SNC_CAN_ID_ACTUAL_RPM:
if (rxHeader.DataLength == FDCAN_DLC_BYTES_8)
{
snc_parse_actual_rpm(data);
}
break;
case SNC_CAN_ID_TARGET_RPM:
if (rxHeader.DataLength == FDCAN_DLC_BYTES_8)
{
snc_parse_target_rpm(data);
}
break;
case SNC_CAN_ID_COMM_DIAG:
if (rxHeader.DataLength == FDCAN_DLC_BYTES_8)
{
snc_parse_comm_diag(data);
}
break;
case SNC_CAN_ID_ODOM:
if (rxHeader.DataLength == FDCAN_DLC_BYTES_8)
{
snc_parse_odom_delta(data);
}
break;
default:
break;
}
}
}
void SNC_CAN_20msTask(void)
{
/* 这里默认发心跳。
速度命令建议由你的上层控制逻辑每 20ms 调 SNC_CAN_SendCmdVel()。 */
(void)SNC_CAN_SendHeartbeat();
}
void SNC_CAN_100msTask(void)
{
/* 使用静态变量记录上一次的接收总数 */
static uint32_t last_rx_total = 0;
/* 如果当前总数和上次记录的总数不一致，说明这 100ms 内成功收到了新消息 */
if (g_snc_can_app.rx_total != last_rx_total)
{
last_rx_total = g_snc_can_app.rx_total;
// 翻转 PE2 电平，产生肉眼可见的闪烁效果
HAL_GPIO_TogglePin(GPIOE, GPIO_PIN_3);
}
else
{
/* 如果 100ms 内没有收到任何新消息（CAN 断开或下位机死机）
强制关闭 LED。
注意：这里假设高电平 (SET) 是熄灭。如果是低电平熄灭，请改为 GPIO_PIN_RESET */
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_3, GPIO_PIN_SET);
}
}
void SNC_CAN_PollOnlineState(uint32_t now_ms)
{
if ((now_ms - g_snc_can_app.status.last_update_ms) > SNC_STATUS_TIMEOUT_MS)
{
g_snc_can_app.status.online = false;
}
}
const SNC_CAN_AppContext_t *SNC_CAN_GetContext(void)
{
return &g_snc_can_app;
}
uint8_t SNC_CAN_ConsumeOdomDelta(int16_t *fl, int16_t *rl,
int16_t *fr, int16_t *rr,
uint32_t *dt_ms)
{
/*
* 原子取走累加器内容并清零。
*
* 本函数由 monitorTask (普通任务上下文) 调用，
* 而写端 snc_parse_odom_delta() 运行在 CAN Rx ISR 中。
*
* 用 taskENTER_CRITICAL / taskEXIT_CRITICAL 保证取走 + 清零
* 的原子性：临界区会关中断，防止 ISR 在取走过程中写入新增量。
*/
SNC_OdomDeltaAccum_t snapshot;
uint8_t count;
taskENTER_CRITICAL();
{
snapshot = g_snc_can_app.odom_accum; /* 结构体拷贝 */
/* 清零累加器，等待下一批帧 */
g_snc_can_app.odom_accum.fl_accum = 0;
g_snc_can_app.odom_accum.rl_accum = 0;
g_snc_can_app.odom_accum.fr_accum = 0;
g_snc_can_app.odom_accum.rr_accum = 0;
g_snc_can_app.odom_accum.first_frame_ms = 0U;
g_snc_can_app.odom_accum.last_frame_ms = 0U;
g_snc_can_app.odom_accum.span_ms = 0U;
g_snc_can_app.odom_accum.frame_count = 0U;
}
taskEXIT_CRITICAL();
count = snapshot.frame_count;
if (count == 0U) {
/* 期间没有新帧到达 */
*fl = 0; *rl = 0; *fr = 0; *rr = 0;
*dt_ms = 0U;
return 0U;
}
/* int32 → int16 饱和截断（正常工况下不会溢出） */
*fl = (int16_t)((snapshot.fl_accum > 32767) ? 32767 :
(snapshot.fl_accum < -32768) ? -32768 : snapshot.fl_accum);
*rl = (int16_t)((snapshot.rl_accum > 32767) ? 32767 :
(snapshot.rl_accum < -32768) ? -32768 : snapshot.rl_accum);
*fr = (int16_t)((snapshot.fr_accum > 32767) ? 32767 :
(snapshot.fr_accum < -32768) ? -32768 : snapshot.fr_accum);
*rr = (int16_t)((snapshot.rr_accum > 32767) ? 32767 :
(snapshot.rr_accum < -32768) ? -32768 : snapshot.rr_accum);
/* 实际累计时间窗 */
if (snapshot.span_ms == 0U) {
/* 首帧或刚恢复时无法可靠计算，返回 0 让调用方使用保底值 */
*dt_ms = 0U;
} else {
*dt_ms = snapshot.span_ms;
}
return count;
}

183
App/Can/snc_can_app.h Normal file
View File

@@ -0,0 +1,183 @@
#ifndef __SNC_CAN_APP_H
#define __SNC_CAN_APP_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include <stdbool.h>
#include "fdcan.h"
#define SNC_ODOM_TIMEOUT_MS 300U
/* ========================= 协议 CAN ID ========================= */
#define SNC_CAN_ID_HEARTBEAT 0x080U
#define SNC_CAN_ID_CMD_VEL 0x100U
#define SNC_CAN_ID_STATUS 0x181U
#define SNC_CAN_ID_ACTUAL_RPM 0x182U
#define SNC_CAN_ID_TARGET_RPM 0x183U
#define SNC_CAN_ID_COMM_DIAG 0x184U
#define SNC_CAN_ID_ODOM 0x200U
/* ========================= 系统状态定义 ========================= */
typedef enum
{
SNC_SYSTEM_BOOTING = 0,
SNC_SYSTEM_OPERATIONAL = 1,
SNC_SYSTEM_SAFE_FAULT = 2
} SNC_SystemState_t;
typedef enum
{
SNC_SYSTEM_HEALTH_OK = 0,
SNC_SYSTEM_HEALTH_WARNING = 1,
SNC_SYSTEM_HEALTH_FAULT = 2
} SNC_SystemHealth_t;
/* ========================= 诊断位定义 ========================= */
typedef enum
{
SNC_DIAG_COMM_TIMEOUT = (1UL << 0),
SNC_DIAG_CAN_BUS_OFF = (1UL << 1),
SNC_DIAG_CMD_CRC_STORM = (1UL << 2),
SNC_DIAG_CMD_CNT_STORM = (1UL << 3),
SNC_DIAG_MOTOR_FL_STALL = (1UL << 4),
SNC_DIAG_MOTOR_RL_STALL = (1UL << 5),
SNC_DIAG_MOTOR_FR_STALL = (1UL << 6),
SNC_DIAG_MOTOR_RR_STALL = (1UL << 7),
SNC_DIAG_CONTROL_SATURATION = (1UL << 8)
} SNC_DiagBits_t;
/* ========================= 下位机状态快照 ========================= */
typedef struct
{
uint8_t state;
uint8_t health;
uint32_t diag_bits;
uint8_t cmd_age_10ms;
uint8_t status_counter;
uint32_t last_update_ms;
bool online;
} SNC_ChassisStatus_t;
/* ========================= 轮速数据 ========================= */
typedef struct
{
int16_t fl;
int16_t rl;
int16_t fr;
int16_t rr;
uint32_t last_update_ms;
} SNC_WheelRpmFrame_t;
/* ========================= 里程增量 ========================= */
typedef struct
{
int16_t fl_delta;
int16_t rl_delta;
int16_t fr_delta;
int16_t rr_delta;
uint32_t last_update_ms;
} SNC_OdomDeltaFrame_t;
/**
* @brief 里程增量累加器
*
* 解决 0x200 增量帧的"漏积分/重复积分"问题：
* - ISR 收到每帧 0x200 时，将增量**累加**到此结构体（不覆盖）
* - 消费者调用 SNC_CAN_ConsumeOdomDelta() 原子取走累计值并清零
*
* 这样即使消费者频率 (100ms) 低于帧到达频率 (~60ms)，
* 也不会丢失任何增量；也不会重复消费同一份增量。
*/
typedef struct
{
int32_t fl_accum; /**< 左前轮累计增量 ticks (int32 防溢出) */
int32_t rl_accum; /**< 左后轮累计增量 ticks */
int32_t fr_accum; /**< 右前轮累计增量 ticks */
int32_t rr_accum; /**< 右后轮累计增量 ticks */
uint32_t first_frame_ms; /**< 本次累加窗口内第一帧的时间戳 [ms] */
uint32_t last_frame_ms; /**< 本次累加窗口内最后一帧的时间戳 [ms] */
uint32_t span_ms; /**< 本次累计增量实际覆盖的时间窗 [ms] */
uint8_t frame_count; /**< 本次累加窗口内收到的帧数 */
} SNC_OdomDeltaAccum_t;
/* ========================= 通信诊断 ========================= */
typedef struct
{
uint8_t valid_cmd_total_lsb;
uint8_t crc_error_total_lsb;
uint8_t counter_reject_total_lsb;
uint8_t can_tx_drop_total_lsb;
uint8_t busoff_total_lsb;
uint8_t rx_overrun_total_lsb;
uint8_t last_accepted_counter;
uint8_t consecutive_counter_errors;
uint8_t consecutive_crc_errors;
uint32_t last_update_ms;
} SNC_CommDiagFrame_t;
/* ========================= 上位机应用层总状态 ========================= */
typedef struct
{
SNC_ChassisStatus_t status;
SNC_WheelRpmFrame_t actual_rpm;
SNC_WheelRpmFrame_t target_rpm;
SNC_OdomDeltaFrame_t odom_delta; /**< 最近一帧快照 (保留兼容性, 勿用于积分) */
SNC_OdomDeltaAccum_t odom_accum; /**< 里程增量累加器 (正确消费入口) */
SNC_CommDiagFrame_t comm_diag;
uint8_t tx_counter;
uint8_t tx_ctrl_flags;
uint32_t tx_total;
uint32_t tx_fail_total;
uint32_t rx_total;
} SNC_CAN_AppContext_t;
/* ========================= 全局上下文 ========================= */
extern SNC_CAN_AppContext_t g_snc_can_app;
/* ========================= 初始化与任务 ========================= */
void SNC_CAN_AppInit(void);
void SNC_CAN_20msTask(void);
void SNC_CAN_100msTask(void);
void SNC_CAN_PollOnlineState(uint32_t now_ms);
/* ========================= 发送接口 ========================= */
HAL_StatusTypeDef SNC_CAN_SendHeartbeat(void);
HAL_StatusTypeDef SNC_CAN_SendCmdVel(float vx_mps, float wz_radps, uint8_t ctrl_flags);
/* ========================= 接收回调 ========================= */
void SNC_CAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t rxFifo0ITs);
/* ========================= 数据读取接口 ========================= */
const SNC_CAN_AppContext_t *SNC_CAN_GetContext(void);
/**
* @brief 原子取走并清零里程增量累加器
*
* 消费者（monitorTask）每周期调用一次，取走自上次消费以来
* ISR 累积的全部增量帧总和。返回后累加器归零，等待下一批帧。
*
* @param[out] fl 左前轮累计增量 ticks (截断为 int16)
* @param[out] rl 左后轮累计增量 ticks
* @param[out] fr 右前轮累计增量 ticks
* @param[out] rr 右后轮累计增量 ticks
* @param[out] dt_ms 累加增量实际覆盖的时间窗 [ms]
* 若无法可靠计算（如系统首帧）则为 0，可用默认帧间隔替代
* @return 本次取走的帧数；0 表示期间没有新帧到达
*/
uint8_t SNC_CAN_ConsumeOdomDelta(int16_t *fl, int16_t *rl,
int16_t *fr, int16_t *rr,
uint32_t *dt_ms);
/* ========================= 工具接口 ========================= */
uint8_t SNC_CAN_Crc8J1850(const uint8_t *data, uint16_t len);
#ifdef __cplusplus
}
#endif
#endif /* __SNC_CAN_APP_H */

68
App/Contract/chassis_can_msg.h Normal file
View File

@@ -0,0 +1,68 @@
#ifndef CHASSIS_CAN_MSG_H
#define CHASSIS_CAN_MSG_H
#include <stdint.h>
/* 强制 1 字节对齐，完美匹配物理总线上的 8 字节数据帧 */
#pragma pack(push, 1)
/* =========================================================
* TX: 上位机 (H743) -> 底盘 (F407)
* ========================================================= */
/**
* @brief 0x100 速度控制帧 (建议 20ms 发送周期) [cite: 608-612, 624]
*/
typedef struct {
int16_t vx_x1000; // Byte 0-1: 线速度 m/s * 1000 (小端) [cite: 612]
int16_t wz_x1000; // Byte 2-3: 角速度 rad/s * 1000 (小端) [cite: 612]
uint8_t ctrl_flags; // Byte 4: 控制标志位 [cite: 612]
uint8_t reserved; // Byte 5: 预留，固定填 0 [cite: 612]
uint8_t rolling_counter; // Byte 6: 滚动计数器 (新counter相对上一帧差值必须在1..3之间) [cite: 612, 622]
uint8_t crc8; // Byte 7: 对 Byte0~6 做的 CRC8-SAE J1850 [cite: 612]
} CanMsg_CmdVel_0x100_t;
/* =========================================================
* RX: 底盘 (F407) -> 上位机 (H743)
* ========================================================= */
/**
* @brief 0x181 底盘状态帧 (20ms 发送周期) [cite: 633-637]
*/
typedef struct {
uint8_t system_state; // Byte 0: 0=BOOTING, 1=OPERATIONAL, 2=SAFE_FAULT [cite: 636, 665]
uint8_t system_health; // Byte 1: 0=OK, 1=WARNING, 2=FAULT [cite: 636, 667]
uint32_t diag_bits; // Byte 2-5: 诊断位图 (小端) [cite: 637, 669]
uint8_t cmd_age_10ms; // Byte 6: 距最近一次合法 0x100 过去多少个 10ms [cite: 637]
uint8_t status_counter; // Byte 7: 状态帧发送计数器 [cite: 637]
} CanMsg_Status_0x181_t;
/**
* @brief 0x200 里程增量帧 (约 60ms 轮询周期) [cite: 655-658]
* @note 这是时间窗内的脉冲增量，上位机需自行累加积分以推算里程 [cite: 660-662]
*/
typedef struct {
int16_t fl_delta_ticks; // Byte 0-1: 左前轮增量 [cite: 658]
int16_t rl_delta_ticks; // Byte 2-3: 左后轮增量 [cite: 658]
int16_t fr_delta_ticks; // Byte 4-5: 右前轮增量 [cite: 658]
int16_t rr_delta_ticks; // Byte 6-7: 右后轮增量 [cite: 658]
} CanMsg_OdomDelta_0x200_t;
/**
* @brief 0x184 通信诊断帧 (100ms 发送周期) [cite: 651-653]
*/
typedef struct {
uint8_t valid_cmd_total; // Byte 0: 合法命令累计 [cite: 653]
uint8_t crc_error_total; // Byte 1: CRC 错误累计 [cite: 653]
uint8_t counter_reject_total; // Byte 2: Counter 拒收累计 [cite: 653]
uint8_t can_tx_drop_total; // Byte 3: CAN 发送丢帧累计 [cite: 653]
uint8_t busoff_total; // Byte 4: Bus-Off 累计 [cite: 653]
uint8_t rx_overrun_total; // Byte 5: FIFO overrun 累计 [cite: 653]
uint8_t last_accepted_counter; // Byte 6: 最近一次接受的 counter [cite: 653]
uint8_t err_nibbles; // Byte 7: 高4位=连续counter错，低4位=连续CRC错 [cite: 653]
} CanMsg_CommDiag_0x184_t;
#pragma pack(pop)
#endif // CHASSIS_CAN_MSG_H

111
App/Contract/robot_blackboard.c Normal file
View File

@@ -0,0 +1,111 @@
#include "robot_blackboard.h"
#include "FreeRTOS.h"
#include "task.h"
#include <string.h>
/* 全局唯一的黑板实例 */
static RobotBlackboard_t g_blackboard = {0};
/* =========================================================
* 生产者 API 实现
* ========================================================= */
void Blackboard_UpdateIMU(const HWT101_Data_t *imu_data) {
if (imu_data == NULL) return;
taskENTER_CRITICAL();
// 更新原始航向角 ([-180, +180) 度)
g_blackboard.imu_yaw.value = imu_data->yaw;
g_blackboard.imu_yaw.timestamp_ms = imu_data->last_update;
g_blackboard.imu_yaw.is_valid = (imu_data->online != 0);
// 更新 unwrap 后的连续航向角 (无跳变, 度)
g_blackboard.imu_yaw_continuous.value = imu_data->yaw_continuous;
g_blackboard.imu_yaw_continuous.timestamp_ms = imu_data->last_update;
g_blackboard.imu_yaw_continuous.is_valid = (imu_data->online != 0);
// 更新角速度 (deg/s)
g_blackboard.imu_wz.value = imu_data->wz;
g_blackboard.imu_wz.timestamp_ms = imu_data->last_update;
g_blackboard.imu_wz.is_valid = (imu_data->online != 0);
taskEXIT_CRITICAL();
}
void Blackboard_UpdateVl53(const Vl53BoardSnapshot_t *vl_data) {
if (vl_data == NULL) return;
taskENTER_CRITICAL();
// 约定: [0]左前, [1]左后, [2]右前, [3]右后
g_blackboard.dist_left_f.value = vl_data->range_mm_filtered[0];
g_blackboard.dist_left_f.is_valid = (vl_data->valid_mask & (1 << 0)) != 0;
g_blackboard.dist_left_f.timestamp_ms = vl_data->tick_ms;
g_blackboard.dist_left_r.value = vl_data->range_mm_filtered[1];
g_blackboard.dist_left_r.is_valid = (vl_data->valid_mask & (1 << 1)) != 0;
g_blackboard.dist_left_r.timestamp_ms = vl_data->tick_ms;
g_blackboard.dist_right_f.value = vl_data->range_mm_filtered[2];
g_blackboard.dist_right_f.is_valid = (vl_data->valid_mask & (1 << 2)) != 0;
g_blackboard.dist_right_f.timestamp_ms = vl_data->tick_ms;
g_blackboard.dist_right_r.value = vl_data->range_mm_filtered[3];
g_blackboard.dist_right_r.is_valid = (vl_data->valid_mask & (1 << 3)) != 0;
g_blackboard.dist_right_r.timestamp_ms = vl_data->tick_ms;
taskEXIT_CRITICAL();
}
void Blackboard_UpdateLaser(const laser_simple_snapshot_t *ls_data) {
if (ls_data == NULL) return;
taskENTER_CRITICAL();
// 1. 前方 STP
g_blackboard.dist_front_stp.value = ls_data->ch[LASER_CH_FRONT_STP].distance_mm;
g_blackboard.dist_front_stp.is_valid = ls_data->ch[LASER_CH_FRONT_STP].valid;
g_blackboard.dist_front_stp.timestamp_ms = ls_data->ch[LASER_CH_FRONT_STP].update_tick_ms;
// 2. 前方 ATK
g_blackboard.dist_front_atk.value = ls_data->ch[LASER_CH_FRONT_ATK].distance_mm;
g_blackboard.dist_front_atk.is_valid = ls_data->ch[LASER_CH_FRONT_ATK].valid;
g_blackboard.dist_front_atk.timestamp_ms = ls_data->ch[LASER_CH_FRONT_ATK].update_tick_ms;
// 3. 后方 STP
g_blackboard.dist_rear_stp.value = ls_data->ch[LASER_CH_REAR_STP].distance_mm;
g_blackboard.dist_rear_stp.is_valid = ls_data->ch[LASER_CH_REAR_STP].valid;
g_blackboard.dist_rear_stp.timestamp_ms = ls_data->ch[LASER_CH_REAR_STP].update_tick_ms;
// 4. 后方 ATK
g_blackboard.dist_rear_atk.value = ls_data->ch[LASER_CH_REAR_ATK].distance_mm;
g_blackboard.dist_rear_atk.is_valid = ls_data->ch[LASER_CH_REAR_ATK].valid;
g_blackboard.dist_rear_atk.timestamp_ms = ls_data->ch[LASER_CH_REAR_ATK].update_tick_ms;
taskEXIT_CRITICAL();
}
void Blackboard_UpdateOdom(float vx, float wz) {
taskENTER_CRITICAL();
g_blackboard.odom_vx = vx;
g_blackboard.odom_wz = wz;
taskEXIT_CRITICAL();
}
void Blackboard_UpdateChassisState(uint8_t state, uint32_t diag, bool online) {
taskENTER_CRITICAL();
g_blackboard.chassis_state = state;
g_blackboard.chassis_diag = diag;
g_blackboard.chassis_online = online;
taskEXIT_CRITICAL();
}
/* =========================================================
* 消费者 API 实现
* ========================================================= */
void Blackboard_GetSnapshot(RobotBlackboard_t *out_snapshot) {
if (out_snapshot == NULL) return;
// 关中断拷贝整个结构体，保证上层算法拿到的数据处于同一个时间切片
taskENTER_CRITICAL();
*out_snapshot = g_blackboard;
taskEXIT_CRITICAL();
}

74
App/Contract/robot_blackboard.h Normal file
View File

@@ -0,0 +1,74 @@
#ifndef ROBOT_BLACKBOARD_H
#define ROBOT_BLACKBOARD_H
#include <stdint.h>
#include <stdbool.h>
#include "IMU/hwt101.h" // 引用 IMU 接口
#include "laser/laser_manager.h" // 引用前后雷达接口
#include "vl53_board.h" // 引用侧向雷达接口
/**
* @brief 标准原子数据，自带时间戳，专治“传感器偷偷掉线”
*/
typedef struct {
float value; // 数据本体 (距离、角度、速度等)
uint32_t timestamp_ms; // 数据更新时的系统时间 (HAL_GetTick)
bool is_valid; // 驱动层判定的有效性 (如是否超量程、是否报警)
} SensorItem_t;
/**
* @brief 机器人全局情报中心 (System Context)
* @note 对齐抽象后的数据，算法层不需要再关心这是 1 号雷达还是 2 号雷达
*/
typedef struct {
/* --- 1. 空间姿态 (整合自 HWT101) --- */
SensorItem_t imu_yaw; // 原始航向角 (deg)，范围 [-180, +180)，跨界时跳变
SensorItem_t imu_yaw_continuous; // unwrap 后的连续航向角 (deg)，无跳变，可直接做差计算转角
SensorItem_t imu_wz; // Z轴角速度 (deg/s，上层使用前需转 rad/s)
/* --- 2. 侧向走廊观测 (整合自 Vl53Board) --- */
SensorItem_t dist_left_f; // 左前侧向距离 (mm)
SensorItem_t dist_left_r; // 左后侧向距离 (mm)
SensorItem_t dist_right_f; // 右前侧向距离 (mm)
SensorItem_t dist_right_r; // 右后侧向距离 (mm)
/* --- 3. 前后到端/防撞观测 (整合自 Laser Manager) --- */
SensorItem_t dist_front_stp; // 前方 STP 激光测距 (mm)
SensorItem_t dist_front_atk; // 前方 ATK 激光测距 (mm)
SensorItem_t dist_rear_stp; // 后方 STP 激光测距 (mm)
SensorItem_t dist_rear_atk; // 后方 ATK 激光测距 (mm)
/* --- 4. 底盘反馈 (整合自 CAN 0x181 & 0x200) --- */
float odom_vx; // 基于 ticks 积分推算的当前线速度 (m/s)
float odom_wz; // 基于 ticks 积分推算的当前角速度 (rad/s)
uint32_t chassis_diag; // 底盘硬诊断位图 [cite: 669]
uint8_t chassis_state; // 0=BOOTING, 1=OPERATIONAL, 2=SAFE_FAULT [cite: 665]
bool chassis_online; // 底盘 CAN 通讯是否正常
} RobotBlackboard_t;
/* =========================================================
* 生产者 API (各种中断/驱动采集任务调用)
* ========================================================= */
void Blackboard_UpdateIMU(const HWT101_Data_t *imu_data);
void Blackboard_UpdateVl53(const Vl53BoardSnapshot_t *vl_data);
void Blackboard_UpdateLaser(const laser_simple_snapshot_t *ls_data);
// 由你的里程计解算函数调用 (传入算好的实时速度)
void Blackboard_UpdateOdom(float vx, float wz);
// 由 CAN 0x181 接收回调调用
void Blackboard_UpdateChassisState(uint8_t state, uint32_t diag, bool online);
/* =========================================================
* 消费者 API (算法/控制任务调用)
* ========================================================= */
/**
* @brief 拍一张极速快照，获取全部对齐的传感器数据
* @note 内部自带临界区保护，绝对防撕裂
*/
void Blackboard_GetSnapshot(RobotBlackboard_t *out_snapshot);
#endif // ROBOT_BLACKBOARD_H

44
App/Contract/robot_cmd_slot.c Normal file
View File

@@ -0,0 +1,44 @@
#include "robot_cmd_slot.h"
#include "FreeRTOS.h"
#include "task.h"
#include "main.h" // 提供 HAL_GetTick()
/* 全局唯一的指令槽实例 */
static RobotTargetCmd_t g_cmd_slot = {0};
void CmdSlot_Push(float vx, float wz, uint8_t flags)
{
// 1. 获取当前系统时间作为时间戳
uint32_t now_ms = HAL_GetTick();
// 2. 进入临界区，关中断，防止写入一半时被高优先级任务或中断打断（防数据撕裂）
taskENTER_CRITICAL();
g_cmd_slot.target_vx = vx;
g_cmd_slot.target_wz = wz;
g_cmd_slot.ctrl_flags = flags;
g_cmd_slot.gen_tick_ms = now_ms;
taskEXIT_CRITICAL();
}
bool CmdSlot_Pop(RobotTargetCmd_t *out_cmd, uint32_t timeout_ms)
{
if (out_cmd == NULL) {
return false;
}
// 1. 进入临界区，安全地拷贝走整个结构体
taskENTER_CRITICAL();
*out_cmd = g_cmd_slot;
taskEXIT_CRITICAL();
// 2. 计算指令老化时间 (利用无符号整型减法自然处理溢出回绕)
uint32_t now_ms = HAL_GetTick();
uint32_t age_ms = now_ms - out_cmd->gen_tick_ms;
// 3. 判定是否超时
if (age_ms > timeout_ms) {
return false; // 🚨 指令太老了，算法层可能死机了！
}
return true; // ✅ 指令新鲜，可以发送
}

37
App/Contract/robot_cmd_slot.h Normal file
View File

@@ -0,0 +1,37 @@
#ifndef ROBOT_CMD_SLOT_H
#define ROBOT_CMD_SLOT_H
#include <stdint.h>
#include <stdbool.h>
/**
* @brief 算法层生成的抽象目标指令
*/
typedef struct {
float target_vx; // 目标线速度 (m/s)
float target_wz; // 目标角速度 (rad/s)
uint8_t ctrl_flags; // 控制模式/标志
uint32_t gen_tick_ms; // 指令生成时的系统时间 (用于底层判断算法是否卡死)
} RobotTargetCmd_t;
/* =========================================================
* API 接口
* ========================================================= */
/**
* @brief 写入接口：由主导航/控制任务调用
* @param vx 期望线速度 (m/s)
* @param wz 期望角速度 (rad/s)
* @param flags 扩展控制标志
*/
void CmdSlot_Push(float vx, float wz, uint8_t flags);
/**
* @brief 读取接口：由 20ms 的 CAN Tx 任务专属调用
* @param out_cmd 获取到的指令拷贝
* @param timeout_ms 容忍算法不更新的最大时间 (推荐 100ms)
* @return true: 指令新鲜可用; false: 算法超时太久，建议强制发 0 停车保护
*/
bool CmdSlot_Pop(RobotTargetCmd_t *out_cmd, uint32_t timeout_ms);
#endif // ROBOT_CMD_SLOT_H

287
App/Contract/robot_odom.c Normal file
View File

@@ -0,0 +1,287 @@
/**
* @file robot_odom.c
* @brief 里程计积分实现：消费 CAN 0x200 轮增量帧，
* 经差分驱动运动学转换为机体速度并推送黑板。
*
* 调用链（monitorTask 100ms 周期）:
* CAN Rx ISR ──> snc_parse_odom_delta() [snc_can_app.c]
* │ (每帧累加到 odom_accum，不覆盖)
* v
* monitorTask ──> SNC_CAN_ConsumeOdomDelta() [snc_can_app.c]
* │ (原子取走累计增量并清零)
* v
* Odom_Update() [robot_odom.c]
* │ (仅在有新帧时调用，杜绝重复积分)
* v
* Blackboard_UpdateOdom(vx, wz) [robot_blackboard.c]
* │
* v
* navTask ──> Blackboard_GetSnapshot() ──> CorridorFilter_Update()
*
* 运动学公式:
* v_left = (fl_delta + rl_delta) / 2 * TICKS_TO_M / dt
* v_right = (fr_delta + rr_delta) / 2 * TICKS_TO_M / dt
* vx = (v_left + v_right) / 2 [m/s]
* wz = (v_right - v_left) / track [rad/s]
*
* 里程累加（用于沿程 s 估计）:
* s += |vx| * dt [m]
*/
#include "robot_odom.h"
#include "robot_blackboard.h"
#include "FreeRTOS.h"
#include "task.h"
#include <math.h>
/* =========================================================
* 内部静态状态
* ========================================================= */
/** @brief 里程计运行时数据（全部由临界区保护） */
static struct {
float vx; /**< 机体线速度 [m/s] */
float wz; /**< 机体角速度 [rad/s] */
float distance; /**< 累计行驶距离 [m]（从 init 复位后积分） */
bool online; /**< 里程计有效标志 */
uint32_t last_update_ms; /**< 上次有效更新时的系统时间 [ms] */
uint16_t zero_cnt; /**< 连续收到零增量的帧数 */
bool initialized; /**< 是否已完成初始化 */
} s_odom = {
.vx = 0.0f,
.wz = 0.0f,
.distance = 0.0f,
.online = false,
.last_update_ms = 0U,
.zero_cnt = 0U,
.initialized = false,
};
/* =========================================================
* 内部辅助函数
* ========================================================= */
/**
* @brief 内部临界区读写接口（使用 taskENTER/EXIT_CRITICAL）
*/
static inline void lock_odom(void)
{
taskENTER_CRITICAL();
}
static inline void unlock_odom(void)
{
taskEXIT_CRITICAL();
}
/**
* @brief 差分驱动运动学核心
*
* @param fl 左前轮增量 ticks
* @param rl 左后轮增量 ticks
* @param fr 右前轮增量 ticks
* @param rr 右后轮增量 ticks
* @param dt 采样周期 [s]
* @param out_vx 输出线速度 [m/s]
* @param out_wz 输出角速度 [rad/s]
*
* @note 内部使用均值滤波：同侧前后轮求平均后再做差速，
* 这样可以抑制单轮打滑对整体估计的影响。
*/
static void differential_kinematics(int16_t fl, int16_t rl,
int16_t fr, int16_t rr,
float dt,
float *out_vx, float *out_wz)
{
/* 1. 同侧均值滤波（抵消前后轮差分） */
float v_left_ticks = 0.5f * ((float)fl + (float)rl); /* [ticks/period] */
float v_right_ticks = 0.5f * ((float)fr + (float)rr); /* [ticks/period] */
/* 2. tick -> m/s */
float v_left = v_left_ticks * ODOM_TICKS_TO_M / dt; /* [m/s] */
float v_right = v_right_ticks * ODOM_TICKS_TO_M / dt; /* [m/s] */
/* 3. 差分驱动运动学逆解 */
/* vx = (vL + vR) / 2 */
/* wz = (vR - vL) / track */
*out_vx = 0.5f * (v_left + v_right);
*out_wz = (v_right - v_left) * ODOM_WHEEL_TRACK_INV;
/* 4. 合理性检查：物理约束限幅
* H743 主频 480MHz，dt 最小约 1ms，
* 若单帧 tick 绝对值超过 65535/dt，说明增量帧发生异常合并/撕裂。 */
const float MAX_VX_PERIOD = 5.0f; /* 允许的最大瞬时线速度 [m/s]（车辆标称最高 1m/s） */
const float MAX_WZ_PERIOD = 20.0f; /* 允许的最大瞬时角速度 [rad/s]（极端原地转向） */
if (fabsf(*out_vx) > MAX_VX_PERIOD) {
*out_vx = (*out_vx > 0.0f) ? MAX_VX_PERIOD : -MAX_VX_PERIOD;
}
if (fabsf(*out_wz) > MAX_WZ_PERIOD) {
*out_wz = (*out_wz > 0.0f) ? MAX_WZ_PERIOD : -MAX_WZ_PERIOD;
}
}
/* =========================================================
* 公共 API 实现
* ========================================================= */
void Odom_Init(void)
{
lock_odom();
s_odom.vx = 0.0f;
s_odom.wz = 0.0f;
s_odom.distance = 0.0f;
s_odom.online = false;
s_odom.last_update_ms = 0U;
s_odom.zero_cnt = 0U;
s_odom.initialized = true;
unlock_odom();
}
void Odom_Update(uint32_t now_ms,
int16_t fl_delta, int16_t rl_delta,
int16_t fr_delta, int16_t rr_delta,
uint32_t odom_span_ms)
{
if (!s_odom.initialized) {
Odom_Init();
}
/* 计算采样周期 dt
*
* 优先使用 odom_span_ms（CAN 0x200 帧的真实时间跨度），
* 这是编码器增量实际覆盖的时间窗，不受 monitorTask 调度抖动影响。
*
* 退化策略：
* - odom_span_ms > 0 → 直接使用（最精确）
* - odom_span_ms == 0 → 仅收到一帧，无法算帧间 dt，
* 退化为 now_ms - last_update_ms（兼容旧行为）
* - 首帧（last_update_ms == 0 且 odom_span_ms == 0）→ 默认 60ms
* （0x200 帧典型间隔）
*/
float dt;
if (odom_span_ms > 0U && odom_span_ms <= 1000U) {
/* 有真实帧时间跨度，优先使用 */
dt = (float)odom_span_ms / 1000.0f;
} else if (s_odom.last_update_ms == 0U || !s_odom.online) {
/*
* 系统首帧，或 odom 刚从断流中恢复但当前只有一帧时，
* 无法可靠反推出这份增量覆盖的真实时间窗，退化为典型帧间隔。
*/
dt = 0.06f;
} else {
uint32_t dt_ms = now_ms - s_odom.last_update_ms;
if (dt_ms == 0U || dt_ms > 1000U) {
/* 超时（超过 1 秒没有新数据）视为里程计离线 */
dt = 0.0f;
} else {
dt = (float)dt_ms / 1000.0f;
}
}
/* 检测零增量：四轮全 0 可能是底盘未动或通信丢失 */
bool is_zero_delta = (fl_delta == 0 && rl_delta == 0 &&
fr_delta == 0 && rr_delta == 0);
if (dt <= 0.0f) {
/* 非法 dt，强制离线 */
lock_odom();
s_odom.online = false;
unlock_odom();
return;
}
/* 计算差分运动学 */
float vx_local, wz_local;
differential_kinematics(fl_delta, rl_delta, fr_delta, rr_delta,
dt, &vx_local, &wz_local);
/* 零速度保护：连续多帧零增量后，强制归零（防止漂移） */
lock_odom();
if (is_zero_delta) {
s_odom.zero_cnt++;
if (s_odom.zero_cnt >= 3U) {
/* 连续 3 帧零增量（~180ms @ 60ms 帧率），认为机器人静止 */
s_odom.vx = 0.0f;
s_odom.wz = 0.0f;
s_odom.online = true;
}
} else {
s_odom.zero_cnt = 0U;
s_odom.vx = vx_local;
s_odom.wz = wz_local;
s_odom.online = true;
/* 积分累积里程（取线速度的绝对值，保证后退也累加） */
s_odom.distance += fabsf(s_odom.vx) * dt;
}
s_odom.last_update_ms = now_ms;
unlock_odom();
/* 推送黑板（黑板内部自带临界区保护） */
Blackboard_UpdateOdom(s_odom.vx, s_odom.wz);
}
void Odom_HandleTimeout(uint32_t now_ms, uint32_t timeout_ms)
{
bool timed_out = false;
if (!s_odom.initialized || timeout_ms == 0U) {
return;
}
lock_odom();
if (s_odom.online &&
s_odom.last_update_ms != 0U &&
(now_ms - s_odom.last_update_ms) > timeout_ms) {
s_odom.vx = 0.0f;
s_odom.wz = 0.0f;
s_odom.online = false;
s_odom.zero_cnt = 0U;
timed_out = true;
}
unlock_odom();
if (timed_out) {
Blackboard_UpdateOdom(0.0f, 0.0f);
}
}
void Odom_GetSpeed(float *out_vx, float *out_wz, OdomStatus_t *out_status)
{
if (out_vx != NULL) *out_vx = s_odom.vx;
if (out_wz != NULL) *out_wz = s_odom.wz;
if (out_status != NULL) {
lock_odom();
out_status->online = s_odom.online;
out_status->zero_delta_count = s_odom.zero_cnt;
out_status->last_update_ms = s_odom.last_update_ms;
unlock_odom();
}
}
float Odom_GetDistance(void)
{
float d;
lock_odom();
d = s_odom.distance;
unlock_odom();
return d;
}
void Odom_Reset(void)
{
lock_odom();
s_odom.vx = 0.0f;
s_odom.wz = 0.0f;
s_odom.distance = 0.0f;
s_odom.online = false;
s_odom.last_update_ms = 0U;
s_odom.zero_cnt = 0U;
unlock_odom();
Blackboard_UpdateOdom(0.0f, 0.0f);
}

115
App/Contract/robot_odom.h Normal file
View File

@@ -0,0 +1,115 @@
/**
* @file robot_odom.h
* @brief 里程计积分模块：从 CAN 0x200 轮增量帧累加积分，
* 经差分驱动运动学转换为机体线速度 vx 与角速度 wz，
* 最终通过 Blackboard_UpdateOdom() 推送给导航流水线。
*
* @note 本模块不依赖任何传感器，只消费 CAN 协议层已有的 odom_delta 数据。
* 所有标定参数统一在 robot_params.h 中管理。
*
* 运动学模型（差分驱动）:
* v_left = (fl_delta + rl_delta) / 2
* v_right = (fr_delta + rr_delta) / 2
* vx = (v_left + v_right) / 2 [m/s]
* wz = (v_right - v_left) / wheel_track [rad/s]
*/
#ifndef ROBOT_ODOM_H
#define ROBOT_ODOM_H
#include <stdint.h>
#include <stdbool.h>
#include "robot_params.h" /* 统一参数头文件 */
/* =========================================================
* 标定参数（从 robot_params.h 引入，勿在此文件重复定义）
* ========================================================= */
#define ODOM_TICKS_PER_REV PARAM_ENCODER_CPR
#define ODOM_WHEEL_DIAMETER PARAM_WHEEL_DIAMETER
#define ODOM_WHEEL_TRACK PARAM_WHEEL_TRACK
/* =========================================================
* 预计算常数（自动生成，勿手动修改）
* ========================================================= */
#define ODOM_TICKS_TO_M (3.14159265358979f * (ODOM_WHEEL_DIAMETER) / (ODOM_TICKS_PER_REV))
/**< @brief 每 tick 对应的轮缘弧长 [m]，即 PI*D / CPR */
#define ODOM_WHEEL_TRACK_INV (1.0f / (ODOM_WHEEL_TRACK))
/**< @brief 轮距倒数 [1/m]，用于快速计算角速度 */
/* =========================================================
* 配置结构
* ========================================================= */
typedef struct {
/** @brief 里程计有效标志，false 时 vx/wz 强制为 0 */
bool online;
/** @brief 里程计在线但 Tick 增量为 0 的连续帧数（用于检测卡死） */
uint16_t zero_delta_count;
/** @brief 上次更新时的系统时间 [ms] */
uint32_t last_update_ms;
} OdomStatus_t;
/* =========================================================
* API 接口
* ========================================================= */
/**
* @brief 里程计模块初始化
* @note 重置内部累加器，建议在系统启动阶段调用一次
*/
void Odom_Init(void);
/**
* @brief 里程计更新主函数
* @param now_ms 当前系统时间 [ms]，由 HAL_GetTick() 提供
* @param fl_delta 左前轮增量 ticks（本周期内编码器增量，可为负）
* @param rl_delta 左后轮增量 ticks
* @param fr_delta 右前轮增量 ticks
* @param rr_delta 右后轮增量 ticks
* @param odom_span_ms 本批次增量帧的真实时间跨度 [ms]，由
* SNC_CAN_ConsumeOdomDelta() 返回。
* 传 0 表示无法计算（如仅一帧），此时退化为
* now_ms - last_update_ms。
*
* @note 由调用方（本例中 monitorTask 或 navTask）定期轮询 CAN 上下文
* 中的 odom_delta 数据后主动调用，推荐调用周期 20~100ms。
* 函数内部完成：均值滤波 -> 差分运动学 -> Blackboard 更新。
*/
void Odom_Update(uint32_t now_ms,
int16_t fl_delta, int16_t rl_delta,
int16_t fr_delta, int16_t rr_delta,
uint32_t odom_span_ms);
/**
* @brief odom 断流超时处理
* @param now_ms 当前系统时间 [ms]
* @param timeout_ms 允许的最大无更新时长 [ms]
*
* @note 超时后会将 vx/wz 清零、标记离线，并同步刷新黑板，
* 防止导航层继续使用最后一次有效速度。
*/
void Odom_HandleTimeout(uint32_t now_ms, uint32_t timeout_ms);
/**
* @brief 读取当前里程计状态快照（供外部任务消费）
* @param out_status 输出状态结构体
* @param out_vx 输出线速度 [m/s]
* @param out_wz 输出角速度 [rad/s]
*
* @note 内部使用临界区保护，调用方可在任何上下文安全使用。
*/
void Odom_GetSpeed(float *out_vx, float *out_wz, OdomStatus_t *out_status);
/**
* @brief 获取累积里程（沿程 s）
* @return 累计行驶距离 [m]（从 Odom_Init 复位后开始积分）
*
* @note 由走廊状态机用于段落终止判断。也可用于比赛计时参考。
*/
float Odom_GetDistance(void);
/**
* @brief 复位里程计（清零累积距离和状态）
*/
void Odom_Reset(void);
#endif /* ROBOT_ODOM_H */

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;
}
}

41
App/IMU/hwt101.h Normal file
View File

@@ -0,0 +1,41 @@
#ifndef HWT101_H
#define HWT101_H
#include <stdint.h>
#include "main.h"
#define HWT101_DMA_BUF_SIZE 256 // 必须是 32 的整数倍
typedef enum {
HWT101_RATE_10HZ = 0x06,
HWT101_RATE_50HZ = 0x08,
HWT101_RATE_100HZ = 0x09, // 推荐：100Hz
HWT101_RATE_200HZ = 0x0B
} HWT101_Rate_t;
typedef enum {
HWT101_BAUD_9600 = 0x02,
HWT101_BAUD_115200 = 0x06,
HWT101_BAUD_230400 = 0x07
} HWT101_Baud_t;
typedef struct {
float yaw; // 原始偏航角 (deg)，范围 [-180, +180)，跨界时跳变
float yaw_continuous; // unwrap 后的连续偏航角 (deg)，无跳变，可直接做差计算转角
float wz; // Z 轴角速度 (deg/s)
uint32_t last_update;
uint8_t online;
} HWT101_Data_t;
/* API 函数声明 */
void HWT101_Init(UART_HandleTypeDef *huart);
void HWT101_Process(void);
// 【升级 2】改为传入指针，安全拷贝数据，防止数据撕裂
void HWT101_GetData(HWT101_Data_t *out_data);
// 在末尾 API 声明处加上这句
void HWT101_ErrorRecovery(UART_HandleTypeDef *huart);
HAL_StatusTypeDef HWT101_Config(HWT101_Rate_t rate, HWT101_Baud_t baud);
HAL_StatusTypeDef HWT101_ZeroYaw(void);
#endif /* HWT101_H */

1926
App/VL53L0X_API/core/inc/vl53l0x_api.h Normal file
View File

File diff suppressed because it is too large Load Diff

85
App/VL53L0X_API/core/inc/vl53l0x_api_calibration.h Normal file
View File

@@ -0,0 +1,85 @@
/*******************************************************************************
* Copyright © 2016, STMicroelectronics International N.V.
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of STMicroelectronics nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/
#ifndef _VL53L0X_API_CALIBRATION_H_
#define _VL53L0X_API_CALIBRATION_H_
#include "vl53l0x_def.h"
#include "vl53l0x_platform.h"
#ifdef __cplusplus
extern "C" {
#endif
VL53L0X_Error VL53L0X_perform_xtalk_calibration(VL53L0X_DEV Dev,
FixPoint1616_t XTalkCalDistance,
FixPoint1616_t *pXTalkCompensationRateMegaCps);
VL53L0X_Error VL53L0X_perform_offset_calibration(VL53L0X_DEV Dev,
FixPoint1616_t CalDistanceMilliMeter,
int32_t *pOffsetMicroMeter);
VL53L0X_Error VL53L0X_set_offset_calibration_data_micro_meter(VL53L0X_DEV Dev,
int32_t OffsetCalibrationDataMicroMeter);
VL53L0X_Error VL53L0X_get_offset_calibration_data_micro_meter(VL53L0X_DEV Dev,
int32_t *pOffsetCalibrationDataMicroMeter);
VL53L0X_Error VL53L0X_apply_offset_adjustment(VL53L0X_DEV Dev);
VL53L0X_Error VL53L0X_perform_ref_spad_management(VL53L0X_DEV Dev,
uint32_t *refSpadCount, uint8_t *isApertureSpads);
VL53L0X_Error VL53L0X_set_reference_spads(VL53L0X_DEV Dev,
uint32_t count, uint8_t isApertureSpads);
VL53L0X_Error VL53L0X_get_reference_spads(VL53L0X_DEV Dev,
uint32_t *pSpadCount, uint8_t *pIsApertureSpads);
VL53L0X_Error VL53L0X_perform_phase_calibration(VL53L0X_DEV Dev,
uint8_t *pPhaseCal, const uint8_t get_data_enable,
const uint8_t restore_config);
VL53L0X_Error VL53L0X_perform_ref_calibration(VL53L0X_DEV Dev,
uint8_t *pVhvSettings, uint8_t *pPhaseCal, uint8_t get_data_enable);
VL53L0X_Error VL53L0X_set_ref_calibration(VL53L0X_DEV Dev,
uint8_t VhvSettings, uint8_t PhaseCal);
VL53L0X_Error VL53L0X_get_ref_calibration(VL53L0X_DEV Dev,
uint8_t *pVhvSettings, uint8_t *pPhaseCal);
#ifdef __cplusplus
}
#endif
#endif /* _VL53L0X_API_CALIBRATION_H_ */

113
App/VL53L0X_API/core/inc/vl53l0x_api_core.h Normal file
View File

@@ -0,0 +1,113 @@
/*******************************************************************************
* Copyright © 2016, STMicroelectronics International N.V.
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of STMicroelectronics nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/
#ifndef _VL53L0X_API_CORE_H_
#define _VL53L0X_API_CORE_H_
#include "vl53l0x_def.h"
#include "vl53l0x_platform.h"
#ifdef __cplusplus
extern "C" {
#endif
VL53L0X_Error VL53L0X_reverse_bytes(uint8_t *data, uint32_t size);
VL53L0X_Error VL53L0X_measurement_poll_for_completion(VL53L0X_DEV Dev);
uint8_t VL53L0X_encode_vcsel_period(uint8_t vcsel_period_pclks);
uint8_t VL53L0X_decode_vcsel_period(uint8_t vcsel_period_reg);
uint32_t VL53L0X_isqrt(uint32_t num);
uint32_t VL53L0X_quadrature_sum(uint32_t a, uint32_t b);
VL53L0X_Error VL53L0X_get_info_from_device(VL53L0X_DEV Dev, uint8_t option);
VL53L0X_Error VL53L0X_set_vcsel_pulse_period(VL53L0X_DEV Dev,
VL53L0X_VcselPeriod VcselPeriodType, uint8_t VCSELPulsePeriodPCLK);
VL53L0X_Error VL53L0X_get_vcsel_pulse_period(VL53L0X_DEV Dev,
VL53L0X_VcselPeriod VcselPeriodType, uint8_t *pVCSELPulsePeriodPCLK);
uint32_t VL53L0X_decode_timeout(uint16_t encoded_timeout);
VL53L0X_Error get_sequence_step_timeout(VL53L0X_DEV Dev,
VL53L0X_SequenceStepId SequenceStepId,
uint32_t *pTimeOutMicroSecs);
VL53L0X_Error set_sequence_step_timeout(VL53L0X_DEV Dev,
VL53L0X_SequenceStepId SequenceStepId,
uint32_t TimeOutMicroSecs);
VL53L0X_Error VL53L0X_set_measurement_timing_budget_micro_seconds(
VL53L0X_DEV Dev,
uint32_t MeasurementTimingBudgetMicroSeconds);
VL53L0X_Error VL53L0X_get_measurement_timing_budget_micro_seconds(
VL53L0X_DEV Dev,
uint32_t *pMeasurementTimingBudgetMicroSeconds);
VL53L0X_Error VL53L0X_load_tuning_settings(VL53L0X_DEV Dev,
uint8_t *pTuningSettingBuffer);
VL53L0X_Error VL53L0X_calc_sigma_estimate(VL53L0X_DEV Dev,
VL53L0X_RangingMeasurementData_t *pRangingMeasurementData,
FixPoint1616_t *pSigmaEstimate);
VL53L0X_Error VL53L0X_calc_dmax(
VL53L0X_DEV Dev, FixPoint1616_t ambRateMeas, uint32_t *pdmax_mm);
VL53L0X_Error VL53L0X_get_total_xtalk_rate(VL53L0X_DEV Dev,
VL53L0X_RangingMeasurementData_t *pRangingMeasurementData,
FixPoint1616_t *ptotal_xtalk_rate_mcps);
VL53L0X_Error VL53L0X_get_total_signal_rate(VL53L0X_DEV Dev,
VL53L0X_RangingMeasurementData_t *pRangingMeasurementData,
FixPoint1616_t *ptotal_signal_rate_mcps);
VL53L0X_Error VL53L0X_get_pal_range_status(VL53L0X_DEV Dev,
uint8_t DeviceRangeStatus,
FixPoint1616_t SignalRate,
uint16_t EffectiveSpadRtnCount,
VL53L0X_RangingMeasurementData_t *pRangingMeasurementData,
uint8_t *pPalRangeStatus);
uint32_t VL53L0X_calc_timeout_mclks(VL53L0X_DEV Dev,
uint32_t timeout_period_us, uint8_t vcsel_period_pclks);
uint16_t VL53L0X_encode_timeout(uint32_t timeout_macro_clks);
#ifdef __cplusplus
}
#endif
#endif /* _VL53L0X_API_CORE_H_ */

47
App/VL53L0X_API/core/inc/vl53l0x_api_ranging.h Normal file
View File

@@ -0,0 +1,47 @@
/*******************************************************************************
* Copyright © 2016, STMicroelectronics International N.V.
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of STMicroelectronics nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/
#ifndef _VL53L0X_API_RANGING_H_
#define _VL53L0X_API_RANGING_H_
#include "vl53l0x_def.h"
#include "vl53l0x_platform.h"
#ifdef __cplusplus
extern "C" {
#endif
#ifdef __cplusplus
}
#endif
#endif /* _VL53L0X_API_RANGING_H_ */

278
App/VL53L0X_API/core/inc/vl53l0x_api_strings.h Normal file
View File

@@ -0,0 +1,278 @@
/*******************************************************************************
* Copyright © 2016, STMicroelectronics International N.V.
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of STMicroelectronics nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/
#ifndef VL53L0X_API_STRINGS_H_
#define VL53L0X_API_STRINGS_H_
#include "vl53l0x_def.h"
#include "vl53l0x_platform.h"
#ifdef __cplusplus
extern "C" {
#endif
VL53L0X_Error VL53L0X_get_device_info(VL53L0X_DEV Dev,
VL53L0X_DeviceInfo_t *pVL53L0X_DeviceInfo);
VL53L0X_Error VL53L0X_get_device_error_string(VL53L0X_DeviceError ErrorCode,
char *pDeviceErrorString);
VL53L0X_Error VL53L0X_get_range_status_string(uint8_t RangeStatus,
char *pRangeStatusString);
VL53L0X_Error VL53L0X_get_pal_error_string(VL53L0X_Error PalErrorCode,
char *pPalErrorString);
VL53L0X_Error VL53L0X_get_pal_state_string(VL53L0X_State PalStateCode,
char *pPalStateString);
VL53L0X_Error VL53L0X_get_sequence_steps_info(
VL53L0X_SequenceStepId SequenceStepId,
char *pSequenceStepsString);
VL53L0X_Error VL53L0X_get_limit_check_info(VL53L0X_DEV Dev,
uint16_t LimitCheckId,
char *pLimitCheckString);
#ifdef USE_EMPTY_STRING
#define VL53L0X_STRING_DEVICE_INFO_NAME ""
#define VL53L0X_STRING_DEVICE_INFO_NAME_TS0 ""
#define VL53L0X_STRING_DEVICE_INFO_NAME_TS1 ""
#define VL53L0X_STRING_DEVICE_INFO_NAME_TS2 ""
#define VL53L0X_STRING_DEVICE_INFO_NAME_ES1 ""
#define VL53L0X_STRING_DEVICE_INFO_TYPE ""
/* PAL ERROR strings */
#define VL53L0X_STRING_ERROR_NONE ""
#define VL53L0X_STRING_ERROR_CALIBRATION_WARNING ""
#define VL53L0X_STRING_ERROR_MIN_CLIPPED ""
#define VL53L0X_STRING_ERROR_UNDEFINED ""
#define VL53L0X_STRING_ERROR_INVALID_PARAMS ""
#define VL53L0X_STRING_ERROR_NOT_SUPPORTED ""
#define VL53L0X_STRING_ERROR_RANGE_ERROR ""
#define VL53L0X_STRING_ERROR_TIME_OUT ""
#define VL53L0X_STRING_ERROR_MODE_NOT_SUPPORTED ""
#define VL53L0X_STRING_ERROR_BUFFER_TOO_SMALL ""
#define VL53L0X_STRING_ERROR_GPIO_NOT_EXISTING ""
#define VL53L0X_STRING_ERROR_GPIO_FUNCTIONALITY_NOT_SUPPORTED ""
#define VL53L0X_STRING_ERROR_CONTROL_INTERFACE ""
#define VL53L0X_STRING_ERROR_INVALID_COMMAND ""
#define VL53L0X_STRING_ERROR_DIVISION_BY_ZERO ""
#define VL53L0X_STRING_ERROR_REF_SPAD_INIT ""
#define VL53L0X_STRING_ERROR_NOT_IMPLEMENTED ""
#define VL53L0X_STRING_UNKNOW_ERROR_CODE ""
/* Range Status */
#define VL53L0X_STRING_RANGESTATUS_NONE ""
#define VL53L0X_STRING_RANGESTATUS_RANGEVALID ""
#define VL53L0X_STRING_RANGESTATUS_SIGMA ""
#define VL53L0X_STRING_RANGESTATUS_SIGNAL ""
#define VL53L0X_STRING_RANGESTATUS_MINRANGE ""
#define VL53L0X_STRING_RANGESTATUS_PHASE ""
#define VL53L0X_STRING_RANGESTATUS_HW ""
/* Range Status */
#define VL53L0X_STRING_STATE_POWERDOWN ""
#define VL53L0X_STRING_STATE_WAIT_STATICINIT ""
#define VL53L0X_STRING_STATE_STANDBY ""
#define VL53L0X_STRING_STATE_IDLE ""
#define VL53L0X_STRING_STATE_RUNNING ""
#define VL53L0X_STRING_STATE_UNKNOWN ""
#define VL53L0X_STRING_STATE_ERROR ""
/* Device Specific */
#define VL53L0X_STRING_DEVICEERROR_NONE ""
#define VL53L0X_STRING_DEVICEERROR_VCSELCONTINUITYTESTFAILURE ""
#define VL53L0X_STRING_DEVICEERROR_VCSELWATCHDOGTESTFAILURE ""
#define VL53L0X_STRING_DEVICEERROR_NOVHVVALUEFOUND ""
#define VL53L0X_STRING_DEVICEERROR_MSRCNOTARGET ""
#define VL53L0X_STRING_DEVICEERROR_SNRCHECK ""
#define VL53L0X_STRING_DEVICEERROR_RANGEPHASECHECK ""
#define VL53L0X_STRING_DEVICEERROR_SIGMATHRESHOLDCHECK ""
#define VL53L0X_STRING_DEVICEERROR_TCC ""
#define VL53L0X_STRING_DEVICEERROR_PHASECONSISTENCY ""
#define VL53L0X_STRING_DEVICEERROR_MINCLIP ""
#define VL53L0X_STRING_DEVICEERROR_RANGECOMPLETE ""
#define VL53L0X_STRING_DEVICEERROR_ALGOUNDERFLOW ""
#define VL53L0X_STRING_DEVICEERROR_ALGOOVERFLOW ""
#define VL53L0X_STRING_DEVICEERROR_RANGEIGNORETHRESHOLD ""
#define VL53L0X_STRING_DEVICEERROR_UNKNOWN ""
/* Check Enable */
#define VL53L0X_STRING_CHECKENABLE_SIGMA_FINAL_RANGE ""
#define VL53L0X_STRING_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE ""
#define VL53L0X_STRING_CHECKENABLE_SIGNAL_REF_CLIP ""
#define VL53L0X_STRING_CHECKENABLE_RANGE_IGNORE_THRESHOLD ""
/* Sequence Step */
#define VL53L0X_STRING_SEQUENCESTEP_TCC ""
#define VL53L0X_STRING_SEQUENCESTEP_DSS ""
#define VL53L0X_STRING_SEQUENCESTEP_MSRC ""
#define VL53L0X_STRING_SEQUENCESTEP_PRE_RANGE ""
#define VL53L0X_STRING_SEQUENCESTEP_FINAL_RANGE ""
#else
#define VL53L0X_STRING_DEVICE_INFO_NAME "VL53L0X cut1.0"
#define VL53L0X_STRING_DEVICE_INFO_NAME_TS0 "VL53L0X TS0"
#define VL53L0X_STRING_DEVICE_INFO_NAME_TS1 "VL53L0X TS1"
#define VL53L0X_STRING_DEVICE_INFO_NAME_TS2 "VL53L0X TS2"
#define VL53L0X_STRING_DEVICE_INFO_NAME_ES1 "VL53L0X ES1 or later"
#define VL53L0X_STRING_DEVICE_INFO_TYPE "VL53L0X"
/* PAL ERROR strings */
#define VL53L0X_STRING_ERROR_NONE \
"No Error"
#define VL53L0X_STRING_ERROR_CALIBRATION_WARNING \
"Calibration Warning Error"
#define VL53L0X_STRING_ERROR_MIN_CLIPPED \
"Min clipped error"
#define VL53L0X_STRING_ERROR_UNDEFINED \
"Undefined error"
#define VL53L0X_STRING_ERROR_INVALID_PARAMS \
"Invalid parameters error"
#define VL53L0X_STRING_ERROR_NOT_SUPPORTED \
"Not supported error"
#define VL53L0X_STRING_ERROR_RANGE_ERROR \
"Range error"
#define VL53L0X_STRING_ERROR_TIME_OUT \
"Time out error"
#define VL53L0X_STRING_ERROR_MODE_NOT_SUPPORTED \
"Mode not supported error"
#define VL53L0X_STRING_ERROR_BUFFER_TOO_SMALL \
"Buffer too small"
#define VL53L0X_STRING_ERROR_GPIO_NOT_EXISTING \
"GPIO not existing"
#define VL53L0X_STRING_ERROR_GPIO_FUNCTIONALITY_NOT_SUPPORTED \
"GPIO funct not supported"
#define VL53L0X_STRING_ERROR_INTERRUPT_NOT_CLEARED \
"Interrupt not Cleared"
#define VL53L0X_STRING_ERROR_CONTROL_INTERFACE \
"Control Interface Error"
#define VL53L0X_STRING_ERROR_INVALID_COMMAND \
"Invalid Command Error"
#define VL53L0X_STRING_ERROR_DIVISION_BY_ZERO \
"Division by zero Error"
#define VL53L0X_STRING_ERROR_REF_SPAD_INIT \
"Reference Spad Init Error"
#define VL53L0X_STRING_ERROR_NOT_IMPLEMENTED \
"Not implemented error"
#define VL53L0X_STRING_UNKNOW_ERROR_CODE \
"Unknown Error Code"
/* Range Status */
#define VL53L0X_STRING_RANGESTATUS_NONE "No Update"
#define VL53L0X_STRING_RANGESTATUS_RANGEVALID "Range Valid"
#define VL53L0X_STRING_RANGESTATUS_SIGMA "Sigma Fail"
#define VL53L0X_STRING_RANGESTATUS_SIGNAL "Signal Fail"
#define VL53L0X_STRING_RANGESTATUS_MINRANGE "Min Range Fail"
#define VL53L0X_STRING_RANGESTATUS_PHASE "Phase Fail"
#define VL53L0X_STRING_RANGESTATUS_HW "Hardware Fail"
/* Range Status */
#define VL53L0X_STRING_STATE_POWERDOWN "POWERDOWN State"
#define VL53L0X_STRING_STATE_WAIT_STATICINIT \
"Wait for staticinit State"
#define VL53L0X_STRING_STATE_STANDBY "STANDBY State"
#define VL53L0X_STRING_STATE_IDLE "IDLE State"
#define VL53L0X_STRING_STATE_RUNNING "RUNNING State"
#define VL53L0X_STRING_STATE_UNKNOWN "UNKNOWN State"
#define VL53L0X_STRING_STATE_ERROR "ERROR State"
/* Device Specific */
#define VL53L0X_STRING_DEVICEERROR_NONE "No Update"
#define VL53L0X_STRING_DEVICEERROR_VCSELCONTINUITYTESTFAILURE \
"VCSEL Continuity Test Failure"
#define VL53L0X_STRING_DEVICEERROR_VCSELWATCHDOGTESTFAILURE \
"VCSEL Watchdog Test Failure"
#define VL53L0X_STRING_DEVICEERROR_NOVHVVALUEFOUND \
"No VHV Value found"
#define VL53L0X_STRING_DEVICEERROR_MSRCNOTARGET \
"MSRC No Target Error"
#define VL53L0X_STRING_DEVICEERROR_SNRCHECK \
"SNR Check Exit"
#define VL53L0X_STRING_DEVICEERROR_RANGEPHASECHECK \
"Range Phase Check Error"
#define VL53L0X_STRING_DEVICEERROR_SIGMATHRESHOLDCHECK \
"Sigma Threshold Check Error"
#define VL53L0X_STRING_DEVICEERROR_TCC \
"TCC Error"
#define VL53L0X_STRING_DEVICEERROR_PHASECONSISTENCY \
"Phase Consistency Error"
#define VL53L0X_STRING_DEVICEERROR_MINCLIP \
"Min Clip Error"
#define VL53L0X_STRING_DEVICEERROR_RANGECOMPLETE \
"Range Complete"
#define VL53L0X_STRING_DEVICEERROR_ALGOUNDERFLOW \
"Range Algo Underflow Error"
#define VL53L0X_STRING_DEVICEERROR_ALGOOVERFLOW \
"Range Algo Overlow Error"
#define VL53L0X_STRING_DEVICEERROR_RANGEIGNORETHRESHOLD \
"Range Ignore Threshold Error"
#define VL53L0X_STRING_DEVICEERROR_UNKNOWN \
"Unknown error code"
/* Check Enable */
#define VL53L0X_STRING_CHECKENABLE_SIGMA_FINAL_RANGE \
"SIGMA FINAL RANGE"
#define VL53L0X_STRING_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE \
"SIGNAL RATE FINAL RANGE"
#define VL53L0X_STRING_CHECKENABLE_SIGNAL_REF_CLIP \
"SIGNAL REF CLIP"
#define VL53L0X_STRING_CHECKENABLE_RANGE_IGNORE_THRESHOLD \
"RANGE IGNORE THRESHOLD"
#define VL53L0X_STRING_CHECKENABLE_SIGNAL_RATE_MSRC \
"SIGNAL RATE MSRC"
#define VL53L0X_STRING_CHECKENABLE_SIGNAL_RATE_PRE_RANGE \
"SIGNAL RATE PRE RANGE"
/* Sequence Step */
#define VL53L0X_STRING_SEQUENCESTEP_TCC "TCC"
#define VL53L0X_STRING_SEQUENCESTEP_DSS "DSS"
#define VL53L0X_STRING_SEQUENCESTEP_MSRC "MSRC"
#define VL53L0X_STRING_SEQUENCESTEP_PRE_RANGE "PRE RANGE"
#define VL53L0X_STRING_SEQUENCESTEP_FINAL_RANGE "FINAL RANGE"
#endif /* USE_EMPTY_STRING */
#ifdef __cplusplus
}
#endif
#endif

666
App/VL53L0X_API/core/inc/vl53l0x_def.h Normal file
View File

@@ -0,0 +1,666 @@
/*******************************************************************************
* Copyright <20> 2016, STMicroelectronics International N.V.
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of STMicroelectronics nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/
/**
* @file VL53L0X_def.h
*
* @brief Type definitions for VL53L0X API.
*
*/
#ifndef _VL53L0X_DEF_H_
#define _VL53L0X_DEF_H_
#ifdef __cplusplus
extern "C" {
#endif
/** @defgroup VL53L0X_globaldefine_group VL53L0X Defines
* @brief VL53L0X Defines
* @{
*/
/** PAL SPECIFICATION major version */
#define VL53L0X10_SPECIFICATION_VER_MAJOR 1
/** PAL SPECIFICATION minor version */
#define VL53L0X10_SPECIFICATION_VER_MINOR 2
/** PAL SPECIFICATION sub version */
#define VL53L0X10_SPECIFICATION_VER_SUB 7
/** PAL SPECIFICATION sub version */
#define VL53L0X10_SPECIFICATION_VER_REVISION 1440
/** VL53L0X PAL IMPLEMENTATION major version */
#define VL53L0X10_IMPLEMENTATION_VER_MAJOR 1
/** VL53L0X PAL IMPLEMENTATION minor version */
#define VL53L0X10_IMPLEMENTATION_VER_MINOR 0
/** VL53L0X PAL IMPLEMENTATION sub version */
#define VL53L0X10_IMPLEMENTATION_VER_SUB 9
/** VL53L0X PAL IMPLEMENTATION sub version */
#define VL53L0X10_IMPLEMENTATION_VER_REVISION 3673
/** PAL SPECIFICATION major version */
#define VL53L0X_SPECIFICATION_VER_MAJOR 1
/** PAL SPECIFICATION minor version */
#define VL53L0X_SPECIFICATION_VER_MINOR 2
/** PAL SPECIFICATION sub version */
#define VL53L0X_SPECIFICATION_VER_SUB 7
/** PAL SPECIFICATION sub version */
#define VL53L0X_SPECIFICATION_VER_REVISION 1440
/** VL53L0X PAL IMPLEMENTATION major version */
#define VL53L0X_IMPLEMENTATION_VER_MAJOR 1
/** VL53L0X PAL IMPLEMENTATION minor version */
#define VL53L0X_IMPLEMENTATION_VER_MINOR 0
/** VL53L0X PAL IMPLEMENTATION sub version */
#define VL53L0X_IMPLEMENTATION_VER_SUB 4
/** VL53L0X PAL IMPLEMENTATION sub version */
#define VL53L0X_IMPLEMENTATION_VER_REVISION 4960
#define VL53L0X_DEFAULT_MAX_LOOP 2000
#define VL53L0X_MAX_STRING_LENGTH 32
#include "vl53l0x_device.h"
#include "vl53l0x_types.h"
/****************************************
* PRIVATE define do not edit
****************************************/
/** @brief Defines the parameters of the Get Version Functions
*/
typedef struct {
uint32_t revision; /*!< revision number */
uint8_t major; /*!< major number */
uint8_t minor; /*!< minor number */
uint8_t build; /*!< build number */
} VL53L0X_Version_t;
/** @brief Defines the parameters of the Get Device Info Functions
*/
typedef struct {
char Name[VL53L0X_MAX_STRING_LENGTH];
/*!< Name of the Device e.g. Left_Distance */
char Type[VL53L0X_MAX_STRING_LENGTH];
/*!< Type of the Device e.g VL53L0X */
char ProductId[VL53L0X_MAX_STRING_LENGTH];
/*!< Product Identifier String */
uint8_t ProductType;
/*!< Product Type, VL53L0X = 1, VL53L1 = 2 */
uint8_t ProductRevisionMajor;
/*!< Product revision major */
uint8_t ProductRevisionMinor;
/*!< Product revision minor */
} VL53L0X_DeviceInfo_t;
/** @defgroup VL53L0X_define_Error_group Error and Warning code returned by API
* The following DEFINE are used to identify the PAL ERROR
* @{
*/
typedef int8_t VL53L0X_Error;
#define VL53L0X_ERROR_NONE ((VL53L0X_Error) 0)
#define VL53L0X_ERROR_CALIBRATION_WARNING ((VL53L0X_Error) - 1)
/*!< Warning invalid calibration data may be in used
* \a VL53L0X_InitData()
* \a VL53L0X_GetOffsetCalibrationData
* \a VL53L0X_SetOffsetCalibrationData
*/
#define VL53L0X_ERROR_MIN_CLIPPED ((VL53L0X_Error) - 2)
/*!< Warning parameter passed was clipped to min before to be applied */
#define VL53L0X_ERROR_UNDEFINED ((VL53L0X_Error) - 3)
/*!< Unqualified error */
#define VL53L0X_ERROR_INVALID_PARAMS ((VL53L0X_Error) - 4)
/*!< Parameter passed is invalid or out of range */
#define VL53L0X_ERROR_NOT_SUPPORTED ((VL53L0X_Error) - 5)
/*!< Function is not supported in current mode or configuration */
#define VL53L0X_ERROR_RANGE_ERROR ((VL53L0X_Error) - 6)
/*!< Device report a ranging error interrupt status */
#define VL53L0X_ERROR_TIME_OUT ((VL53L0X_Error) - 7)
/*!< Aborted due to time out */
#define VL53L0X_ERROR_MODE_NOT_SUPPORTED ((VL53L0X_Error) - 8)
/*!< Asked mode is not supported by the device */
#define VL53L0X_ERROR_BUFFER_TOO_SMALL ((VL53L0X_Error) - 9)
/*!< ... */
#define VL53L0X_ERROR_GPIO_NOT_EXISTING ((VL53L0X_Error) - 10)
/*!< User tried to setup a non-existing GPIO pin */
#define VL53L0X_ERROR_GPIO_FUNCTIONALITY_NOT_SUPPORTED ((VL53L0X_Error) - 11)
/*!< unsupported GPIO functionality */
#define VL53L0X_ERROR_INTERRUPT_NOT_CLEARED ((VL53L0X_Error) - 12)
/*!< Error during interrupt clear */
#define VL53L0X_ERROR_CONTROL_INTERFACE ((VL53L0X_Error) - 20)
/*!< error reported from IO functions */
#define VL53L0X_ERROR_INVALID_COMMAND ((VL53L0X_Error) - 30)
/*!< The command is not allowed in the current device state
* (power down)
*/
#define VL53L0X_ERROR_DIVISION_BY_ZERO ((VL53L0X_Error) - 40)
/*!< In the function a division by zero occurs */
#define VL53L0X_ERROR_REF_SPAD_INIT ((VL53L0X_Error) - 50)
/*!< Error during reference SPAD initialization */
#define VL53L0X_ERROR_NOT_IMPLEMENTED ((VL53L0X_Error) - 99)
/*!< Tells requested functionality has not been implemented yet or
* not compatible with the device
*/
/** @} VL53L0X_define_Error_group */
/** @defgroup VL53L0X_define_DeviceModes_group Defines Device modes
* Defines all possible modes for the device
* @{
*/
typedef uint8_t VL53L0X_DeviceModes;
#define VL53L0X_DEVICEMODE_SINGLE_RANGING ((VL53L0X_DeviceModes) 0)
#define VL53L0X_DEVICEMODE_CONTINUOUS_RANGING ((VL53L0X_DeviceModes) 1)
#define VL53L0X_DEVICEMODE_SINGLE_HISTOGRAM ((VL53L0X_DeviceModes) 2)
#define VL53L0X_DEVICEMODE_CONTINUOUS_TIMED_RANGING ((VL53L0X_DeviceModes) 3)
#define VL53L0X_DEVICEMODE_SINGLE_ALS ((VL53L0X_DeviceModes) 10)
#define VL53L0X_DEVICEMODE_GPIO_DRIVE ((VL53L0X_DeviceModes) 20)
#define VL53L0X_DEVICEMODE_GPIO_OSC ((VL53L0X_DeviceModes) 21)
/* ... Modes to be added depending on device */
/** @} VL53L0X_define_DeviceModes_group */
/** @defgroup VL53L0X_define_HistogramModes_group Defines Histogram modes
* Defines all possible Histogram modes for the device
* @{
*/
typedef uint8_t VL53L0X_HistogramModes;
#define VL53L0X_HISTOGRAMMODE_DISABLED ((VL53L0X_HistogramModes) 0)
/*!< Histogram Disabled */
#define VL53L0X_HISTOGRAMMODE_REFERENCE_ONLY ((VL53L0X_HistogramModes) 1)
/*!< Histogram Reference array only */
#define VL53L0X_HISTOGRAMMODE_RETURN_ONLY ((VL53L0X_HistogramModes) 2)
/*!< Histogram Return array only */
#define VL53L0X_HISTOGRAMMODE_BOTH ((VL53L0X_HistogramModes) 3)
/*!< Histogram both Reference and Return Arrays */
/* ... Modes to be added depending on device */
/** @} VL53L0X_define_HistogramModes_group */
/** @defgroup VL53L0X_define_PowerModes_group List of available Power Modes
* List of available Power Modes
* @{
*/
typedef uint8_t VL53L0X_PowerModes;
#define VL53L0X_POWERMODE_STANDBY_LEVEL1 ((VL53L0X_PowerModes) 0)
/*!< Standby level 1 */
#define VL53L0X_POWERMODE_STANDBY_LEVEL2 ((VL53L0X_PowerModes) 1)
/*!< Standby level 2 */
#define VL53L0X_POWERMODE_IDLE_LEVEL1 ((VL53L0X_PowerModes) 2)
/*!< Idle level 1 */
#define VL53L0X_POWERMODE_IDLE_LEVEL2 ((VL53L0X_PowerModes) 3)
/*!< Idle level 2 */
/** @} VL53L0X_define_PowerModes_group */
#define VL53L0X_DMAX_LUT_SIZE 7
/*!< Defines the number of items in the DMAX lookup table */
/** @brief Structure defining data pair that makes up the DMAX Lookup table.
*/
typedef struct {
FixPoint1616_t ambRate_mcps[VL53L0X_DMAX_LUT_SIZE];
/*!< Ambient rate (mcps) */
FixPoint1616_t dmax_mm[VL53L0X_DMAX_LUT_SIZE];
/*!< DMAX Value (mm) */
} VL53L0X_DMaxLUT_t;
/** @brief Defines all parameters for the device
*/
typedef struct {
VL53L0X_DeviceModes DeviceMode;
/*!< Defines type of measurement to be done for the next measure */
VL53L0X_HistogramModes HistogramMode;
/*!< Defines type of histogram measurement to be done for the next
* measure
*/
uint32_t MeasurementTimingBudgetMicroSeconds;
/*!< Defines the allowed total time for a single measurement */
uint32_t InterMeasurementPeriodMilliSeconds;
/*!< Defines time between two consecutive measurements (between two
* measurement starts). If set to 0 means back-to-back mode
*/
uint8_t XTalkCompensationEnable;
/*!< Tells if Crosstalk compensation shall be enable or not */
uint16_t XTalkCompensationRangeMilliMeter;
/*!< CrossTalk compensation range in millimeter */
FixPoint1616_t XTalkCompensationRateMegaCps;
/*!< CrossTalk compensation rate in Mega counts per seconds.
* Expressed in 16.16 fixed point format.
*/
int32_t RangeOffsetMicroMeters;
/*!< Range offset adjustment (mm). */
uint8_t LimitChecksEnable[VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS];
/*!< This Array store all the Limit Check enable for this device. */
uint8_t LimitChecksStatus[VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS];
/*!< This Array store all the Status of the check linked to last
* measurement.
*/
FixPoint1616_t LimitChecksValue[VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS];
/*!< This Array store all the Limit Check value for this device */
VL53L0X_DMaxLUT_t dmax_lut;
/*!< Lookup table defining ambient rates and associated
* dmax values.
*/
uint8_t WrapAroundCheckEnable;
/*!< Tells if Wrap Around Check shall be enable or not */
} VL53L0X_DeviceParameters_t;
/** @defgroup VL53L0X_define_State_group Defines the current status
* of the device
* Defines the current status of the device
* @{
*/
typedef uint8_t VL53L0X_State;
#define VL53L0X_STATE_POWERDOWN ((VL53L0X_State) 0)
/*!< Device is in HW reset */
#define VL53L0X_STATE_WAIT_STATICINIT ((VL53L0X_State) 1)
/*!< Device is initialized and wait for static initialization */
#define VL53L0X_STATE_STANDBY ((VL53L0X_State) 2)
/*!< Device is in Low power Standby mode */
#define VL53L0X_STATE_IDLE ((VL53L0X_State) 3)
/*!< Device has been initialized and ready to do measurements */
#define VL53L0X_STATE_RUNNING ((VL53L0X_State) 4)
/*!< Device is performing measurement */
#define VL53L0X_STATE_UNKNOWN ((VL53L0X_State) 98)
/*!< Device is in unknown state and need to be rebooted */
#define VL53L0X_STATE_ERROR ((VL53L0X_State) 99)
/*!< Device is in error state and need to be rebooted */
/** @} VL53L0X_define_State_group */
/**
* @struct VL53L0X_RangeData_t
* @brief Range measurement data.
*/
typedef struct {
uint32_t TimeStamp; /*!< 32-bit time stamp. */
uint32_t MeasurementTimeUsec;
/*!< Give the Measurement time needed by the device to do the
* measurement.
*/
uint16_t RangeMilliMeter; /*!< range distance in millimeter. */
uint16_t RangeDMaxMilliMeter;
/*!< Tells what is the maximum detection distance of the device
* in current setup and environment conditions (Filled when
* applicable)
*/
FixPoint1616_t SignalRateRtnMegaCps;
/*!< Return signal rate (MCPS)\n these is a 16.16 fix point
* value, which is effectively a measure of target
* reflectance.
*/
FixPoint1616_t AmbientRateRtnMegaCps;
/*!< Return ambient rate (MCPS)\n these is a 16.16 fix point
* value, which is effectively a measure of the ambien
* t light.
*/
uint16_t EffectiveSpadRtnCount;
/*!< Return the effective SPAD count for the return signal.
* To obtain Real value it should be divided by 256
*/
uint8_t ZoneId;
/*!< Denotes which zone and range scheduler stage the range
* data relates to.
*/
uint8_t RangeFractionalPart;
/*!< Fractional part of range distance. Final value is a
* FixPoint168 value.
*/
uint8_t RangeStatus;
/*!< Range Status for the current measurement. This is device
* dependent. Value = 0 means value is valid.
* See \ref RangeStatusPage
*/
} VL53L0X_RangingMeasurementData_t;
#define VL53L0X_HISTOGRAM_BUFFER_SIZE 24
/**
* @struct VL53L0X_HistogramData_t
* @brief Histogram measurement data.
*/
typedef struct {
/* Histogram Measurement data */
uint32_t HistogramData[VL53L0X_HISTOGRAM_BUFFER_SIZE];
/*!< Histogram data */
/*!< Indicate the types of histogram data :
*Return only, Reference only, both Return and Reference
*/
uint8_t FirstBin; /*!< First Bin value */
uint8_t BufferSize; /*!< Buffer Size - Set by the user.*/
uint8_t NumberOfBins;
/*!< Number of bins filled by the histogram measurement */
VL53L0X_DeviceError ErrorStatus;
/*!< Error status of the current measurement. \n
* see @a ::VL53L0X_DeviceError @a VL53L0X_GetStatusErrorString()
*/
} VL53L0X_HistogramMeasurementData_t;
#define VL53L0X_REF_SPAD_BUFFER_SIZE 6
/**
* @struct VL53L0X_SpadData_t
* @brief Spad Configuration Data.
*/
typedef struct {
uint8_t RefSpadEnables[VL53L0X_REF_SPAD_BUFFER_SIZE];
/*!< Reference Spad Enables */
uint8_t RefGoodSpadMap[VL53L0X_REF_SPAD_BUFFER_SIZE];
/*!< Reference Spad Good Spad Map */
} VL53L0X_SpadData_t;
typedef struct {
FixPoint1616_t OscFrequencyMHz; /* Frequency used */
uint16_t LastEncodedTimeout;
/* last encoded Time out used for timing budget*/
VL53L0X_GpioFunctionality Pin0GpioFunctionality;
/* store the functionality of the GPIO: pin0 */
uint32_t FinalRangeTimeoutMicroSecs;
/*!< Execution time of the final range*/
uint8_t FinalRangeVcselPulsePeriod;
/*!< Vcsel pulse period (pll clocks) for the final range measurement*/
uint32_t PreRangeTimeoutMicroSecs;
/*!< Execution time of the final range*/
uint8_t PreRangeVcselPulsePeriod;
/*!< Vcsel pulse period (pll clocks) for the pre-range measurement*/
uint16_t SigmaEstRefArray;
/*!< Reference array sigma value in 1/100th of [mm] e.g. 100 = 1mm */
uint16_t SigmaEstEffPulseWidth;
/*!< Effective Pulse width for sigma estimate in 1/100th
* of ns e.g. 900 = 9.0ns
*/
uint16_t SigmaEstEffAmbWidth;
/*!< Effective Ambient width for sigma estimate in 1/100th of ns
* e.g. 500 = 5.0ns
*/
/* Indicate if read from device has been done (==1) or not (==0) */
uint8_t ReadDataFromDeviceDone;
uint8_t ModuleId; /* Module ID */
uint8_t Revision; /* test Revision */
char ProductId[VL53L0X_MAX_STRING_LENGTH];
/* Product Identifier String */
uint8_t ReferenceSpadCount; /* used for ref spad management */
uint8_t ReferenceSpadType; /* used for ref spad management */
uint8_t RefSpadsInitialised; /* reports if ref spads are initialised. */
uint32_t PartUIDUpper; /*!< Unique Part ID Upper */
uint32_t PartUIDLower; /*!< Unique Part ID Lower */
/*!< Peek Signal rate at 400 mm*/
FixPoint1616_t SignalRateMeasFixed400mm;
} VL53L0X_DeviceSpecificParameters_t;
/**
* @struct VL53L0X_DevData_t
*
* @brief VL53L0X PAL device ST private data structure \n
* End user should never access any of these field directly
*
* These must never access directly but only via macro
*/
typedef struct {
int32_t Part2PartOffsetNVMMicroMeter;
/*!< backed up NVM value */
int32_t Part2PartOffsetAdjustmentNVMMicroMeter;
/*!< backed up NVM value representing additional offset adjustment */
VL53L0X_DeviceParameters_t CurrentParameters;
/*!< Current Device Parameter */
VL53L0X_RangingMeasurementData_t LastRangeMeasure;
/*!< Ranging Data */
VL53L0X_HistogramMeasurementData_t LastHistogramMeasure;
/*!< Histogram Data */
VL53L0X_DeviceSpecificParameters_t DeviceSpecificParameters;
/*!< Parameters specific to the device */
VL53L0X_SpadData_t SpadData;
/*!< Spad Data */
uint8_t SequenceConfig;
/*!< Internal value for the sequence config */
uint8_t RangeFractionalEnable;
/*!< Enable/Disable fractional part of ranging data */
VL53L0X_State PalState;
/*!< Current state of the PAL for this device */
VL53L0X_PowerModes PowerMode;
/*!< Current Power Mode */
uint16_t SigmaEstRefArray;
/*!< Reference array sigma value in 1/100th of [mm] e.g. 100 = 1mm */
uint16_t SigmaEstEffPulseWidth;
/*!< Effective Pulse width for sigma estimate in 1/100th
* of ns e.g. 900 = 9.0ns
*/
uint16_t SigmaEstEffAmbWidth;
/*!< Effective Ambient width for sigma estimate in 1/100th of ns
* e.g. 500 = 5.0ns
*/
uint8_t StopVariable;
/*!< StopVariable used during the stop sequence */
uint16_t targetRefRate;
/*!< Target Ambient Rate for Ref spad management */
FixPoint1616_t SigmaEstimate;
/*!< Sigma Estimate - based on ambient & VCSEL rates and
* signal_total_events
*/
FixPoint1616_t SignalEstimate;
/*!< Signal Estimate - based on ambient & VCSEL rates and cross talk */
FixPoint1616_t LastSignalRefMcps;
/*!< Latest Signal ref in Mcps */
uint8_t *pTuningSettingsPointer;
/*!< Pointer for Tuning Settings table */
uint8_t UseInternalTuningSettings;
/*!< Indicate if we use Tuning Settings table */
uint16_t LinearityCorrectiveGain;
/*!< Linearity Corrective Gain value in x1000 */
} VL53L0X_DevData_t;
/** @defgroup VL53L0X_define_InterruptPolarity_group Defines the Polarity
* of the Interrupt
* Defines the Polarity of the Interrupt
* @{
*/
typedef uint8_t VL53L0X_InterruptPolarity;
#define VL53L0X_INTERRUPTPOLARITY_LOW ((VL53L0X_InterruptPolarity) 0)
/*!< Set active low polarity best setup for falling edge. */
#define VL53L0X_INTERRUPTPOLARITY_HIGH ((VL53L0X_InterruptPolarity) 1)
/*!< Set active high polarity best setup for rising edge. */
/** @} VL53L0X_define_InterruptPolarity_group */
/** @defgroup VL53L0X_define_VcselPeriod_group Vcsel Period Defines
* Defines the range measurement for which to access the vcsel period.
* @{
*/
typedef uint8_t VL53L0X_VcselPeriod;
#define VL53L0X_VCSEL_PERIOD_PRE_RANGE ((VL53L0X_VcselPeriod) 0)
/*!<Identifies the pre-range vcsel period. */
#define VL53L0X_VCSEL_PERIOD_FINAL_RANGE ((VL53L0X_VcselPeriod) 1)
/*!<Identifies the final range vcsel period. */
/** @} VL53L0X_define_VcselPeriod_group */
/** @defgroup VL53L0X_define_SchedulerSequence_group Defines the steps
* carried out by the scheduler during a range measurement.
* @{
* Defines the states of all the steps in the scheduler
* i.e. enabled/disabled.
*/
typedef struct {
uint8_t TccOn; /*!<Reports if Target Centre Check On */
uint8_t MsrcOn; /*!<Reports if MSRC On */
uint8_t DssOn; /*!<Reports if DSS On */
uint8_t PreRangeOn; /*!<Reports if Pre-Range On */
uint8_t FinalRangeOn; /*!<Reports if Final-Range On */
} VL53L0X_SchedulerSequenceSteps_t;
/** @} VL53L0X_define_SchedulerSequence_group */
/** @defgroup VL53L0X_define_SequenceStepId_group Defines the Polarity
* of the Interrupt
* Defines the the sequence steps performed during ranging..
* @{
*/
typedef uint8_t VL53L0X_SequenceStepId;
#define VL53L0X_SEQUENCESTEP_TCC ((VL53L0X_VcselPeriod) 0)
/*!<Target CentreCheck identifier. */
#define VL53L0X_SEQUENCESTEP_DSS ((VL53L0X_VcselPeriod) 1)
/*!<Dynamic Spad Selection function Identifier. */
#define VL53L0X_SEQUENCESTEP_MSRC ((VL53L0X_VcselPeriod) 2)
/*!<Minimum Signal Rate Check function Identifier. */
#define VL53L0X_SEQUENCESTEP_PRE_RANGE ((VL53L0X_VcselPeriod) 3)
/*!<Pre-Range check Identifier. */
#define VL53L0X_SEQUENCESTEP_FINAL_RANGE ((VL53L0X_VcselPeriod) 4)
/*!<Final Range Check Identifier. */
#define VL53L0X_SEQUENCESTEP_NUMBER_OF_CHECKS 5
/*!<Number of Sequence Step Managed by the API. */
/** @} VL53L0X_define_SequenceStepId_group */
/* MACRO Definitions */
/** @defgroup VL53L0X_define_GeneralMacro_group General Macro Defines
* General Macro Defines
* @{
*/
/* Defines */
#define VL53L0X_SETPARAMETERFIELD(Dev, field, value) \
PALDevDataSet(Dev, CurrentParameters.field, value)
#define VL53L0X_GETPARAMETERFIELD(Dev, field, variable) \
(variable = ((PALDevDataGet(Dev, CurrentParameters)).field))
#define VL53L0X_SETARRAYPARAMETERFIELD(Dev, field, index, value) \
PALDevDataSet(Dev, CurrentParameters.field[index], value)
#define VL53L0X_GETARRAYPARAMETERFIELD(Dev, field, index, variable) \
(variable = (PALDevDataGet(Dev, CurrentParameters)).field[index])
#define VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, field, value) \
PALDevDataSet(Dev, DeviceSpecificParameters.field, value)
#define VL53L0X_GETDEVICESPECIFICPARAMETER(Dev, field) \
PALDevDataGet(Dev, DeviceSpecificParameters).field
#define VL53L0X_FIXPOINT1616TOFIXPOINT97(Value) \
(uint16_t)((Value>>9)&0xFFFF)
#define VL53L0X_FIXPOINT97TOFIXPOINT1616(Value) \
(FixPoint1616_t)(Value<<9)
#define VL53L0X_FIXPOINT1616TOFIXPOINT88(Value) \
(uint16_t)((Value>>8)&0xFFFF)
#define VL53L0X_FIXPOINT88TOFIXPOINT1616(Value) \
(FixPoint1616_t)(Value<<8)
#define VL53L0X_FIXPOINT1616TOFIXPOINT412(Value) \
(uint16_t)((Value>>4)&0xFFFF)
#define VL53L0X_FIXPOINT412TOFIXPOINT1616(Value) \
(FixPoint1616_t)(Value<<4)
#define VL53L0X_FIXPOINT1616TOFIXPOINT313(Value) \
(uint16_t)((Value>>3)&0xFFFF)
#define VL53L0X_FIXPOINT313TOFIXPOINT1616(Value) \
(FixPoint1616_t)(Value<<3)
#define VL53L0X_FIXPOINT1616TOFIXPOINT08(Value) \
(uint8_t)((Value>>8)&0x00FF)
#define VL53L0X_FIXPOINT08TOFIXPOINT1616(Value) \
(FixPoint1616_t)(Value<<8)
#define VL53L0X_FIXPOINT1616TOFIXPOINT53(Value) \
(uint8_t)((Value>>13)&0x00FF)
#define VL53L0X_FIXPOINT53TOFIXPOINT1616(Value) \
(FixPoint1616_t)(Value<<13)
#define VL53L0X_FIXPOINT1616TOFIXPOINT102(Value) \
(uint16_t)((Value>>14)&0x0FFF)
#define VL53L0X_FIXPOINT102TOFIXPOINT1616(Value) \
(FixPoint1616_t)(Value<<12)
#define VL53L0X_MAKEUINT16(lsb, msb) (uint16_t)((((uint16_t)msb)<<8) + \
(uint16_t)lsb)
/** @} VL53L0X_define_GeneralMacro_group */
/** @} VL53L0X_globaldefine_group */
#ifdef __cplusplus
}
#endif
#define TRACE_MODULE_API 0
#define _LOG_FUNCTION_START(...) (void)0
#define _LOG_FUNCTION_END(...) (void)0
#define _LOG_FUNCTION_END_FMT(...) (void)0
#endif /* _VL53L0X_DEF_H_ */

262
App/VL53L0X_API/core/inc/vl53l0x_device.h Normal file
View File

@@ -0,0 +1,262 @@
/*******************************************************************************
* Copyright © 2016, STMicroelectronics International N.V.
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of STMicroelectronics nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/
/**
* Device specific defines. To be adapted by implementer for the targeted
* device.
*/
#ifndef _VL53L0X_DEVICE_H_
#define _VL53L0X_DEVICE_H_
#include "vl53l0x_types.h"
/** @defgroup VL53L0X_DevSpecDefines_group VL53L0X cut1.1 Device
* Specific Defines
* @brief VL53L0X cut1.1 Device Specific Defines
* @{
*/
/** @defgroup VL53L0X_DeviceError_group Device Error
* @brief Device Error code
*
* This enum is Device specific it should be updated in the implementation
* Use @a VL53L0X_GetStatusErrorString() to get the string.
* It is related to Status Register of the Device.
* @{
*/
typedef uint8_t VL53L0X_DeviceError;
#define VL53L0X_DEVICEERROR_NONE ((VL53L0X_DeviceError) 0)
/*!< 0 NoError */
#define VL53L0X_DEVICEERROR_VCSELCONTINUITYTESTFAILURE ((VL53L0X_DeviceError) 1)
#define VL53L0X_DEVICEERROR_VCSELWATCHDOGTESTFAILURE ((VL53L0X_DeviceError) 2)
#define VL53L0X_DEVICEERROR_NOVHVVALUEFOUND ((VL53L0X_DeviceError) 3)
#define VL53L0X_DEVICEERROR_MSRCNOTARGET ((VL53L0X_DeviceError) 4)
#define VL53L0X_DEVICEERROR_SNRCHECK ((VL53L0X_DeviceError) 5)
#define VL53L0X_DEVICEERROR_RANGEPHASECHECK ((VL53L0X_DeviceError) 6)
#define VL53L0X_DEVICEERROR_SIGMATHRESHOLDCHECK ((VL53L0X_DeviceError) 7)
#define VL53L0X_DEVICEERROR_TCC ((VL53L0X_DeviceError) 8)
#define VL53L0X_DEVICEERROR_PHASECONSISTENCY ((VL53L0X_DeviceError) 9)
#define VL53L0X_DEVICEERROR_MINCLIP ((VL53L0X_DeviceError) 10)
#define VL53L0X_DEVICEERROR_RANGECOMPLETE ((VL53L0X_DeviceError) 11)
#define VL53L0X_DEVICEERROR_ALGOUNDERFLOW ((VL53L0X_DeviceError) 12)
#define VL53L0X_DEVICEERROR_ALGOOVERFLOW ((VL53L0X_DeviceError) 13)
#define VL53L0X_DEVICEERROR_RANGEIGNORETHRESHOLD ((VL53L0X_DeviceError) 14)
/** @} end of VL53L0X_DeviceError_group */
/** @defgroup VL53L0X_CheckEnable_group Check Enable list
* @brief Check Enable code
*
* Define used to specify the LimitCheckId.
* Use @a VL53L0X_GetLimitCheckInfo() to get the string.
* @{
*/
#define VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE 0
#define VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE 1
#define VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP 2
#define VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD 3
#define VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC 4
#define VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE 5
#define VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS 6
/** @} end of VL53L0X_CheckEnable_group */
/** @defgroup VL53L0X_GpioFunctionality_group Gpio Functionality
* @brief Defines the different functionalities for the device GPIO(s)
* @{
*/
typedef uint8_t VL53L0X_GpioFunctionality;
#define VL53L0X_GPIOFUNCTIONALITY_OFF \
((VL53L0X_GpioFunctionality) 0) /*!< NO Interrupt */
#define VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_LOW \
((VL53L0X_GpioFunctionality) 1) /*!< Level Low (value < thresh_low) */
#define VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_HIGH \
((VL53L0X_GpioFunctionality) 2) /*!< Level High (value>thresh_high) */
#define VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_OUT \
((VL53L0X_GpioFunctionality) 3)
/*!< Out Of Window (value < thresh_low OR value > thresh_high) */
#define VL53L0X_GPIOFUNCTIONALITY_NEW_MEASURE_READY \
((VL53L0X_GpioFunctionality) 4) /*!< New Sample Ready */
/** @} end of VL53L0X_GpioFunctionality_group */
/* Device register map */
/** @defgroup VL53L0X_DefineRegisters_group Define Registers
* @brief List of all the defined registers
* @{
*/
#define VL53L0X_REG_SYSRANGE_START 0x000
/** mask existing bit in #VL53L0X_REG_SYSRANGE_START*/
#define VL53L0X_REG_SYSRANGE_MODE_MASK 0x0F
/** bit 0 in #VL53L0X_REG_SYSRANGE_START write 1 toggle state in
* continuous mode and arm next shot in single shot mode
*/
#define VL53L0X_REG_SYSRANGE_MODE_START_STOP 0x01
/** bit 1 write 0 in #VL53L0X_REG_SYSRANGE_START set single shot mode */
#define VL53L0X_REG_SYSRANGE_MODE_SINGLESHOT 0x00
/** bit 1 write 1 in #VL53L0X_REG_SYSRANGE_START set back-to-back
* operation mode
*/
#define VL53L0X_REG_SYSRANGE_MODE_BACKTOBACK 0x02
/** bit 2 write 1 in #VL53L0X_REG_SYSRANGE_START set timed operation
* mode
*/
#define VL53L0X_REG_SYSRANGE_MODE_TIMED 0x04
/** bit 3 write 1 in #VL53L0X_REG_SYSRANGE_START set histogram operation
* mode
*/
#define VL53L0X_REG_SYSRANGE_MODE_HISTOGRAM 0x08
#define VL53L0X_REG_SYSTEM_THRESH_HIGH 0x000C
#define VL53L0X_REG_SYSTEM_THRESH_LOW 0x000E
#define VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG 0x0001
#define VL53L0X_REG_SYSTEM_RANGE_CONFIG 0x0009
#define VL53L0X_REG_SYSTEM_INTERMEASUREMENT_PERIOD 0x0004
#define VL53L0X_REG_SYSTEM_INTERRUPT_CONFIG_GPIO 0x000A
#define VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_DISABLED 0x00
#define VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_LEVEL_LOW 0x01
#define VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_LEVEL_HIGH 0x02
#define VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_OUT_OF_WINDOW 0x03
#define VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY 0x04
#define VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH 0x0084
#define VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR 0x000B
/* Result registers */
#define VL53L0X_REG_RESULT_INTERRUPT_STATUS 0x0013
#define VL53L0X_REG_RESULT_RANGE_STATUS 0x0014
#define VL53L0X_REG_RESULT_CORE_PAGE 1
#define VL53L0X_REG_RESULT_CORE_AMBIENT_WINDOW_EVENTS_RTN 0x00BC
#define VL53L0X_REG_RESULT_CORE_RANGING_TOTAL_EVENTS_RTN 0x00C0
#define VL53L0X_REG_RESULT_CORE_AMBIENT_WINDOW_EVENTS_REF 0x00D0
#define VL53L0X_REG_RESULT_CORE_RANGING_TOTAL_EVENTS_REF 0x00D4
#define VL53L0X_REG_RESULT_PEAK_SIGNAL_RATE_REF 0x00B6
/* Algo register */
#define VL53L0X_REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM 0x0028
#define VL53L0X_REG_I2C_SLAVE_DEVICE_ADDRESS 0x008a
/* Check Limit registers */
#define VL53L0X_REG_MSRC_CONFIG_CONTROL 0x0060
#define VL53L0X_REG_PRE_RANGE_CONFIG_MIN_SNR 0X0027
#define VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW 0x0056
#define VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH 0x0057
#define VL53L0X_REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT 0x0064
#define VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_SNR 0X0067
#define VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW 0x0047
#define VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH 0x0048
#define VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT 0x0044
#define VL53L0X_REG_PRE_RANGE_CONFIG_SIGMA_THRESH_HI 0X0061
#define VL53L0X_REG_PRE_RANGE_CONFIG_SIGMA_THRESH_LO 0X0062
/* PRE RANGE registers */
#define VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD 0x0050
#define VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI 0x0051
#define VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_LO 0x0052
#define VL53L0X_REG_SYSTEM_HISTOGRAM_BIN 0x0081
#define VL53L0X_REG_HISTOGRAM_CONFIG_INITIAL_PHASE_SELECT 0x0033
#define VL53L0X_REG_HISTOGRAM_CONFIG_READOUT_CTRL 0x0055
#define VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD 0x0070
#define VL53L0X_REG_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI 0x0071
#define VL53L0X_REG_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_LO 0x0072
#define VL53L0X_REG_CROSSTALK_COMPENSATION_PEAK_RATE_MCPS 0x0020
#define VL53L0X_REG_MSRC_CONFIG_TIMEOUT_MACROP 0x0046
#define VL53L0X_REG_SOFT_RESET_GO2_SOFT_RESET_N 0x00bf
#define VL53L0X_REG_IDENTIFICATION_MODEL_ID 0x00c0
#define VL53L0X_REG_IDENTIFICATION_REVISION_ID 0x00c2
#define VL53L0X_REG_OSC_CALIBRATE_VAL 0x00f8
#define VL53L0X_SIGMA_ESTIMATE_MAX_VALUE 65535
/* equivalent to a range sigma of 655.35mm */
#define VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH 0x032
#define VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0 0x0B0
#define VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_1 0x0B1
#define VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_2 0x0B2
#define VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_3 0x0B3
#define VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_4 0x0B4
#define VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_5 0x0B5
#define VL53L0X_REG_GLOBAL_CONFIG_REF_EN_START_SELECT 0xB6
#define VL53L0X_REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD 0x4E /* 0x14E */
#define VL53L0X_REG_DYNAMIC_SPAD_REF_EN_START_OFFSET 0x4F /* 0x14F */
#define VL53L0X_REG_POWER_MANAGEMENT_GO1_POWER_FORCE 0x80
/*
* Speed of light in um per 1E-10 Seconds
*/
#define VL53L0X_SPEED_OF_LIGHT_IN_AIR 2997
#define VL53L0X_REG_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV 0x0089
#define VL53L0X_REG_ALGO_PHASECAL_LIM 0x0030 /* 0x130 */
#define VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT 0x0030
/** @} VL53L0X_DefineRegisters_group */
/** @} VL53L0X_DevSpecDefines_group */
#endif
/* _VL53L0X_DEVICE_H_ */

194
App/VL53L0X_API/core/inc/vl53l0x_interrupt_threshold_settings.h Normal file
View File

@@ -0,0 +1,194 @@
/*******************************************************************************
* Copyright © 2016, STMicroelectronics International N.V.
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of STMicroelectronics nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/
#ifndef _VL53L0X_INTERRUPT_THRESHOLD_SETTINGS_H_
#define _VL53L0X_INTERRUPT_THRESHOLD_SETTINGS_H_
#ifdef __cplusplus
extern "C" {
#endif
uint8_t InterruptThresholdSettings[] = {
/* Start of Interrupt Threshold Settings */
0x1, 0xff, 0x00,
0x1, 0x80, 0x01,
0x1, 0xff, 0x01,
0x1, 0x00, 0x00,
0x1, 0xff, 0x01,
0x1, 0x4f, 0x02,
0x1, 0xFF, 0x0E,
0x1, 0x00, 0x03,
0x1, 0x01, 0x84,
0x1, 0x02, 0x0A,
0x1, 0x03, 0x03,
0x1, 0x04, 0x08,
0x1, 0x05, 0xC8,
0x1, 0x06, 0x03,
0x1, 0x07, 0x8D,
0x1, 0x08, 0x08,
0x1, 0x09, 0xC6,
0x1, 0x0A, 0x01,
0x1, 0x0B, 0x02,
0x1, 0x0C, 0x00,
0x1, 0x0D, 0xD5,
0x1, 0x0E, 0x18,
0x1, 0x0F, 0x12,
0x1, 0x10, 0x01,
0x1, 0x11, 0x82,
0x1, 0x12, 0x00,
0x1, 0x13, 0xD5,
0x1, 0x14, 0x18,
0x1, 0x15, 0x13,
0x1, 0x16, 0x03,
0x1, 0x17, 0x86,
0x1, 0x18, 0x0A,
0x1, 0x19, 0x09,
0x1, 0x1A, 0x08,
0x1, 0x1B, 0xC2,
0x1, 0x1C, 0x03,
0x1, 0x1D, 0x8F,
0x1, 0x1E, 0x0A,
0x1, 0x1F, 0x06,
0x1, 0x20, 0x01,
0x1, 0x21, 0x02,
0x1, 0x22, 0x00,
0x1, 0x23, 0xD5,
0x1, 0x24, 0x18,
0x1, 0x25, 0x22,
0x1, 0x26, 0x01,
0x1, 0x27, 0x82,
0x1, 0x28, 0x00,
0x1, 0x29, 0xD5,
0x1, 0x2A, 0x18,
0x1, 0x2B, 0x0B,
0x1, 0x2C, 0x28,
0x1, 0x2D, 0x78,
0x1, 0x2E, 0x28,
0x1, 0x2F, 0x91,
0x1, 0x30, 0x00,
0x1, 0x31, 0x0B,
0x1, 0x32, 0x00,
0x1, 0x33, 0x0B,
0x1, 0x34, 0x00,
0x1, 0x35, 0xA1,
0x1, 0x36, 0x00,
0x1, 0x37, 0xA0,
0x1, 0x38, 0x00,
0x1, 0x39, 0x04,
0x1, 0x3A, 0x28,
0x1, 0x3B, 0x30,
0x1, 0x3C, 0x0C,
0x1, 0x3D, 0x04,
0x1, 0x3E, 0x0F,
0x1, 0x3F, 0x79,
0x1, 0x40, 0x28,
0x1, 0x41, 0x1E,
0x1, 0x42, 0x2F,
0x1, 0x43, 0x87,
0x1, 0x44, 0x00,
0x1, 0x45, 0x0B,
0x1, 0x46, 0x00,
0x1, 0x47, 0x0B,
0x1, 0x48, 0x00,
0x1, 0x49, 0xA7,
0x1, 0x4A, 0x00,
0x1, 0x4B, 0xA6,
0x1, 0x4C, 0x00,
0x1, 0x4D, 0x04,
0x1, 0x4E, 0x01,
0x1, 0x4F, 0x00,
0x1, 0x50, 0x00,
0x1, 0x51, 0x80,
0x1, 0x52, 0x09,
0x1, 0x53, 0x08,
0x1, 0x54, 0x01,
0x1, 0x55, 0x00,
0x1, 0x56, 0x0F,
0x1, 0x57, 0x79,
0x1, 0x58, 0x09,
0x1, 0x59, 0x05,
0x1, 0x5A, 0x00,
0x1, 0x5B, 0x60,
0x1, 0x5C, 0x05,
0x1, 0x5D, 0xD1,
0x1, 0x5E, 0x0C,
0x1, 0x5F, 0x3C,
0x1, 0x60, 0x00,
0x1, 0x61, 0xD0,
0x1, 0x62, 0x0B,
0x1, 0x63, 0x03,
0x1, 0x64, 0x28,
0x1, 0x65, 0x10,
0x1, 0x66, 0x2A,
0x1, 0x67, 0x39,
0x1, 0x68, 0x0B,
0x1, 0x69, 0x02,
0x1, 0x6A, 0x28,
0x1, 0x6B, 0x10,
0x1, 0x6C, 0x2A,
0x1, 0x6D, 0x61,
0x1, 0x6E, 0x0C,
0x1, 0x6F, 0x00,
0x1, 0x70, 0x0F,
0x1, 0x71, 0x79,
0x1, 0x72, 0x00,
0x1, 0x73, 0x0B,
0x1, 0x74, 0x00,
0x1, 0x75, 0x0B,
0x1, 0x76, 0x00,
0x1, 0x77, 0xA1,
0x1, 0x78, 0x00,
0x1, 0x79, 0xA0,
0x1, 0x7A, 0x00,
0x1, 0x7B, 0x04,
0x1, 0xFF, 0x04,
0x1, 0x79, 0x1D,
0x1, 0x7B, 0x27,
0x1, 0x96, 0x0E,
0x1, 0x97, 0xFE,
0x1, 0x98, 0x03,
0x1, 0x99, 0xEF,
0x1, 0x9A, 0x02,
0x1, 0x9B, 0x44,
0x1, 0x73, 0x07,
0x1, 0x70, 0x01,
0x1, 0xff, 0x01,
0x1, 0x00, 0x01,
0x1, 0xff, 0x00,
0x00, 0x00, 0x00
};
#ifdef __cplusplus
}
#endif
#endif /* _VL53L0X_INTERRUPT_THRESHOLD_SETTINGS_H_ */

146
App/VL53L0X_API/core/inc/vl53l0x_tuning.h Normal file
View File

@@ -0,0 +1,146 @@
/*******************************************************************************
* Copyright © 2016, STMicroelectronics International N.V.
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of STMicroelectronics nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/
#ifndef _VL53L0X_TUNING_H_
#define _VL53L0X_TUNING_H_
#include "vl53l0x_def.h"
#ifdef __cplusplus
extern "C" {
#endif
uint8_t DefaultTuningSettings[] = {
/* update 02/11/2015_v36 */
0x01, 0xFF, 0x01,
0x01, 0x00, 0x00,
0x01, 0xFF, 0x00,
0x01, 0x09, 0x00,
0x01, 0x10, 0x00,
0x01, 0x11, 0x00,
0x01, 0x24, 0x01,
0x01, 0x25, 0xff,
0x01, 0x75, 0x00,
0x01, 0xFF, 0x01,
0x01, 0x4e, 0x2c,
0x01, 0x48, 0x00,
0x01, 0x30, 0x20,
0x01, 0xFF, 0x00,
0x01, 0x30, 0x09, /* mja changed from 0x64. */
0x01, 0x54, 0x00,
0x01, 0x31, 0x04,
0x01, 0x32, 0x03,
0x01, 0x40, 0x83,
0x01, 0x46, 0x25,
0x01, 0x60, 0x00,
0x01, 0x27, 0x00,
0x01, 0x50, 0x06,
0x01, 0x51, 0x00,
0x01, 0x52, 0x96,
0x01, 0x56, 0x08,
0x01, 0x57, 0x30,
0x01, 0x61, 0x00,
0x01, 0x62, 0x00,
0x01, 0x64, 0x00,
0x01, 0x65, 0x00,
0x01, 0x66, 0xa0,
0x01, 0xFF, 0x01,
0x01, 0x22, 0x32,
0x01, 0x47, 0x14,
0x01, 0x49, 0xff,
0x01, 0x4a, 0x00,
0x01, 0xFF, 0x00,
0x01, 0x7a, 0x0a,
0x01, 0x7b, 0x00,
0x01, 0x78, 0x21,
0x01, 0xFF, 0x01,
0x01, 0x23, 0x34,
0x01, 0x42, 0x00,
0x01, 0x44, 0xff,
0x01, 0x45, 0x26,
0x01, 0x46, 0x05,
0x01, 0x40, 0x40,
0x01, 0x0E, 0x06,
0x01, 0x20, 0x1a,
0x01, 0x43, 0x40,
0x01, 0xFF, 0x00,
0x01, 0x34, 0x03,
0x01, 0x35, 0x44,
0x01, 0xFF, 0x01,
0x01, 0x31, 0x04,
0x01, 0x4b, 0x09,
0x01, 0x4c, 0x05,
0x01, 0x4d, 0x04,
0x01, 0xFF, 0x00,
0x01, 0x44, 0x00,
0x01, 0x45, 0x20,
0x01, 0x47, 0x08,
0x01, 0x48, 0x28,
0x01, 0x67, 0x00,
0x01, 0x70, 0x04,
0x01, 0x71, 0x01,
0x01, 0x72, 0xfe,
0x01, 0x76, 0x00,
0x01, 0x77, 0x00,
0x01, 0xFF, 0x01,
0x01, 0x0d, 0x01,
0x01, 0xFF, 0x00,
0x01, 0x80, 0x01,
0x01, 0x01, 0xF8,
0x01, 0xFF, 0x01,
0x01, 0x8e, 0x01,
0x01, 0x00, 0x01,
0x01, 0xFF, 0x00,
0x01, 0x80, 0x00,
0x00, 0x00, 0x00
};
#ifdef __cplusplus
}
#endif
#endif /* _VL53L0X_TUNING_H_ */

3141
App/VL53L0X_API/core/src/vl53l0x_api.c Normal file
View File

File diff suppressed because it is too large Load Diff

1288
App/VL53L0X_API/core/src/vl53l0x_api_calibration.c Normal file
View File

File diff suppressed because it is too large Load Diff

2128
App/VL53L0X_API/core/src/vl53l0x_api_core.c Normal file
View File

File diff suppressed because it is too large Load Diff

42
App/VL53L0X_API/core/src/vl53l0x_api_ranging.c Normal file
View File

@@ -0,0 +1,42 @@
/*******************************************************************************
* Copyright © 2016, STMicroelectronics International N.V.
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of STMicroelectronics nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
******************************************************************************/
#include "vl53l0x_api.h"
#include "vl53l0x_api_core.h"
#ifndef __KERNEL__
#include <stdlib.h>
#endif
#define LOG_FUNCTION_START(fmt, ...) \
_LOG_FUNCTION_START(TRACE_MODULE_API, fmt, ##__VA_ARGS__)
#define LOG_FUNCTION_END(status, ...) \
_LOG_FUNCTION_END(TRACE_MODULE_API, status, ##__VA_ARGS__)
#define LOG_FUNCTION_END_FMT(status, fmt, ...) \
_LOG_FUNCTION_END_FMT(TRACE_MODULE_API, status, fmt, ##__VA_ARGS__)

466
App/VL53L0X_API/core/src/vl53l0x_api_strings.c Normal file
View File

@@ -0,0 +1,466 @@
/*******************************************************************************
* Copyright © 2016, STMicroelectronics International N.V.
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of STMicroelectronics nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
******************************************************************************/
#include "vl53l0x_api.h"
#include "vl53l0x_api_core.h"
#include "vl53l0x_api_strings.h"
#ifndef __KERNEL__
#include <stdlib.h>
#endif
#define LOG_FUNCTION_START(fmt, ...) \
_LOG_FUNCTION_START(TRACE_MODULE_API, fmt, ##__VA_ARGS__)
#define LOG_FUNCTION_END(status, ...) \
_LOG_FUNCTION_END(TRACE_MODULE_API, status, ##__VA_ARGS__)
#define LOG_FUNCTION_END_FMT(status, fmt, ...) \
_LOG_FUNCTION_END_FMT(TRACE_MODULE_API, status, fmt, ##__VA_ARGS__)
VL53L0X_Error VL53L0X_check_part_used(VL53L0X_DEV Dev,
uint8_t *Revision,
VL53L0X_DeviceInfo_t *pVL53L0X_DeviceInfo)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
uint8_t ModuleIdInt;
char *ProductId_tmp;
LOG_FUNCTION_START("");
Status = VL53L0X_get_info_from_device(Dev, 2);
if (Status == VL53L0X_ERROR_NONE) {
ModuleIdInt = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev, ModuleId);
if (ModuleIdInt == 0) {
*Revision = 0;
VL53L0X_COPYSTRING(pVL53L0X_DeviceInfo->ProductId, "");
} else {
*Revision = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev, Revision);
ProductId_tmp = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev,
ProductId);
VL53L0X_COPYSTRING(pVL53L0X_DeviceInfo->ProductId,
ProductId_tmp);
}
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X_get_device_info(VL53L0X_DEV Dev,
VL53L0X_DeviceInfo_t *pVL53L0X_DeviceInfo)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
uint8_t revision_id;
uint8_t Revision;
Status = VL53L0X_check_part_used(Dev, &Revision, pVL53L0X_DeviceInfo);
if (Status == VL53L0X_ERROR_NONE) {
if (Revision == 0) {
VL53L0X_COPYSTRING(pVL53L0X_DeviceInfo->Name,
VL53L0X_STRING_DEVICE_INFO_NAME_TS0);
} else if ((Revision <= 34) && (Revision != 32)) {
VL53L0X_COPYSTRING(pVL53L0X_DeviceInfo->Name,
VL53L0X_STRING_DEVICE_INFO_NAME_TS1);
} else if (Revision < 39) {
VL53L0X_COPYSTRING(pVL53L0X_DeviceInfo->Name,
VL53L0X_STRING_DEVICE_INFO_NAME_TS2);
} else {
VL53L0X_COPYSTRING(pVL53L0X_DeviceInfo->Name,
VL53L0X_STRING_DEVICE_INFO_NAME_ES1);
}
VL53L0X_COPYSTRING(pVL53L0X_DeviceInfo->Type,
VL53L0X_STRING_DEVICE_INFO_TYPE);
}
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_RdByte(Dev,
VL53L0X_REG_IDENTIFICATION_MODEL_ID,
&pVL53L0X_DeviceInfo->ProductType);
}
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_RdByte(Dev,
VL53L0X_REG_IDENTIFICATION_REVISION_ID,
&revision_id);
pVL53L0X_DeviceInfo->ProductRevisionMajor = 1;
pVL53L0X_DeviceInfo->ProductRevisionMinor =
(revision_id & 0xF0) >> 4;
}
return Status;
}
VL53L0X_Error VL53L0X_get_device_error_string(VL53L0X_DeviceError ErrorCode,
char *pDeviceErrorString)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
LOG_FUNCTION_START("");
switch (ErrorCode) {
case VL53L0X_DEVICEERROR_NONE:
VL53L0X_COPYSTRING(pDeviceErrorString,
VL53L0X_STRING_DEVICEERROR_NONE);
break;
case VL53L0X_DEVICEERROR_VCSELCONTINUITYTESTFAILURE:
VL53L0X_COPYSTRING(pDeviceErrorString,
VL53L0X_STRING_DEVICEERROR_VCSELCONTINUITYTESTFAILURE);
break;
case VL53L0X_DEVICEERROR_VCSELWATCHDOGTESTFAILURE:
VL53L0X_COPYSTRING(pDeviceErrorString,
VL53L0X_STRING_DEVICEERROR_VCSELWATCHDOGTESTFAILURE);
break;
case VL53L0X_DEVICEERROR_NOVHVVALUEFOUND:
VL53L0X_COPYSTRING(pDeviceErrorString,
VL53L0X_STRING_DEVICEERROR_NOVHVVALUEFOUND);
break;
case VL53L0X_DEVICEERROR_MSRCNOTARGET:
VL53L0X_COPYSTRING(pDeviceErrorString,
VL53L0X_STRING_DEVICEERROR_MSRCNOTARGET);
break;
case VL53L0X_DEVICEERROR_SNRCHECK:
VL53L0X_COPYSTRING(pDeviceErrorString,
VL53L0X_STRING_DEVICEERROR_SNRCHECK);
break;
case VL53L0X_DEVICEERROR_RANGEPHASECHECK:
VL53L0X_COPYSTRING(pDeviceErrorString,
VL53L0X_STRING_DEVICEERROR_RANGEPHASECHECK);
break;
case VL53L0X_DEVICEERROR_SIGMATHRESHOLDCHECK:
VL53L0X_COPYSTRING(pDeviceErrorString,
VL53L0X_STRING_DEVICEERROR_SIGMATHRESHOLDCHECK);
break;
case VL53L0X_DEVICEERROR_TCC:
VL53L0X_COPYSTRING(pDeviceErrorString,
VL53L0X_STRING_DEVICEERROR_TCC);
break;
case VL53L0X_DEVICEERROR_PHASECONSISTENCY:
VL53L0X_COPYSTRING(pDeviceErrorString,
VL53L0X_STRING_DEVICEERROR_PHASECONSISTENCY);
break;
case VL53L0X_DEVICEERROR_MINCLIP:
VL53L0X_COPYSTRING(pDeviceErrorString,
VL53L0X_STRING_DEVICEERROR_MINCLIP);
break;
case VL53L0X_DEVICEERROR_RANGECOMPLETE:
VL53L0X_COPYSTRING(pDeviceErrorString,
VL53L0X_STRING_DEVICEERROR_RANGECOMPLETE);
break;
case VL53L0X_DEVICEERROR_ALGOUNDERFLOW:
VL53L0X_COPYSTRING(pDeviceErrorString,
VL53L0X_STRING_DEVICEERROR_ALGOUNDERFLOW);
break;
case VL53L0X_DEVICEERROR_ALGOOVERFLOW:
VL53L0X_COPYSTRING(pDeviceErrorString,
VL53L0X_STRING_DEVICEERROR_ALGOOVERFLOW);
break;
case VL53L0X_DEVICEERROR_RANGEIGNORETHRESHOLD:
VL53L0X_COPYSTRING(pDeviceErrorString,
VL53L0X_STRING_DEVICEERROR_RANGEIGNORETHRESHOLD);
break;
default:
VL53L0X_COPYSTRING(pDeviceErrorString,
VL53L0X_STRING_UNKNOW_ERROR_CODE);
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X_get_range_status_string(uint8_t RangeStatus,
char *pRangeStatusString)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
LOG_FUNCTION_START("");
switch (RangeStatus) {
case 0:
VL53L0X_COPYSTRING(pRangeStatusString,
VL53L0X_STRING_RANGESTATUS_RANGEVALID);
break;
case 1:
VL53L0X_COPYSTRING(pRangeStatusString,
VL53L0X_STRING_RANGESTATUS_SIGMA);
break;
case 2:
VL53L0X_COPYSTRING(pRangeStatusString,
VL53L0X_STRING_RANGESTATUS_SIGNAL);
break;
case 3:
VL53L0X_COPYSTRING(pRangeStatusString,
VL53L0X_STRING_RANGESTATUS_MINRANGE);
break;
case 4:
VL53L0X_COPYSTRING(pRangeStatusString,
VL53L0X_STRING_RANGESTATUS_PHASE);
break;
case 5:
VL53L0X_COPYSTRING(pRangeStatusString,
VL53L0X_STRING_RANGESTATUS_HW);
break;
default: /**/
VL53L0X_COPYSTRING(pRangeStatusString,
VL53L0X_STRING_RANGESTATUS_NONE);
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X_get_pal_error_string(VL53L0X_Error PalErrorCode,
char *pPalErrorString)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
LOG_FUNCTION_START("");
switch (PalErrorCode) {
case VL53L0X_ERROR_NONE:
VL53L0X_COPYSTRING(pPalErrorString,
VL53L0X_STRING_ERROR_NONE);
break;
case VL53L0X_ERROR_CALIBRATION_WARNING:
VL53L0X_COPYSTRING(pPalErrorString,
VL53L0X_STRING_ERROR_CALIBRATION_WARNING);
break;
case VL53L0X_ERROR_MIN_CLIPPED:
VL53L0X_COPYSTRING(pPalErrorString,
VL53L0X_STRING_ERROR_MIN_CLIPPED);
break;
case VL53L0X_ERROR_UNDEFINED:
VL53L0X_COPYSTRING(pPalErrorString,
VL53L0X_STRING_ERROR_UNDEFINED);
break;
case VL53L0X_ERROR_INVALID_PARAMS:
VL53L0X_COPYSTRING(pPalErrorString,
VL53L0X_STRING_ERROR_INVALID_PARAMS);
break;
case VL53L0X_ERROR_NOT_SUPPORTED:
VL53L0X_COPYSTRING(pPalErrorString,
VL53L0X_STRING_ERROR_NOT_SUPPORTED);
break;
case VL53L0X_ERROR_INTERRUPT_NOT_CLEARED:
VL53L0X_COPYSTRING(pPalErrorString,
VL53L0X_STRING_ERROR_INTERRUPT_NOT_CLEARED);
break;
case VL53L0X_ERROR_RANGE_ERROR:
VL53L0X_COPYSTRING(pPalErrorString,
VL53L0X_STRING_ERROR_RANGE_ERROR);
break;
case VL53L0X_ERROR_TIME_OUT:
VL53L0X_COPYSTRING(pPalErrorString,
VL53L0X_STRING_ERROR_TIME_OUT);
break;
case VL53L0X_ERROR_MODE_NOT_SUPPORTED:
VL53L0X_COPYSTRING(pPalErrorString,
VL53L0X_STRING_ERROR_MODE_NOT_SUPPORTED);
break;
case VL53L0X_ERROR_BUFFER_TOO_SMALL:
VL53L0X_COPYSTRING(pPalErrorString,
VL53L0X_STRING_ERROR_BUFFER_TOO_SMALL);
break;
case VL53L0X_ERROR_GPIO_NOT_EXISTING:
VL53L0X_COPYSTRING(pPalErrorString,
VL53L0X_STRING_ERROR_GPIO_NOT_EXISTING);
break;
case VL53L0X_ERROR_GPIO_FUNCTIONALITY_NOT_SUPPORTED:
VL53L0X_COPYSTRING(pPalErrorString,
VL53L0X_STRING_ERROR_GPIO_FUNCTIONALITY_NOT_SUPPORTED);
break;
case VL53L0X_ERROR_CONTROL_INTERFACE:
VL53L0X_COPYSTRING(pPalErrorString,
VL53L0X_STRING_ERROR_CONTROL_INTERFACE);
break;
case VL53L0X_ERROR_INVALID_COMMAND:
VL53L0X_COPYSTRING(pPalErrorString,
VL53L0X_STRING_ERROR_INVALID_COMMAND);
break;
case VL53L0X_ERROR_DIVISION_BY_ZERO:
VL53L0X_COPYSTRING(pPalErrorString,
VL53L0X_STRING_ERROR_DIVISION_BY_ZERO);
break;
case VL53L0X_ERROR_REF_SPAD_INIT:
VL53L0X_COPYSTRING(pPalErrorString,
VL53L0X_STRING_ERROR_REF_SPAD_INIT);
break;
case VL53L0X_ERROR_NOT_IMPLEMENTED:
VL53L0X_COPYSTRING(pPalErrorString,
VL53L0X_STRING_ERROR_NOT_IMPLEMENTED);
break;
default:
VL53L0X_COPYSTRING(pPalErrorString,
VL53L0X_STRING_UNKNOW_ERROR_CODE);
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X_get_pal_state_string(VL53L0X_State PalStateCode,
char *pPalStateString)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
LOG_FUNCTION_START("");
switch (PalStateCode) {
case VL53L0X_STATE_POWERDOWN:
VL53L0X_COPYSTRING(pPalStateString,
VL53L0X_STRING_STATE_POWERDOWN);
break;
case VL53L0X_STATE_WAIT_STATICINIT:
VL53L0X_COPYSTRING(pPalStateString,
VL53L0X_STRING_STATE_WAIT_STATICINIT);
break;
case VL53L0X_STATE_STANDBY:
VL53L0X_COPYSTRING(pPalStateString,
VL53L0X_STRING_STATE_STANDBY);
break;
case VL53L0X_STATE_IDLE:
VL53L0X_COPYSTRING(pPalStateString,
VL53L0X_STRING_STATE_IDLE);
break;
case VL53L0X_STATE_RUNNING:
VL53L0X_COPYSTRING(pPalStateString,
VL53L0X_STRING_STATE_RUNNING);
break;
case VL53L0X_STATE_UNKNOWN:
VL53L0X_COPYSTRING(pPalStateString,
VL53L0X_STRING_STATE_UNKNOWN);
break;
case VL53L0X_STATE_ERROR:
VL53L0X_COPYSTRING(pPalStateString,
VL53L0X_STRING_STATE_ERROR);
break;
default:
VL53L0X_COPYSTRING(pPalStateString,
VL53L0X_STRING_STATE_UNKNOWN);
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X_get_sequence_steps_info(
VL53L0X_SequenceStepId SequenceStepId,
char *pSequenceStepsString)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
LOG_FUNCTION_START("");
switch (SequenceStepId) {
case VL53L0X_SEQUENCESTEP_TCC:
VL53L0X_COPYSTRING(pSequenceStepsString,
VL53L0X_STRING_SEQUENCESTEP_TCC);
break;
case VL53L0X_SEQUENCESTEP_DSS:
VL53L0X_COPYSTRING(pSequenceStepsString,
VL53L0X_STRING_SEQUENCESTEP_DSS);
break;
case VL53L0X_SEQUENCESTEP_MSRC:
VL53L0X_COPYSTRING(pSequenceStepsString,
VL53L0X_STRING_SEQUENCESTEP_MSRC);
break;
case VL53L0X_SEQUENCESTEP_PRE_RANGE:
VL53L0X_COPYSTRING(pSequenceStepsString,
VL53L0X_STRING_SEQUENCESTEP_PRE_RANGE);
break;
case VL53L0X_SEQUENCESTEP_FINAL_RANGE:
VL53L0X_COPYSTRING(pSequenceStepsString,
VL53L0X_STRING_SEQUENCESTEP_FINAL_RANGE);
break;
default:
Status = VL53L0X_ERROR_INVALID_PARAMS;
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X_get_limit_check_info(VL53L0X_DEV Dev,
uint16_t LimitCheckId,
char *pLimitCheckString)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
LOG_FUNCTION_START("");
switch (LimitCheckId) {
case VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE:
VL53L0X_COPYSTRING(pLimitCheckString,
VL53L0X_STRING_CHECKENABLE_SIGMA_FINAL_RANGE);
break;
case VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE:
VL53L0X_COPYSTRING(pLimitCheckString,
VL53L0X_STRING_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE);
break;
case VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP:
VL53L0X_COPYSTRING(pLimitCheckString,
VL53L0X_STRING_CHECKENABLE_SIGNAL_REF_CLIP);
break;
case VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD:
VL53L0X_COPYSTRING(pLimitCheckString,
VL53L0X_STRING_CHECKENABLE_RANGE_IGNORE_THRESHOLD);
break;
case VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC:
VL53L0X_COPYSTRING(pLimitCheckString,
VL53L0X_STRING_CHECKENABLE_SIGNAL_RATE_MSRC);
break;
case VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE:
VL53L0X_COPYSTRING(pLimitCheckString,
VL53L0X_STRING_CHECKENABLE_SIGNAL_RATE_PRE_RANGE);
break;
default:
VL53L0X_COPYSTRING(pLimitCheckString,
VL53L0X_STRING_UNKNOW_ERROR_CODE);
}
LOG_FUNCTION_END(Status);
return Status;
}

55
App/VL53L0X_API/platform/README.md Normal file
View File

@@ -0,0 +1,55 @@
# VL53L0X 多传感器模板（STM32H743 + FreeRTOS）
这组模板按固定顺序组织 4 颗 VL53L0X：
1. 左上
2. 左下
3. 右上
4. 右下
## 文件说明
- `vl53l0x_platform.h/.c`
- ST 官方 API 的平台适配层
- 不再把 `hi2c1` 硬编码到驱动里
- 读写失败时不会污染 `RdWord/RdDWord` 输出
- `PollingDelay()` 在调度器已启动时使用 `vTaskDelay()`
- `vl53_board.h/.c`
- 4 颗 VL53L0X 的板级管理层
- 负责 `XSHUT` 拉低/逐个拉起/逐个改地址
- 默认运行地址：0x54 / 0x56 / 0x58 / 0x5A（8-bit 写法）
- `vl53_task.h/.c`
- FreeRTOS 任务骨架
- 10ms 周期轮询一遍 4 颗传感器
- 采集结果写成一个快照，避免上层散读散写
## 你需要改的地方
只要改 `vl53_board.c` 里的 `g_vl53_hw_cfg[]`：
- `hi2c`
- `xshut_port`
- `xshut_pin`
- `gpio1_port` / `gpio1_pin`（如果暂时不用中断，可保持 `NULL/0`）
## 集成方式
1. 保留 ST 官方 API 的 `vl53l0x_api*.c`、`vl53l0x_def.h` 等核心文件不动。
2. 用本模板替换你原来的 `vl53l0x_platform.h/.c`。
3. 新增 `vl53_board.*` 和 `vl53_task.*` 到 `App/sensors/` 或你自己的目录。
4. 在 CubeMX 里保留任务壳函数，在任务壳里调用 `AppVl53_TaskLoop()`。
5. 不要把 VL53 逻辑塞进你 20ms 的 `canTxTask`。
## 建议任务频率
- `canTxTask`: 保持 20ms
- `vl53Task`: 10ms 唤醒，内部读 ready 后取数
- 估计/控制：50–100Hz
## 备注
- 地址写法用的是 **8-bit**，默认地址是 `0x52`。
- 多颗同总线时，必须所有器件先 `XSHUT=RESET`，然后逐个拉起并改地址。
- 这版模板默认走“单任务采集 + 快照共享”，适合你当前的 ASER 架构。

166
App/VL53L0X_API/platform/vl53_board.c Normal file
View File

@@ -0,0 +1,166 @@
#include "vl53_board.h"
#include "FreeRTOS.h"
#include "task.h"
#include "robot_params.h"
/* ================= 卡尔曼滤波底层实现 ================= */
static void vl53_kalman_init(Vl53Kalman_t *kf, float q, float r) {
kf->x = 0.0f;
kf->p = 1.0f;
kf->q = q;
kf->r = r;
kf->initialized = 0;
}
static float vl53_kalman_update(Vl53Kalman_t *kf, float measurement) {
if (kf->initialized == 0) {
kf->x = measurement;
kf->initialized = 1;
return kf->x;
}
kf->p = kf->p + kf->q;
float k = kf->p / (kf->p + kf->r);
kf->x = kf->x + k * (measurement - kf->x);
kf->p = (1.0f - k) * kf->p;
return kf->x;
}
/* ================= ST 官方配置序列 ================= */
static VL53L0X_Error vl53_do_static_init(VL53L0X_DEV dev, uint32_t timing_budget_us)
{
VL53L0X_Error status;
uint32_t ref_spad_count = 0u;
uint8_t is_aperture_spads = 0u;
uint8_t vhv_settings = 0u;
uint8_t phase_cal = 0u;
status = VL53L0X_StaticInit(dev);
if (status != VL53L0X_ERROR_NONE) return status;
status = VL53L0X_PerformRefSpadManagement(dev, &ref_spad_count, &is_aperture_spads);
if (status != VL53L0X_ERROR_NONE) return status;
status = VL53L0X_PerformRefCalibration(dev, &vhv_settings, &phase_cal);
if (status != VL53L0X_ERROR_NONE) return status;
status = VL53L0X_SetDeviceMode(dev, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING);
if (status != VL53L0X_ERROR_NONE) return status;
status = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(dev, timing_budget_us);
if (status != VL53L0X_ERROR_NONE) return status;
status = VL53L0X_SetLimitCheckEnable(dev, VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 1u);
if (status != VL53L0X_ERROR_NONE) return status;
status = VL53L0X_SetLimitCheckEnable(dev, VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, 1u);
if (status != VL53L0X_ERROR_NONE) return status;
return VL53L0X_ERROR_NONE;
}
/* ================= 核心管理器 API ================= */
VL53L0X_Error Vl53Board_Init(Vl53Board_t *board, const Vl53BoardHwCfg_t *hw_cfgs, uint8_t count, uint32_t timing_budget_us)
{
if ((board == NULL) || (hw_cfgs == NULL) || (count == 0) || (count > VL53_MAX_DEVS_PER_BOARD)) {
return VL53L0X_ERROR_INVALID_PARAMS;
}
memset(board, 0, sizeof(Vl53Board_t));
board->dev_count = count;
board->timing_budget_us = (timing_budget_us == 0u) ? 33000u : timing_budget_us;
for (uint8_t i = 0; i < count; i++) {
board->dev[i].name = hw_cfgs[i].name;
board->dev[i].id = hw_cfgs[i].id;
board->dev[i].comms_type = 1u;
board->dev[i].comms_speed_khz = 400u;
board->dev[i].is_present = 0u;
VL53L0X_PlatformAttachBus(&board->dev[i], hw_cfgs[i].hi2c, VL53L0X_DEFAULT_ADDR_8BIT, 100u, NULL);
VL53L0X_PlatformAttachPins(&board->dev[i], hw_cfgs[i].xshut_port, hw_cfgs[i].xshut_pin, NULL, 0);
VL53L0X_PlatformSetXShut(&board->dev[i], GPIO_PIN_RESET);
/* 初始化卡尔曼滤波器：Q/R 从 robot_params.h 读取 */
vl53_kalman_init(&board->kf[i], PARAM_VL53_KALMAN_Q, PARAM_VL53_KALMAN_R);
}
vTaskDelay(pdMS_TO_TICKS(10));
for (uint8_t i = 0; i < count; i++) {
if (VL53L0X_PlatformSetXShut(&board->dev[i], GPIO_PIN_SET) != VL53L0X_ERROR_NONE) continue;
vTaskDelay(pdMS_TO_TICKS(20));
board->dev[i].I2cDevAddr = VL53L0X_DEFAULT_ADDR_8BIT;
if (VL53L0X_PlatformChangeAddress(&board->dev[i], hw_cfgs[i].runtime_addr_8bit) != VL53L0X_ERROR_NONE) continue;
if (VL53L0X_DataInit(&board->dev[i]) != VL53L0X_ERROR_NONE) continue;
if (vl53_do_static_init(&board->dev[i], board->timing_budget_us) != VL53L0X_ERROR_NONE) continue;
board->init_mask |= (uint8_t)(1u << i);
board->dev[i].is_present = 1u;
}
return VL53L0X_ERROR_NONE;
}
VL53L0X_Error Vl53Board_StartContinuous(Vl53Board_t *board)
{
if (board == NULL) return VL53L0X_ERROR_INVALID_PARAMS;
for (uint8_t i = 0; i < board->dev_count; i++) {
if (board->init_mask & (1u << i)) VL53L0X_StartMeasurement(&board->dev[i]);
}
return VL53L0X_ERROR_NONE;
}
VL53L0X_Error Vl53Board_StopContinuous(Vl53Board_t *board)
{
if (board == NULL) return VL53L0X_ERROR_INVALID_PARAMS;
for (uint8_t i = 0; i < board->dev_count; i++) {
if (board->init_mask & (1u << i)) VL53L0X_StopMeasurement(&board->dev[i]);
}
return VL53L0X_ERROR_NONE;
}
VL53L0X_Error Vl53Board_ReadAll(Vl53Board_t *board, Vl53BoardSnapshot_t *snapshot)
{
if (board == NULL || snapshot == NULL) return VL53L0X_ERROR_INVALID_PARAMS;
memset(snapshot, 0, sizeof(Vl53BoardSnapshot_t));
snapshot->tick_ms = xTaskGetTickCount() * portTICK_PERIOD_MS;
for (uint8_t i = 0; i < board->dev_count; i++) {
if ((board->init_mask & (1u << i)) == 0u) {
snapshot->range_status[i] = 255u;
continue;
}
uint8_t ready = 0u;
if (VL53L0X_GetMeasurementDataReady(&board->dev[i], &ready) != VL53L0X_ERROR_NONE) continue;
if (ready) {
VL53L0X_RangingMeasurementData_t data;
memset(&data, 0, sizeof(data));
if (VL53L0X_GetRangingMeasurementData(&board->dev[i], &data) == VL53L0X_ERROR_NONE) {
/* 1. 写入原始数据 */
snapshot->range_mm[i] = data.RangeMilliMeter;
snapshot->range_status[i] = data.RangeStatus;
if (data.RangeStatus == 0u) {
/* 2. 标记有效并更新卡尔曼滤波器 */
snapshot->valid_mask |= (1u << i);
snapshot->range_mm_filtered[i] = vl53_kalman_update(&board->kf[i], (float)data.RangeMilliMeter);
} else {
/* 测距失败时，滤波值维持上一次的历史最佳估计不变 */
snapshot->range_mm_filtered[i] = board->kf[i].x;
}
VL53L0X_ClearInterruptMask(&board->dev[i], 0u);
}
} else {
/* 如果没准备好，把上一帧的历史值顺延下来，防止读到 0 */
snapshot->range_mm_filtered[i] = board->kf[i].x;
}
}
return VL53L0X_ERROR_NONE;
}

59
App/VL53L0X_API/platform/vl53_board.h Normal file
View File

@@ -0,0 +1,59 @@
#ifndef VL53_BOARD_H
#define VL53_BOARD_H
#include <stdint.h>
#include <string.h>
#include "vl53l0x_api.h"
#include "vl53l0x_platform.h"
#ifdef __cplusplus
extern "C" {
#endif
#define VL53_MAX_DEVS_PER_BOARD 4
typedef struct {
I2C_HandleTypeDef *hi2c;
GPIO_TypeDef *xshut_port;
uint16_t xshut_pin;
uint8_t runtime_addr_8bit;
const char *name;
uint8_t id;
} Vl53BoardHwCfg_t;
/* ================= 内部卡尔曼滤波器结构 ================= */
typedef struct {
float x; // 最优估计值 (滤波后的距离)
float p; // 估计误差协方差
float q; // 过程噪声
float r; // 测量噪声
uint8_t initialized; // 防止第一次开机从0缓慢爬升
} Vl53Kalman_t;
/* ================= 快照数据结构 (对外接口) ================= */
typedef struct {
uint32_t tick_ms;
uint16_t range_mm[VL53_MAX_DEVS_PER_BOARD]; /* 接口1：原始硬件数据 */
float range_mm_filtered[VL53_MAX_DEVS_PER_BOARD]; /* 接口2：卡尔曼平滑数据 */
uint8_t range_status[VL53_MAX_DEVS_PER_BOARD];
uint8_t valid_mask;
} Vl53BoardSnapshot_t;
typedef struct {
VL53L0X_Dev_t dev[VL53_MAX_DEVS_PER_BOARD];
Vl53Kalman_t kf[VL53_MAX_DEVS_PER_BOARD]; /* 每个传感器配备一个专属卡尔曼滤波器 */
uint8_t init_mask;
uint8_t dev_count;
uint32_t timing_budget_us;
} Vl53Board_t;
VL53L0X_Error Vl53Board_Init(Vl53Board_t *board, const Vl53BoardHwCfg_t *hw_cfgs, uint8_t count, uint32_t timing_budget_us);
VL53L0X_Error Vl53Board_StartContinuous(Vl53Board_t *board);
VL53L0X_Error Vl53Board_StopContinuous(Vl53Board_t *board);
VL53L0X_Error Vl53Board_ReadAll(Vl53Board_t *board, Vl53BoardSnapshot_t *snapshot);
#ifdef __cplusplus
}
#endif
#endif /* VL53_BOARD_H */

315
App/VL53L0X_API/platform/vl53l0x_platform.c Normal file
View File

@@ -0,0 +1,315 @@
#include "vl53l0x_platform.h"
#include "vl53l0x_api.h"
#if VL53_USE_FREERTOS_DELAY
#include "FreeRTOS.h"
#include "task.h"
#endif
#if defined(configSUPPORT_STATIC_ALLOCATION) || defined(configUSE_MUTEXES)
#include "semphr.h"
#define VL53_USE_FREERTOS_MUTEX 1
#else
#define VL53_USE_FREERTOS_MUTEX 0
#endif
static VL53L0X_Error vl53_hal_to_pal(HAL_StatusTypeDef hal_status)
{
switch (hal_status) {
case HAL_OK:
return VL53L0X_ERROR_NONE;
case HAL_TIMEOUT:
return VL53L0X_ERROR_TIME_OUT;
default:
return VL53L0X_ERROR_CONTROL_INTERFACE;
}
}
static VL53L0X_Error vl53_validate_dev(VL53L0X_DEV Dev)
{
if ((Dev == NULL) || (Dev->hi2c == NULL)) {
return VL53L0X_ERROR_CONTROL_INTERFACE;
}
return VL53L0X_ERROR_NONE;
}
static VL53L0X_Error vl53_bus_lock(VL53L0X_DEV Dev)
{
#if VL53_USE_FREERTOS_MUTEX
if ((Dev != NULL) && (Dev->bus_lock != NULL)) {
if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED) {
const TickType_t wait_ticks = pdMS_TO_TICKS((Dev->io_timeout_ms == 0u) ? 10u : Dev->io_timeout_ms);
if (xSemaphoreTake((SemaphoreHandle_t)Dev->bus_lock, wait_ticks) != pdTRUE) {
return VL53L0X_ERROR_TIME_OUT;
}
}
}
#else
(void)Dev;
#endif
return VL53L0X_ERROR_NONE;
}
static void vl53_bus_unlock(VL53L0X_DEV Dev)
{
#if VL53_USE_FREERTOS_MUTEX
if ((Dev != NULL) && (Dev->bus_lock != NULL)) {
if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED) {
(void)xSemaphoreGive((SemaphoreHandle_t)Dev->bus_lock);
}
}
#else
(void)Dev;
#endif
}
VL53L0X_Error VL53L0X_LockSequenceAccess(VL53L0X_DEV Dev)
{
(void)Dev;
/* Shared I2C bus is already serialized per I/O transaction below. */
return VL53L0X_ERROR_NONE;
}
VL53L0X_Error VL53L0X_UnlockSequenceAccess(VL53L0X_DEV Dev)
{
(void)Dev;
return VL53L0X_ERROR_NONE;
}
void VL53L0X_PlatformAttachBus(VL53L0X_DEV Dev,
I2C_HandleTypeDef *hi2c,
uint8_t i2c_addr_8bit,
uint16_t io_timeout_ms,
void *bus_lock)
{
if (Dev == NULL) {
return;
}
Dev->hi2c = hi2c;
Dev->I2cDevAddr = i2c_addr_8bit;
Dev->io_timeout_ms = (io_timeout_ms == 0u) ? 100u : io_timeout_ms;
Dev->bus_lock = bus_lock;
}
void VL53L0X_PlatformAttachPins(VL53L0X_DEV Dev,
GPIO_TypeDef *xshut_port,
uint16_t xshut_pin,
GPIO_TypeDef *gpio1_port,
uint16_t gpio1_pin)
{
if (Dev == NULL) {
return;
}
Dev->xshut_port = xshut_port;
Dev->xshut_pin = xshut_pin;
Dev->gpio1_port = gpio1_port;
Dev->gpio1_pin = gpio1_pin;
}
VL53L0X_Error VL53L0X_PlatformSetXShut(VL53L0X_DEV Dev, GPIO_PinState state)
{
if ((Dev == NULL) || (Dev->xshut_port == NULL)) {
return VL53L0X_ERROR_INVALID_PARAMS;
}
HAL_GPIO_WritePin(Dev->xshut_port, Dev->xshut_pin, state);
return VL53L0X_ERROR_NONE;
}
VL53L0X_Error VL53L0X_PlatformBootDevice(VL53L0X_DEV Dev, uint32_t reset_low_ms, uint32_t boot_wait_ms)
{
VL53L0X_Error status = VL53L0X_PlatformSetXShut(Dev, GPIO_PIN_RESET);
if (status != VL53L0X_ERROR_NONE) {
return status;
}
if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED) {
vTaskDelay(pdMS_TO_TICKS(reset_low_ms));
} else {
HAL_Delay(reset_low_ms);
}
status = VL53L0X_PlatformSetXShut(Dev, GPIO_PIN_SET);
if (status != VL53L0X_ERROR_NONE) {
return status;
}
if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED) {
vTaskDelay(pdMS_TO_TICKS(boot_wait_ms));
} else {
HAL_Delay(boot_wait_ms);
}
return VL53L0X_ERROR_NONE;
}
uint32_t VL53L0X_PlatformGetTick(void)
{
return HAL_GetTick();
}
VL53L0X_Error VL53L0X_WriteMulti(VL53L0X_DEV Dev, uint8_t index, uint8_t *pdata, uint32_t count)
{
HAL_StatusTypeDef hal_status;
VL53L0X_Error status;
if ((pdata == NULL) || (count == 0u) || (count > 255u)) {
return VL53L0X_ERROR_INVALID_PARAMS;
}
status = vl53_validate_dev(Dev);
if (status != VL53L0X_ERROR_NONE) {
return status;
}
status = vl53_bus_lock(Dev);
if (status != VL53L0X_ERROR_NONE) {
return status;
}
hal_status = HAL_I2C_Mem_Write(Dev->hi2c,
Dev->I2cDevAddr,
index,
I2C_MEMADD_SIZE_8BIT,
pdata,
(uint16_t)count,
Dev->io_timeout_ms);
vl53_bus_unlock(Dev);
return vl53_hal_to_pal(hal_status);
}
VL53L0X_Error VL53L0X_ReadMulti(VL53L0X_DEV Dev, uint8_t index, uint8_t *pdata, uint32_t count)
{
HAL_StatusTypeDef hal_status;
VL53L0X_Error status;
if ((pdata == NULL) || (count == 0u) || (count > 255u)) {
return VL53L0X_ERROR_INVALID_PARAMS;
}
status = vl53_validate_dev(Dev);
if (status != VL53L0X_ERROR_NONE) {
return status;
}
status = vl53_bus_lock(Dev);
if (status != VL53L0X_ERROR_NONE) {
return status;
}
hal_status = HAL_I2C_Mem_Read(Dev->hi2c,
Dev->I2cDevAddr,
index,
I2C_MEMADD_SIZE_8BIT,
pdata,
(uint16_t)count,
Dev->io_timeout_ms);
vl53_bus_unlock(Dev);
return vl53_hal_to_pal(hal_status);
}
VL53L0X_Error VL53L0X_WrByte(VL53L0X_DEV Dev, uint8_t index, uint8_t data)
{
return VL53L0X_WriteMulti(Dev, index, &data, 1u);
}
VL53L0X_Error VL53L0X_WrWord(VL53L0X_DEV Dev, uint8_t index, uint16_t data)
{
uint8_t buffer[2];
buffer[0] = (uint8_t)(data >> 8);
buffer[1] = (uint8_t)(data & 0xFFu);
return VL53L0X_WriteMulti(Dev, index, buffer, 2u);
}
VL53L0X_Error VL53L0X_WrDWord(VL53L0X_DEV Dev, uint8_t index, uint32_t data)
{
uint8_t buffer[4];
buffer[0] = (uint8_t)(data >> 24);
buffer[1] = (uint8_t)(data >> 16);
buffer[2] = (uint8_t)(data >> 8);
buffer[3] = (uint8_t)(data & 0xFFu);
return VL53L0X_WriteMulti(Dev, index, buffer, 4u);
}
VL53L0X_Error VL53L0X_RdByte(VL53L0X_DEV Dev, uint8_t index, uint8_t *data)
{
if (data == NULL) {
return VL53L0X_ERROR_INVALID_PARAMS;
}
return VL53L0X_ReadMulti(Dev, index, data, 1u);
}
VL53L0X_Error VL53L0X_RdWord(VL53L0X_DEV Dev, uint8_t index, uint16_t *data)
{
uint8_t buffer[2] = {0u, 0u};
VL53L0X_Error status;
if (data == NULL) {
return VL53L0X_ERROR_INVALID_PARAMS;
}
status = VL53L0X_ReadMulti(Dev, index, buffer, 2u);
if (status == VL53L0X_ERROR_NONE) {
*data = (((uint16_t)buffer[0]) << 8) | buffer[1];
}
return status;
}
VL53L0X_Error VL53L0X_RdDWord(VL53L0X_DEV Dev, uint8_t index, uint32_t *data)
{
uint8_t buffer[4] = {0u, 0u, 0u, 0u};
VL53L0X_Error status;
if (data == NULL) {
return VL53L0X_ERROR_INVALID_PARAMS;
}
status = VL53L0X_ReadMulti(Dev, index, buffer, 4u);
if (status == VL53L0X_ERROR_NONE) {
*data = (((uint32_t)buffer[0]) << 24)
| (((uint32_t)buffer[1]) << 16)
| (((uint32_t)buffer[2]) << 8)
| ((uint32_t)buffer[3]);
}
return status;
}
VL53L0X_Error VL53L0X_UpdateByte(VL53L0X_DEV Dev, uint8_t index, uint8_t AndData, uint8_t OrData)
{
VL53L0X_Error status;
uint8_t data = 0u;
status = VL53L0X_RdByte(Dev, index, &data);
if (status != VL53L0X_ERROR_NONE) {
return status;
}
data = (uint8_t)((data & AndData) | OrData);
return VL53L0X_WrByte(Dev, index, data);
}
VL53L0X_Error VL53L0X_PollingDelay(VL53L0X_DEV Dev)
{
(void)Dev;
#if VL53_USE_FREERTOS_DELAY
if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED) {
vTaskDelay(pdMS_TO_TICKS(2u));
} else {
HAL_Delay(2u);
}
#else
HAL_Delay(2u);
#endif
return VL53L0X_ERROR_NONE;
}
VL53L0X_Error VL53L0X_PlatformChangeAddress(VL53L0X_DEV Dev, uint8_t new_addr_8bit)
{
VL53L0X_Error status;
if (new_addr_8bit == 0u) {
return VL53L0X_ERROR_INVALID_PARAMS;
}
status = VL53L0X_SetDeviceAddress(Dev, new_addr_8bit);
if (status == VL53L0X_ERROR_NONE) {
Dev->I2cDevAddr = new_addr_8bit;
if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED) {
vTaskDelay(pdMS_TO_TICKS(2u));
} else {
HAL_Delay(2u);
}
}
return status;
}

89
App/VL53L0X_API/platform/vl53l0x_platform.h Normal file
View File

@@ -0,0 +1,89 @@
#ifndef VL53L0X_PLATFORM_H
#define VL53L0X_PLATFORM_H
#include <stdint.h>
#include <string.h>
#include "main.h"
#ifndef VL53L0X_SINGLE_DEVICE_DRIVER
#define VL53L0X_SINGLE_DEVICE_DRIVER 0
#endif
#include "vl53l0x_def.h"
#ifdef __cplusplus
extern "C" {
#endif
#ifndef VL53_USE_FREERTOS_DELAY
#define VL53_USE_FREERTOS_DELAY 1
#endif
#ifndef VL53L0X_COPYSTRING
#define VL53L0X_COPYSTRING(dst, src) strcpy((dst), (src))
#endif
#ifndef VL53L0X_DEFAULT_ADDR_8BIT
#define VL53L0X_DEFAULT_ADDR_8BIT 0x52u
#endif
typedef struct {
VL53L0X_DevData_t Data;
uint8_t I2cDevAddr; /* ST API expects 8-bit address, default 0x52 */
uint8_t comms_type;
uint16_t comms_speed_khz;
I2C_HandleTypeDef *hi2c;
GPIO_TypeDef *xshut_port;
uint16_t xshut_pin;
GPIO_TypeDef *gpio1_port; /* optional */
uint16_t gpio1_pin;
void *bus_lock; /* optional shared mutex/lock handle */
const char *name;
uint8_t id; /* user label: 1=左上, 2=左下, 3=右上, 4=右下 */
uint8_t is_present;
uint16_t io_timeout_ms;
} VL53L0X_Dev_t;
typedef VL53L0X_Dev_t* VL53L0X_DEV;
#define PALDevDataGet(Dev, field) ((Dev)->Data.field)
#define PALDevDataSet(Dev, field, data) ((Dev)->Data.field = (data))
VL53L0X_Error VL53L0X_LockSequenceAccess(VL53L0X_DEV Dev);
VL53L0X_Error VL53L0X_UnlockSequenceAccess(VL53L0X_DEV Dev);
VL53L0X_Error VL53L0X_WriteMulti(VL53L0X_DEV Dev, uint8_t index, uint8_t *pdata, uint32_t count);
VL53L0X_Error VL53L0X_ReadMulti(VL53L0X_DEV Dev, uint8_t index, uint8_t *pdata, uint32_t count);
VL53L0X_Error VL53L0X_WrByte(VL53L0X_DEV Dev, uint8_t index, uint8_t data);
VL53L0X_Error VL53L0X_WrWord(VL53L0X_DEV Dev, uint8_t index, uint16_t data);
VL53L0X_Error VL53L0X_WrDWord(VL53L0X_DEV Dev, uint8_t index, uint32_t data);
VL53L0X_Error VL53L0X_RdByte(VL53L0X_DEV Dev, uint8_t index, uint8_t *data);
VL53L0X_Error VL53L0X_RdWord(VL53L0X_DEV Dev, uint8_t index, uint16_t *data);
VL53L0X_Error VL53L0X_RdDWord(VL53L0X_DEV Dev, uint8_t index, uint32_t *data);
VL53L0X_Error VL53L0X_UpdateByte(VL53L0X_DEV Dev, uint8_t index, uint8_t AndData, uint8_t OrData);
VL53L0X_Error VL53L0X_PollingDelay(VL53L0X_DEV Dev);
void VL53L0X_PlatformAttachBus(VL53L0X_DEV Dev,
I2C_HandleTypeDef *hi2c,
uint8_t i2c_addr_8bit,
uint16_t io_timeout_ms,
void *bus_lock);
void VL53L0X_PlatformAttachPins(VL53L0X_DEV Dev,
GPIO_TypeDef *xshut_port,
uint16_t xshut_pin,
GPIO_TypeDef *gpio1_port,
uint16_t gpio1_pin);
VL53L0X_Error VL53L0X_PlatformSetXShut(VL53L0X_DEV Dev, GPIO_PinState state);
VL53L0X_Error VL53L0X_PlatformBootDevice(VL53L0X_DEV Dev, uint32_t reset_low_ms, uint32_t boot_wait_ms);
VL53L0X_Error VL53L0X_PlatformChangeAddress(VL53L0X_DEV Dev, uint8_t new_addr_8bit);
uint32_t VL53L0X_PlatformGetTick(void);
#ifdef __cplusplus
}
#endif
#endif /* VL53L0X_PLATFORM_H */

18
App/VL53L0X_API/platform/vl53l0x_platform_log.h Normal file
View File

@@ -0,0 +1,18 @@
#ifndef _VL53L0X_PLATFORM_LOG_H_
#define _VL53L0X_PLATFORM_LOG_H_
/* 1. 解决 TRACE_MODULE_API 未定义的报错 */
#define TRACE_MODULE_API 0
/* 2. 强行屏蔽官方所有【带下划线】的底层日志宏 */
#define _LOG_FUNCTION_START(...) (void)0
#define _LOG_FUNCTION_END(...) (void)0
#define _LOG_FUNCTION_END_FMT(...) (void)0
/* 3. 强行屏蔽官方所有【不带下划线】的普通日志宏 */
#define LOG_FUNCTION_START(...) (void)0
#define LOG_FUNCTION_END(...) (void)0
#define LOG_FUNCTION_END_FMT(...) (void)0
#define VL53L0X_ErrLog(...) (void)0
#endif /* _VL53L0X_PLATFORM_LOG_H_ */

15
App/VL53L0X_API/platform/vl53l0x_types.h Normal file
View File

@@ -0,0 +1,15 @@
#ifndef _VL53L0X_TYPES_H_
#define _VL53L0X_TYPES_H_
/* 引入现代 C 语言标准整数类型库 */
#include <stdint.h>
#include <stddef.h>
#ifndef NULL
#define NULL 0
#endif
/* 核心修复：填补 ST 官方定义的定点数类型 */
typedef uint32_t FixPoint1616_t;
#endif /* _VL53L0X_TYPES_H_ */

423
App/app_tasks.c Normal file
View File

@@ -0,0 +1,423 @@
#include "app_tasks.h"
#include "FreeRTOS.h"
#include "cmsis_os.h"
#include "main.h"
#include "snc_can_app.h"
#include "robot_params.h" /* 统一参数配置 */
#include <stdio.h>
#include <string.h>
/* 引入各个传感器和黑板的头文件 */
#include "Contract/robot_blackboard.h"
#include "Contract/robot_odom.h"
#include "laser/laser_manager.h"
#include "IMU/hwt101.h"
#include "vl53_board.h"
#include "Contract/robot_cmd_slot.h"
#include "preproc/corridor_preproc.h"
#include "est/corridor_filter.h"
#include "nav/corridor_ctrl.h"
#include "nav/segment_fsm.h"
#include "nav/nav_script.h"
extern osMutexId_t logMutexHandle;
/* 如果你的项目中没有引入 i2c.h，可以通过 extern 声明 I2C 句柄 */
extern I2C_HandleTypeDef hi2c1;
extern I2C_HandleTypeDef hi2c2;
static float g_test_vx = 0.0f;
static float g_test_wz = 0.0f;
static uint32_t AppTasks_MsToTicks(uint32_t ms)
{
uint32_t ticks = pdMS_TO_TICKS(ms);
return (ticks == 0U) ? 1U : ticks;
}
static void App_UpdateTestCommand(void)
{
g_test_vx = 0.0f;
g_test_wz = 0.0f;
}
static void App_PrintStatus(void)
{
// 【新增】静态计数器，由于是 static 修饰，它的值在函数退出后不会丢失
static uint8_t print_divider = 0;
// 假设外部任务是 100ms 调用一次该函数：
// 如果设定 >= 5，就是 5 * 100ms = 500ms 打印一次
// 如果设定 >= 10，就是 10 * 100ms = 1000ms (1秒) 打印一次
if (++print_divider < 5) {
return; // 还没到设定的次数，直接 return，不执行后续获取快照和打印的动作
}
print_divider = 0; // 计数器清零，重新开始下一轮计数
// 1. 获取全局黑板的快照
RobotBlackboard_t snap;
Blackboard_GetSnapshot(&snap);
// 2. 申请串口互斥锁，防止多线程打印打架导致乱码
if (logMutexHandle != NULL) {
(void)osMutexAcquire(logMutexHandle, osWaitForever);
}
// 3. 打印表头和当前系统时间
printf("\r\n========== Robot Status [%lu ms] ==========\r\n", HAL_GetTick());
// 4. 打印 IMU 姿态
if (snap.imu_yaw.is_valid) {
printf("[IMU] Yaw: %7.2f deg | Cont: %7.2f deg | Wz: %7.2f\r\n",
snap.imu_yaw.value, snap.imu_yaw_continuous.value, snap.imu_wz.value);
} else {
printf("[IMU] --- OFFLINE or INVALID ---\r\n");
}
// 5. 打印侧向阵列雷达 (强制转为 int，去掉多余的小数点)
printf("[VL53L0X] Left-F : %4d mm | Left-R : %4d mm\r\n",
(int)snap.dist_left_f.value, (int)snap.dist_left_r.value);
printf(" Right-F: %4d mm | Right-R: %4d mm\r\n",
(int)snap.dist_right_f.value, (int)snap.dist_right_r.value);
// 6. 打印前后激光测距雷达 (四个探头独立显示)
printf("[LASER] F-STP: %4d mm | F-ATK: %4d mm\r\n",
(int)snap.dist_front_stp.value, (int)snap.dist_front_atk.value);
printf(" R-STP: %4d mm | R-ATK: %4d mm\r\n",
(int)snap.dist_rear_stp.value, (int)snap.dist_rear_atk.value);
printf("==================================================\r\n");
// 7. 释放锁
if (logMutexHandle != NULL) {
(void)osMutexRelease(logMutexHandle);
}
}
void AppTasks_RunCanTxTask(void *argument)
{
(void)argument;
uint32_t wake_tick = osKernelGetTickCount();
const uint32_t period_ticks = AppTasks_MsToTicks(20U);
// 用于存放从指令槽里取出来的控制指令
RobotTargetCmd_t current_cmd;
for (;;) {
// 维持心跳发送
SNC_CAN_20msTask();
// 从指令槽中获取最新指令，设置 100ms 超时容忍度
if (CmdSlot_Pop(&current_cmd, 100)) {
// 如果算法层在 100ms 内有更新指令，正常下发给底盘
(void)SNC_CAN_SendCmdVel(current_cmd.target_vx, current_cmd.target_wz, current_cmd.ctrl_flags);
} else {
// 🚨 触发安全保护：如果超过 100ms 没收到上层新指令（可能算法死机或网络断开）
// 强制发送 0 速度，让底盘立刻停车，防止飞车撞墙
(void)SNC_CAN_SendCmdVel(0.0f, 0.0f, 0U);
}
wake_tick += period_ticks;
(void)osDelayUntil(wake_tick);
}
}
void AppTasks_RunMonitorTask(void *argument)
{
(void)argument;
uint32_t wake_tick = osKernelGetTickCount();
const uint32_t period_ticks = AppTasks_MsToTicks(100U);
for (;;) {
uint32_t now_ms = HAL_GetTick();
// 1. 执行 CAN 侧的周期性监控 (心跳/超时判断)
SNC_CAN_100msTask();
SNC_CAN_PollOnlineState(now_ms);
// 2. 获取 CAN 数据上下文
const SNC_CAN_AppContext_t *can_ctx = SNC_CAN_GetContext();
// 3. 将底盘的核心状态写入全局黑板
Blackboard_UpdateChassisState(can_ctx->status.state,
can_ctx->status.diag_bits,
can_ctx->status.online);
// 4. 里程计更新：从 CAN 0x200 累加器原子取走增量，积分并推送黑板
// 使用 SNC_CAN_ConsumeOdomDelta() 解决漏积分/重复积分问题：
// - ISR 侧每帧累加，不覆盖
// - 此处原子取走并清零，每份增量恰好消费一次
{
int16_t odom_fl, odom_rl, odom_fr, odom_rr;
uint32_t odom_span_ms;
uint8_t odom_frames = SNC_CAN_ConsumeOdomDelta(
&odom_fl, &odom_rl,
&odom_fr, &odom_rr,
&odom_span_ms);
if (odom_frames > 0U) {
/* 有新的增量帧需要积分 */
Odom_Update(now_ms,
odom_fl, odom_rl,
odom_fr, odom_rr,
odom_span_ms);
} else {
/* odom 断流保护：超过协议允许时间后主动清零黑板里的速度 */
Odom_HandleTimeout(now_ms, SNC_ODOM_TIMEOUT_MS);
}
}
/* 指令下发已由 navTask 接管，此处不再发送测试指令 */
// 调用纯净版播报员 (内部自带分频逻辑)
// App_PrintStatus();
wake_tick += period_ticks;
(void)osDelayUntil(wake_tick);
}
}
/* =========================================================
* 1. 前后激光雷达任务 (Laser Manager)
* ========================================================= */
void AppTasks_RunLaserTestTask_Impl(void *argument)
{
(void)argument;
// 初始化前后防撞雷达
LASER_SIMPLE_Init();
for (;;)
{
// 底层高频巡检
LASER_SIMPLE_Poll(HAL_GetTick());
// 获取无撕裂快照
const laser_simple_snapshot_t *snap = LASER_SIMPLE_GetSnapshot();
// 写入全局数据黑板
Blackboard_UpdateLaser(snap);
// 维持 50ms 巡检频率 (避障数据建议稍快一些)
osDelay(50);
}
}
/* =========================================================
* 2. IMU 姿态传感器任务 (HWT101)
* ========================================================= */
void AppTasks_RunImuTask_Impl(void *argument)
{
(void)argument;
// 初始化 IMU (开启 DMA 接收)，使用 UART7
HWT101_Init(&huart7);
osDelay(100);
HWT101_ZeroYaw();
for (;;) {
// 高频解析 DMA 数据
HWT101_Process();
// 安全拷贝 IMU 数据并写入黑板
HWT101_Data_t imu_data;
HWT101_GetData(&imu_data);
Blackboard_UpdateIMU(&imu_data);
osDelay(10); // 10ms 解析与更新周期
}
}
/* =========================================================
* 3. 侧向阵列雷达任务 (VL53L0X 双总线)
* ========================================================= */
void AppTasks_RunVl53Task_Impl(void *argument)
{
(void)argument;
static Vl53Board_t left_board;
static Vl53Board_t right_board;
/* --- 配置左侧雷达 (I2C2) --- */
Vl53BoardHwCfg_t left_cfgs[2] = {
{&hi2c2, GPIOB, GPIO_PIN_1, 0x62, "LF", 0}, // 左前 (PB1)
{&hi2c2, GPIOC, GPIO_PIN_5, 0x64, "LR", 1} // 左后 (PC5)
};
// 【修改 1】：测距预算从 robot_params.h 读取
Vl53Board_Init(&left_board, left_cfgs, 2, PARAM_VL53_TIMING_BUDGET);
Vl53Board_StartContinuous(&left_board);
/* --- 配置右侧雷达 (I2C1) --- */
Vl53BoardHwCfg_t right_cfgs[2] = {
{&hi2c1, GPIOD, GPIO_PIN_13, 0x56, "RF", 2}, // 右前 (PD13)
{&hi2c1, GPIOD, GPIO_PIN_14, 0x58, "RR", 3} // 右后 (PD14)
};
// 【修改 2】：同样从 robot_params.h 读取
Vl53Board_Init(&right_board, right_cfgs, 2, PARAM_VL53_TIMING_BUDGET);
Vl53Board_StartContinuous(&right_board);
Vl53BoardSnapshot_t left_snap, right_snap, combined_snap;
for(;;) {
// 读取左右两侧的雷达快照
Vl53Board_ReadAll(&left_board, &left_snap);
Vl53Board_ReadAll(&right_board, &right_snap);
// 合并到一个统一的 Snapshot 中送给黑板
memset(&combined_snap, 0, sizeof(combined_snap));
combined_snap.tick_ms = HAL_GetTick();
// 映射约定：[0]左前，[1]左后，[2]右前，[3]右后
for(int i = 0; i < 2; i++) {
// 左侧复制到 [0], [1]
combined_snap.range_mm[i] = left_snap.range_mm[i];
combined_snap.range_mm_filtered[i] = left_snap.range_mm_filtered[i];
combined_snap.range_status[i] = left_snap.range_status[i];
// 右侧偏移 2 个身位，复制到 [2], [3]
combined_snap.range_mm[i+2] = right_snap.range_mm[i];
combined_snap.range_mm_filtered[i+2] = right_snap.range_mm_filtered[i];
combined_snap.range_status[i+2] = right_snap.range_status[i];
}
// 合并掩码 (右侧掩码左移 2 位)
combined_snap.valid_mask = (left_snap.valid_mask & 0x03) | ((right_snap.valid_mask & 0x03) << 2);
// 统一推送到黑板
Blackboard_UpdateVl53(&combined_snap);
// 【修改 3】：休眠 100ms 匹配 VL53L0X 的新测距周期
osDelay(100);
}
}
/* =========================================================
* 4. 走廊导航控制任务 (Nav Pipeline)
* 周期：20ms (50Hz)，与 CAN 发送同频
* 流水线：Obs → Filter → Ctrl → Script → FSM → CmdSlot_Push
* ========================================================= */
void AppTasks_RunNavTask_Impl(void *argument)
{
(void)argument;
uint32_t wake_tick = osKernelGetTickCount();
const uint32_t period_ticks = AppTasks_MsToTicks(20U);
uint32_t last_ms = HAL_GetTick();
/* 等传感器全部就绪再启动脚本 (避免刚上电全是脏数据) */
osDelay(500);
NavScript_Start(); // 开始执行比赛脚本
for (;;) {
uint32_t now_ms = HAL_GetTick();
float dt_s = (float)(now_ms - last_ms) / 1000.0f;
if (dt_s <= 0.0f || dt_s > 0.5f) {
dt_s = 0.02f; /* 容错：防止首拍或溢出 */
}
last_ms = now_ms;
/* --- Step 1: 拍摄黑板快照 --- */
RobotBlackboard_t board;
Blackboard_GetSnapshot(&board);
/* --- Step 2: 预处理 → 清洗观测 --- */
CorridorObs_t obs;
CorridorPreproc_ExtractObs(&board, now_ms, &obs);
/* --- Step 3: 互补滤波 → 走廊状态估计 --- */
/* 注意: HWT101 输出 wz 单位是 °/s，EKF 需要 rad/s，必须转换！ */
float imu_wz_raw = board.imu_wz.is_valid ? board.imu_wz.value : 0.0f;
float imu_wz = PARAM_DEG2RAD(imu_wz_raw);
float odom_vx = board.odom_vx; /* 里程计已接入，由 monitorTask 调用 Odom_Update() 更新 */
/* IMU 连续 yaw → rad，作为 EKF 额外航向观测 */
float imu_yaw_cont_rad = board.imu_yaw_continuous.is_valid
? PARAM_DEG2RAD(board.imu_yaw_continuous.value) : 0.0f;
bool imu_yaw_ok = board.imu_yaw_continuous.is_valid;
CorridorState_t corridor_state;
CorridorFilter_Update(&obs, imu_wz, odom_vx, dt_s,
imu_yaw_cont_rad, imu_yaw_ok, &corridor_state);
/* --- Step 4: 段脚本执行器 → 决定当前阶段和目标 --- */
NavScriptOutput_t script_out;
float imu_yaw_cont_deg = board.imu_yaw_continuous.is_valid
? board.imu_yaw_continuous.value : 0.0f;
NavScript_Update(&obs, &corridor_state, imu_yaw_cont_deg, &script_out);
/* --- Step 5: 控制律 → 计算期望 v, w --- */
RawCmd_t raw_cmd;
if (script_out.use_override) {
/* 脚本覆盖模式：直接用脚本的输出（入口对准/原地转向/退出等） */
raw_cmd.t_ms = now_ms;
raw_cmd.v = script_out.override_v;
raw_cmd.w = script_out.override_w;
raw_cmd.flags = 0U;
} else if (script_out.request_corridor) {
/* 走廊跟踪模式：使用走廊控制器 */
CorridorCtrl_Compute(&corridor_state, &obs, imu_wz, &raw_cmd);
} else {
/* 默认停车 */
raw_cmd.t_ms = now_ms;
raw_cmd.v = 0.0f;
raw_cmd.w = 0.0f;
raw_cmd.flags = 0U;
}
/* --- Step 6: 段状态机 → 安全仲裁 --- */
SegFsmOutput_t fsm_out;
SegFsm_Update(&raw_cmd, &obs, &corridor_state, &fsm_out);
/* --- Step 7: 将安全后的指令喂给 CAN 发送层 --- */
CmdSlot_Push(fsm_out.safe_v, fsm_out.safe_w, 0U);
wake_tick += period_ticks;
(void)osDelayUntil(wake_tick);
}
}
void AppTasks_Init(void)
{
SNC_CAN_AppInit();
Odom_Init();
/* --- 初始化走廊滤波器 (EKF) --- */
CorridorFilterConfig_t filter_cfg = {
.sensor_base_length = PARAM_SENSOR_BASE_LENGTH, /* 实测：同侧前后雷达间距 */
.corridor_width = PARAM_CORRIDOR_WIDTH, /* 实测：走廊宽度 */
.y_offset = 0.0f, /* 0 = 绝对居中 */
.side_sensor_inset = PARAM_VL53_SIDE_INSET, /* 实测：侧向传感器内缩距离 */
.robot_width = PARAM_ROBOT_WIDTH, /* 实测：车体宽度 */
.alpha_theta = 0.98f, /* 保留兼容，EKF 内部使用 Q/R */
.alpha_y = 0.7f,
};
CorridorFilter_Init(&filter_cfg);
/* --- 初始化走廊控制器 --- */
CorridorCtrlConfig_t ctrl_cfg = {
.kp_theta = PARAM_CTRL_KP_THETA, /* 调优：航向比例增益 */
.kd_theta = PARAM_CTRL_KD_THETA, /* 调优：航向微分增益 */
.kp_y = PARAM_CTRL_KP_Y, /* 调优：横向比例增益 */
.v_cruise = PARAM_CTRL_V_CRUISE, /* 调优：巡航速度 */
.w_max = PARAM_CTRL_W_MAX, /* 角速度限幅 */
.v_max = PARAM_CTRL_V_MAX, /* 线速度限幅 */
.speed_reduction_k = PARAM_CTRL_SPEED_REDUCTION, /* 调优：弯道减速系数 */
};
CorridorCtrl_Init(&ctrl_cfg);
/* --- 初始化段状态机 --- */
SegFsmConfig_t fsm_cfg = {
.d_front_stop = PARAM_SAFE_D_FRONT_STOP, /* 安全：前向停车距离 */
.d_front_approach = PARAM_SAFE_D_FRONT_APPROACH, /* 安全：前向减速距离 */
.approach_min_v = PARAM_SAFE_APPROACH_MIN_V, /* 安全：减速区最低速度 */
.conf_estop_thresh = PARAM_SAFE_CONF_ESTOP, /* 安全：E-Stop 置信度阈值 */
};
SegFsm_Init(&fsm_cfg);
SegFsm_Start(); /* P0 修复: 必须显式启动安全状态机，否则 IDLE 状态直接输出零速 */
/* --- 初始化段脚本执行器 --- */
NavScriptConfig_t script_cfg = {
.turn_target_angle = 3.14159265f, /* 固定：180度转向 */
.turn_omega = PARAM_SCRIPT_TURN_OMEGA, /* 调优：转向角速度 */
.corridor_length = 3.0f, /* 备用：垄沟长度估计 */
.entry_align_timeout = PARAM_SCRIPT_ENTRY_TIMEOUT, /* 调优：入口对准超时 */
.d_entry_exit_front = 0.10f, /* 调优：出入口距离阈值 */
.entry_align_v = PARAM_SCRIPT_ENTRY_V, /* 调优：入口对准速度 */
.exit_runout_m = PARAM_SCRIPT_EXIT_RUNOUT, /* 调优：退出后冲出距离 */
.exit_v = PARAM_SCRIPT_EXIT_V, /* P1 修复：退出直线速度独立参数 */
};
NavScript_Init(&script_cfg);
}

33
App/app_tasks.h Normal file
View File

@@ -0,0 +1,33 @@
#ifndef APP_TASKS_H
#define APP_TASKS_H
#include "cmsis_os2.h"
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
/* =========================================================
* 启动接力标志位定义 (按 Bit 划分)
* ========================================================= */
#define INIT_FLAG_CAN_READY (1UL << 0) // Bit 0: CAN 握手完成
#define INIT_FLAG_IMU_READY (1UL << 1) // Bit 1: IMU 初始化完成
#define INIT_FLAG_VL53_READY (1UL << 2) // Bit 2: 侧向雷达初始化完成
#define INIT_FLAG_LASER_READY (1UL << 3) // Bit 3: 防撞雷达初始化完成
/* 全局事件组句柄声明 */
extern osEventFlagsId_t g_init_events;
void AppTasks_RunCanTxTask(void *argument);
void AppTasks_RunMonitorTask(void *argument);
/* 三个核心传感器采集与黑板更新任务 */
void AppTasks_RunImuTask_Impl(void *argument);
void AppTasks_RunLaserTestTask_Impl(void *argument);
void AppTasks_RunVl53Task_Impl(void *argument);
void AppTasks_RunNavTask_Impl(void *argument);
void AppTasks_Init(void);
#ifdef __cplusplus
}
#endif
#endif /* APP_TASKS_H */

806
App/est/corridor_ekf.c Normal file
View File

@@ -0,0 +1,806 @@
/**
* @file corridor_ekf.c
* @brief 鲁棒 EKF 走廊相对定位滤波器实现
*
* 完整算法流程：
*
* 【预测步】Predict(vx, wz, dt)
* x_pred = f(x, u) -- 非线性状态转移
* P_pred = F * P * F^T + Q -- 协方差预测
*
* 【更新步】Update(obs)
* z = h(x_pred) -- 观测预测
* y = z_meas - z -- 新息 (Innovation)
* S = H * P_pred * H^T + R -- 新息协方差
* d² = y^T * S^(-1) * y -- 马氏距离平方
* if d² > χ²_threshold: 拒绝观测 (鲁棒)
* K = P_pred * H^T * S^(-1) -- 卡尔曼增益
* x = x_pred + K * y -- 状态更新
* P = (I - K * H) * P_pred -- 协方差更新
*/
#include "corridor_ekf.h"
#include <math.h>
#include <string.h>
/* =========================================================
* 内部静态状态
* ========================================================= */
static CorridorEKFConfig_t s_cfg;
static CorridorEKFState_t s_state;
static bool s_initialized = false;
static uint32_t s_last_update_ms = 0U;
/* 协方差上界保护阈值 */
#define P_MAX_DIAG 100.0f
/* =========================================================
* 内部辅助函数
* ========================================================= */
/** 限幅 */
static inline float clampf(float val, float lo, float hi)
{
if (val < lo) return lo;
if (val > hi) return hi;
return val;
}
/** 对称矩阵拷贝 + 上三角 symmetrize */
static void symmetrize(float M[3][3])
{
M[1][0] = M[0][1];
M[2][0] = M[0][2];
M[2][1] = M[1][2];
}
/** P 上界保护 */
static void protect_P(float P[3][3])
{
for (int i = 0; i < 3; i++) {
if (P[i][i] > P_MAX_DIAG) P[i][i] = P_MAX_DIAG;
if (P[i][i] < 0.0f) P[i][i] = 0.0f;
}
}
/** 计算 2x2 对称矩阵的逆 (原地) */
static bool invert_2x2_sym(float S[2][2])
{
float det = S[0][0] * S[1][1] - S[0][1] * S[1][0];
if (fabsf(det) < 1e-8f) {
return false; // 奇异矩阵
}
float inv_det = 1.0f / det;
float S00 = S[0][0];
S[0][0] = inv_det * S[1][1];
S[1][1] = inv_det * S00;
S[0][1] = -inv_det * S[0][1];
S[1][0] = S[0][1];
return true;
}
/** 2x2 对称矩阵求逆 (原地) */
static bool invert_3x3_cholesky(float S[3][3])
{
// 使用 Cholesky 分解求逆
float L[3][3] = {0};
for (int i = 0; i < 3; i++) {
for (int j = 0; j <= i; j++) {
float sum = S[i][j];
for (int k = 0; k < j; k++) {
sum -= L[i][k] * L[j][k];
}
if (i == j) {
if (sum <= 0.0f) return false;
L[i][j] = sqrtf(sum);
} else {
L[i][j] = sum / L[j][j];
}
}
}
// 求逆: S_inv = L^(-T) * L^(-1)
float Linv[3][3] = {0};
for (int i = 0; i < 3; i++) {
Linv[i][i] = 1.0f / L[i][i];
for (int j = i - 1; j >= 0; j--) {
float sum = 0.0f;
for (int k = j + 1; k <= i; k++) {
sum += L[k][j] * Linv[k][i];
}
Linv[j][i] = -sum / L[j][j];
}
}
// S_inv = Linv^T * Linv
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
float sum = 0.0f;
for (int k = 0; k < 3; k++) {
sum += Linv[k][i] * Linv[k][j];
}
S[i][j] = sum;
}
}
return true;
}
/** 计算马氏距离平方 (新息向量 y, 新息协方差 S_inv) */
static float mahalanobis_d2_3dof(const float y[3], const float S_inv[3][3])
{
// d² = y^T * S_inv * y
float tmp[3] = {0};
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
tmp[i] += S_inv[i][j] * y[j];
}
}
float d2 = 0.0f;
for (int i = 0; i < 3; i++) {
d2 += y[i] * tmp[i];
}
return d2;
}
/** 计算马氏距离平方 (1DOF: 只用 e_y) */
static float mahalanobis_d2_1dof(float y_ey, float S_inv_00)
{
return y_ey * y_ey * S_inv_00;
}
/** 计算马氏距离平方 (2DOF: e_ey + e_th_avg) */
static float mahalanobis_d2_2dof(const float y[2], const float S_inv[2][2])
{
float tmp[2] = {0};
for (int i = 0; i < 2; i++) {
for (int j = 0; j < 2; j++) {
tmp[i] += S_inv[i][j] * y[j];
}
}
float d2 = 0.0f;
for (int i = 0; i < 2; i++) {
d2 += y[i] * tmp[i];
}
return d2;
}
/** 零矩阵 */
static void zero_3x3(float M[3][3])
{
memset(M, 0, sizeof(float) * 9);
}
/** 单位矩阵 */
static void eye_3x3(float M[3][3])
{
zero_3x3(M);
M[0][0] = M[1][1] = M[2][2] = 1.0f;
}
/* =========================================================
* API 实现
* ========================================================= */
void CorridorEKF_Init(const CorridorEKFConfig_t *config)
{
s_cfg = *config;
memset(&s_state, 0, sizeof(s_state));
/* 初始化状态 */
s_state.x[0] = 0.0f; // e_y
s_state.x[1] = 0.0f; // e_th
s_state.x[2] = 0.0f; // s
/* 初始化协方差 */
eye_3x3(s_state.P);
s_state.P[0][0] = config->P0_diag[0];
s_state.P[1][1] = config->P0_diag[1];
s_state.P[2][2] = config->P0_diag[2];
s_initialized = true;
}
void CorridorEKF_Reset(void)
{
if (!s_initialized) return;
s_state.x[0] = 0.0f;
s_state.x[1] = 0.0f;
s_state.x[2] = 0.0f;
eye_3x3(s_state.P);
s_state.P[0][0] = s_cfg.P0_diag[0];
s_state.P[1][1] = s_cfg.P0_diag[1];
s_state.P[2][2] = s_cfg.P0_diag[2];
s_last_update_ms = 0U;
}
void CorridorEKF_SetProcessNoise(float q_ey, float q_eth, float q_s)
{
s_cfg.q_ey = q_ey;
s_cfg.q_eth = q_eth;
s_cfg.q_s = q_s;
}
void CorridorEKF_SetMeasurementNoise(float r_ey, float r_eth)
{
s_cfg.r_ey = r_ey;
s_cfg.r_eth = r_eth;
}
/* =========================================================
* 预测步 (Predict)
* ========================================================= */
void CorridorEKF_Predict(float odom_vx, float imu_wz, float dt)
{
if (!s_initialized || dt <= 0.0f) return;
float e_y = s_state.x[0];
float e_th = s_state.x[1];
float s = s_state.x[2];
float vx = odom_vx;
float wz = imu_wz;
/* 状态预测: x_pred = f(x, u) */
float cos_th = cosf(e_th);
float sin_th = sinf(e_th);
/* 安全检查: cos_th 不能太小 (防止数值爆炸) */
if (fabsf(cos_th) < 0.01f) cos_th = (cos_th >= 0.0f) ? 0.01f : -0.01f;
float e_y_pred = e_y + vx * sin_th * dt;
float e_th_pred = e_th + wz * dt;
float s_pred = s + vx * cos_th * dt;
s_state.x[0] = e_y_pred;
s_state.x[1] = e_th_pred;
s_state.x[2] = s_pred;
/* 雅可比矩阵 F (状态转移的 Jacobian) */
float F[3][3] = {0};
F[0][0] = 1.0f;
F[0][1] = vx * cos_th * dt; // de_y/de_th
F[0][2] = 0.0f;
F[1][0] = 0.0f;
F[1][1] = 1.0f;
F[1][2] = 0.0f;
F[2][0] = 0.0f;
F[2][1] = -vx * sin_th * dt; // ds/de_th
F[2][2] = 1.0f;
/* 过程噪声协方差 Q (对角) */
float Q[3][3] = {0};
Q[0][0] = s_cfg.q_ey * dt * dt;
Q[1][1] = s_cfg.q_eth * dt * dt;
Q[2][2] = s_cfg.q_s * dt * dt;
/* 协方差预测: P_pred = F * P * F^T + Q */
float F_P[3][3] = {0};
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
F_P[i][j] += F[i][k] * s_state.P[k][j];
}
}
}
float P_pred[3][3] = {0};
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
P_pred[i][j] += F_P[i][k] * F[j][k]; // F^T: F[j][k] = F[k][j]
}
}
P_pred[i][i] += Q[i][i]; // 加过程噪声
}
symmetrize(P_pred);
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
s_state.P[i][j] = P_pred[i][j];
}
}
protect_P(s_state.P);
}
/* =========================================================
* 观测步 (Update) - 鲁棒 EKF
* ========================================================= */
int CorridorEKF_Update(const CorridorObs_t *obs, CorridorState_t *out_state)
{
if (!s_initialized) return 0;
int updated_obs_count = 0;
/* 清除新息和拒绝掩码 */
memset(s_state.K, 0, sizeof(s_state.K));
uint8_t reject_mask = 0U;
float max_maha_d2 = 0.0f;
/* ----------------------------------------------------
* 提取有效观测
* ---------------------------------------------------- */
bool left_ok = ((obs->valid_mask & (1U << 0)) != 0U) &&
((obs->valid_mask & (1U << 1)) != 0U);
bool right_ok = ((obs->valid_mask & (1U << 2)) != 0U) &&
((obs->valid_mask & (1U << 3)) != 0U);
/* 左右侧横向平均距离 */
float d_lf = obs->d_lf, d_lr = obs->d_lr;
float d_rf = obs->d_rf, d_rr = obs->d_rr;
float Ls = s_cfg.sensor_base_length;
float W = s_cfg.corridor_width;
float yoff = s_cfg.y_offset;
float inset = s_cfg.side_sensor_inset;
float Rw = s_cfg.robot_width;
/* 传感器居中时的理论读数 (考虑车体宽度和传感器内缩)
*
* 推导 (以左侧为例)：
* 沟道宽 W，车体宽 Rw，传感器内缩 inset
* 居中时：
* 车体左边缘到左墙 = (W - Rw) / 2
* 传感器到左墙 = (W - Rw) / 2 + inset (传感器比边缘更靠内)
*
* 所以 d_center = (W - Rw) / 2 + inset
*
* 验证：W=0.40, Rw=0.20, inset=0
* d_center = (0.40-0.20)/2 + 0 = 0.10m ✓ (每边余量10cm)
*
* 单侧公式：e_y = d_center - d_left (左侧传感器越近墙，偏差越大)
* 双侧公式：e_y = [(d_center - d_left) + (d_right - d_center)] / 2
* = (d_right - d_left) / 2 (d_center 被消掉)
*
* ⚠ 当 inset = 0 且 Rw = 0 时，d_center = W/2，退化回原始行为
*/
float d_center = (W - Rw) / 2.0f + inset; /* 传感器居中时的理论读数 */
/* 观测值 (测量) */
float z_ey = 0.0f;
float z_eth_L = 0.0f;
float z_eth_R = 0.0f;
int valid_sides = 0;
if (left_ok) {
z_ey += d_center - ((d_lf + d_lr) / 2.0f) - yoff;
z_eth_L = atan2f(d_lr - d_lf, Ls);
valid_sides++;
}
if (right_ok) {
z_ey += ((d_rf + d_rr) / 2.0f) - d_center - yoff;
z_eth_R = atan2f(d_rf - d_rr, Ls);
valid_sides++;
}
if (valid_sides == 0) {
/* 两边都失效: 协方差持续膨胀，输出预测值 */
out_state->t_ms = obs->t_ms;
out_state->e_y = s_state.x[0];
out_state->e_th = s_state.x[1];
out_state->s = s_state.x[2];
out_state->conf = clampf(1.0f - (s_state.P[0][0] + s_state.P[1][1]) * 0.1f, 0.0f, 1.0f);
out_state->obs_reject_mask = 0xFF;
out_state->mahalanobis_d2 = 0.0f;
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
out_state->P[i][j] = s_state.P[i][j];
}
}
/* 协方差膨胀 (无观测时的信任衰减) */
s_state.P[0][0] += s_cfg.q_ey * 5.0f;
s_state.P[1][1] += s_cfg.q_eth * 5.0f;
protect_P(s_state.P);
return 0;
}
/* 横向观测: 两侧平均 */
if (valid_sides == 2) {
z_ey /= 2.0f;
}
/* ----------------------------------------------------
* 构建观测向量 z 和预测观测 h(x)
* ---------------------------------------------------- */
float e_y = s_state.x[0];
float e_th = s_state.x[1];
/* 预测观测 h(x) */
float h_ey = e_y; // 横向: z_ey ≈ e_y
float h_eth_L = e_th; // 左侧航向: z_eth_L ≈ e_th
float h_eth_R = e_th; // 右侧航向: z_eth_R ≈ e_th
/* 新息向量 y = z - h(x) */
float y[3] = {0};
int obs_idx = 0;
/* e_y 观测 */
y[obs_idx++] = z_ey - h_ey;
/* e_th_L 观测 */
if (left_ok) y[obs_idx++] = z_eth_L - h_eth_L;
/* e_th_R 观测 */
if (right_ok) y[obs_idx++] = z_eth_R - h_eth_R;
/* ----------------------------------------------------
* 构建 H 矩阵 (Jacobian of h(x))
* ---------------------------------------------------- */
/* H 布局:
* 航向角观测对应列是 1,0,0 (e_y 观测量)
* 航向角观测对应列是 0,1,0 (e_th 观测量)
* s 不被直接观测
*/
float H[3][3] = {0};
int H_row = 0;
/* e_y 行: H = [1, 0, 0] */
H[H_row][0] = 1.0f; H[H_row][1] = 0.0f; H[H_row][2] = 0.0f;
H_row++;
/* e_th_L 行: H = [0, 1, 0] */
if (left_ok) {
H[H_row][0] = 0.0f; H[H_row][1] = 1.0f; H[H_row][2] = 0.0f;
H_row++;
}
/* e_th_R 行: H = [0, 1, 0] */
if (right_ok) {
H[H_row][0] = 0.0f; H[H_row][1] = 1.0f; H[H_row][2] = 0.0f;
H_row++;
}
int obs_count = H_row;
/* ----------------------------------------------------
* 构建观测噪声协方差 R (根据有效侧数量调整)
* ---------------------------------------------------- */
float R[3][3] = {0};
R[0][0] = s_cfg.r_ey; // e_y 的噪声
if (left_ok && right_ok) {
/* 双侧: 航向噪声更小 (两个独立观测平均) */
R[1][1] = s_cfg.r_eth * 0.5f; // e_th_L
R[2][2] = s_cfg.r_eth * 0.5f; // e_th_R
} else {
/* 单侧: 航向噪声较大 */
if (left_ok) {
R[1][1] = s_cfg.r_eth;
}
if (right_ok) {
R[1][1] = s_cfg.r_eth;
}
}
/* ----------------------------------------------------
* 计算新息协方差 S = H * P * H^T + R
* ---------------------------------------------------- */
float HP[3][3] = {0};
for (int i = 0; i < obs_count; i++) {
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
HP[i][j] += H[i][k] * s_state.P[k][j];
}
}
}
float S[3][3] = {0};
for (int i = 0; i < obs_count; i++) {
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
S[i][j] += HP[i][k] * H[j][k]; // H^T: H[j][k] = H[k][j]
}
}
S[i][i] += R[i][i]; // 加观测噪声
}
/* ----------------------------------------------------
* 计算 S 的逆
* ---------------------------------------------------- */
float S_inv[3][3] = {0};
if (obs_count == 1) {
/* 1 观测: 标量 */
if (fabsf(S[0][0]) < 1e-8f) {
goto output_result;
}
S_inv[0][0] = 1.0f / S[0][0];
} else if (obs_count == 2) {
/* 2 观测：2x2 - 拷贝到局部矩阵 */
float S_2x2[2][2];
S_2x2[0][0] = S[0][0]; S_2x2[0][1] = S[0][1];
S_2x2[1][0] = S[1][0]; S_2x2[1][1] = S[1][1];
if (!invert_2x2_sym(S_2x2)) {
goto output_result;
}
/* 拷贝回 S_inv */
S_inv[0][0] = S_2x2[0][0]; S_inv[0][1] = S_2x2[0][1];
S_inv[1][0] = S_2x2[1][0]; S_inv[1][1] = S_2x2[1][1];
} else {
/* 3 观测: 3x3 */
memcpy(S_inv, S, sizeof(float) * 9);
if (!invert_3x3_cholesky(S_inv)) {
goto output_result;
}
}
/* ----------------------------------------------------
* χ² 马氏距离检验 (鲁棒拒绝)
* ---------------------------------------------------- */
float d2_total = 0.0f;
if (obs_count == 1) {
d2_total = mahalanobis_d2_1dof(y[0], S_inv[0][0]);
} else if (obs_count == 2) {
d2_total = mahalanobis_d2_2dof(y, (const float (*)[2])S_inv);
} else {
d2_total = mahalanobis_d2_3dof(y, S_inv);
}
max_maha_d2 = d2_total;
/* 单自由度检验: e_y 单独检验 */
float d2_ey = mahalanobis_d2_1dof(y[0], S_inv[0][0]);
if (d2_ey > s_cfg.chi2_1dof) {
reject_mask |= (1U << 0); // 拒绝 e_y
}
/* 1 DOF 门限约 3.84 (95%), 约 6.63 (99%) */
/* 如果整体 d² 过大，拒绝最可疑的观测 */
if (obs_count >= 2) {
/* 检验 e_th_L */
if (left_ok && !(reject_mask & (1U << 0))) {
/* 需要重新计算不含 e_y 的马氏距离 */
/* 简化: 用 y[1]^2 / S[1][1] 作为 1DOF 近似 */
if (fabsf(S[1][1]) > 1e-8f) {
float d2_eth_L = y[1] * y[1] / S[1][1];
if (d2_eth_L > s_cfg.chi2_1dof) {
reject_mask |= (1U << 1); // 拒绝 e_th_L
}
}
}
/* 检验 e_th_R */
if (right_ok && !(reject_mask & (1U << 0)) && obs_count >= 3) {
if (fabsf(S[2][2]) > 1e-8f) {
float d2_eth_R = y[2] * y[2] / S[2][2];
if (d2_eth_R > s_cfg.chi2_1dof) {
reject_mask |= (1U << 2); // 拒绝 e_th_R
}
}
}
}
/* ----------------------------------------------------
* 计算卡尔曼增益 K = P * H^T * S^(-1)
* P1 修复: 必须做完整矩阵乘法 (P*H^T) * S_inv，
* 而不能只乘 S_inv 的对角项 S_inv[j][j]。
* 后者会忽略 S 的非对角元素（观测间相关性），
* 导致卡尔曼增益不正确，影响滤波收敛性。
* ---------------------------------------------------- */
float HT[3][3] = {0};
for (int i = 0; i < 3; i++) {
for (int j = 0; j < obs_count; j++) {
HT[i][j] = H[j][i]; // H^T
}
}
/* Step 1: PH^T = P * H^T, 结果为 3×obs_count */
float PHT[3][3] = {0};
for (int i = 0; i < 3; i++) {
for (int j = 0; j < obs_count; j++) {
for (int k = 0; k < 3; k++) {
PHT[i][j] += s_state.P[i][k] * HT[k][j];
}
}
}
/* Step 2: K = PHT * S_inv, 结果为 3×obs_count */
float K[3][3] = {0};
for (int i = 0; i < 3; i++) {
for (int j = 0; j < obs_count; j++) {
for (int k = 0; k < obs_count; k++) {
K[i][j] += PHT[i][k] * S_inv[k][j];
}
}
}
/* ----------------------------------------------------
* 跳过被拒绝的观测，更新剩余观测
* ---------------------------------------------------- */
int used_obs = 0;
for (int i = 0; i < obs_count; i++) {
uint8_t bit = (i == 0) ? (1U << 0) : ((i == 1) ? (1U << 1) : (1U << 2));
if (reject_mask & bit) {
/*
* 关键：被拒绝的观测不仅不能更新状态，
* 也不能参与后续 KH/P 更新，否则会错误收缩协方差。
*/
K[0][i] = 0.0f;
K[1][i] = 0.0f;
K[2][i] = 0.0f;
continue;
}
/* 状态更新: x += K[:, i] * y[i] */
s_state.x[0] += K[0][i] * y[i];
s_state.x[1] += K[1][i] * y[i];
s_state.x[2] += K[2][i] * y[i];
used_obs++;
}
/* ----------------------------------------------------
* 协方差更新: P = (I - K * H) * P_pred
* 简化 Joseph 形式: P = (I - K*H) * P * (I - K*H)^T + K * R * K^T
* 这里使用简化形式: P = (I - K*H) * P
* ---------------------------------------------------- */
float KH[3][3] = {0};
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
for (int k = 0; k < obs_count; k++) {
KH[i][j] += K[i][k] * H[k][j];
}
}
}
float I_KH[3][3];
I_KH[0][0] = 1.0f - KH[0][0]; I_KH[0][1] = -KH[0][1]; I_KH[0][2] = -KH[0][2];
I_KH[1][0] = -KH[1][0]; I_KH[1][1] = 1.0f - KH[1][1]; I_KH[1][2] = -KH[1][2];
I_KH[2][0] = -KH[2][0]; I_KH[2][1] = -KH[2][1]; I_KH[2][2] = 1.0f - KH[2][2];
float P_tmp[3][3] = {0};
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
P_tmp[i][j] += I_KH[i][k] * s_state.P[k][j];
}
}
}
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
s_state.P[i][j] = P_tmp[i][j];
}
}
symmetrize(s_state.P);
protect_P(s_state.P);
updated_obs_count = used_obs;
output_result:
/* ----------------------------------------------------
* 填充输出
* ---------------------------------------------------- */
out_state->t_ms = obs->t_ms;
out_state->e_y = s_state.x[0];
out_state->e_th = s_state.x[1];
out_state->s = s_state.x[2];
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
out_state->P[i][j] = s_state.P[i][j];
}
}
out_state->mahalanobis_d2 = max_maha_d2;
out_state->obs_reject_mask = reject_mask;
/* 置信度: 基于协方差迹和拒绝比例 */
float P_trace = s_state.P[0][0] + s_state.P[1][1] + s_state.P[2][2];
float conf_from_P = clampf(1.0f - P_trace * 0.5f, 0.0f, 1.0f);
/* 根据有效侧数加权 */
float side_factor = (valid_sides == 2) ? 1.0f : 0.7f;
/* 根据拒绝比例降低置信度 */
float reject_ratio = 0.0f;
if (obs_count > 0) {
int rejected = 0;
if (reject_mask & (1U << 0)) rejected++;
if (reject_mask & (1U << 1)) rejected++;
if (reject_mask & (1U << 2)) rejected++;
reject_ratio = (float)rejected / (float)obs_count;
}
out_state->conf = clampf(conf_from_P * side_factor * (1.0f - reject_ratio * 0.5f), 0.0f, 1.0f);
return updated_obs_count;
}
/* =========================================================
* 辅助 API
* ========================================================= */
/* =========================================================
* IMU 航向观测更新 (独立 1DOF 标量 EKF 更新)
*
* 观测方程: z_eth_imu = imu_yaw_rad - imu_yaw_ref_rad
* 对应状态: e_th (x[1])
* H = [0, 1, 0] (只观测 e_th)
*
* 设计意图：
* - IMU 内部维护的 yaw 经过模块校准，比外部积分 wz 更稳定
* - 在侧墙观测丢失 (转弯/单侧退化) 时提供航向约束
* - 使用较大 R 值，让侧墙观测在有效时主导
* ========================================================= */
void CorridorEKF_UpdateIMUYaw(float imu_yaw_rad, float imu_yaw_ref_rad, bool valid)
{
if (!s_initialized || !valid) return;
/* 观测值: IMU 相对于走廊参考方向的航向偏差 */
float z_eth_imu = imu_yaw_rad - imu_yaw_ref_rad;
/* 新息: y = z - h(x), h(x) = e_th = x[1] */
float y_imu = z_eth_imu - s_state.x[1];
/* H = [0, 1, 0] → S = P[1][1] + R_imu */
float R_imu = s_cfg.r_eth_imu;
float S_imu = s_state.P[1][1] + R_imu;
if (fabsf(S_imu) < 1e-8f) return;
/* χ² 1DOF 检验: d² = y² / S */
float d2_imu = y_imu * y_imu / S_imu;
if (d2_imu > s_cfg.chi2_1dof) return; /* 拒绝异常观测 */
/* 卡尔曼增益: K = P * H^T / S = P[:][1] / S */
float K[3];
float S_inv = 1.0f / S_imu;
K[0] = s_state.P[0][1] * S_inv;
K[1] = s_state.P[1][1] * S_inv;
K[2] = s_state.P[2][1] * S_inv;
/* 状态更新: x += K * y */
s_state.x[0] += K[0] * y_imu;
s_state.x[1] += K[1] * y_imu;
s_state.x[2] += K[2] * y_imu;
/* 协方差更新: P = (I - K*H) * P
* H = [0, 1, 0], 所以 KH[i][j] = K[i] * H[j] = K[i] * δ(j,1) */
float P_new[3][3];
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
P_new[i][j] = s_state.P[i][j] - K[i] * s_state.P[1][j];
}
}
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
s_state.P[i][j] = P_new[i][j];
}
}
symmetrize(s_state.P);
protect_P(s_state.P);
}
void CorridorEKF_GetState(CorridorState_t *out)
{
if (!s_initialized || out == NULL) return;
out->t_ms = s_last_update_ms;
out->e_y = s_state.x[0];
out->e_th = s_state.x[1];
out->s = s_state.x[2];
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
out->P[i][j] = s_state.P[i][j];
}
}
out->mahalanobis_d2 = 0.0f;
out->obs_reject_mask = 0U;
float P_trace = s_state.P[0][0] + s_state.P[1][1] + s_state.P[2][2];
out->conf = clampf(1.0f - P_trace * 0.5f, 0.0f, 1.0f);
}

137
App/est/corridor_ekf.h Normal file
View File

@@ -0,0 +1,137 @@
/**
* @file corridor_ekf.h
* @brief 鲁棒 EKF 走廊相对定位滤波器
*
* 状态向量：x = [e_y, e_th, s]^T
* - e_y : 横向偏差 (m)
* - e_th : 航向偏差 (rad)
* - s : 沿走廊进度 (m)
*
* 观测向量：z = [z_ey, z_eth_L, z_eth_R]^T
* - z_ey : 横向偏差观测 (由左右侧平均距离差计算)
* - z_eth_L: 左侧航向观测 (atan2((d_lr-d_lf), L_s))
* - z_eth_R: 右侧航向观测 (atan2((d_rf-d_rr), L_s))
*
* 鲁棒机制：
* - χ² 马氏距离检验拒绝异常观测
* - 自适应观测噪声 (根据传感器健康度调整 R 矩阵)
* - 协方差上界保护 (防止发散)
*/
#ifndef CORRIDOR_EKF_H
#define CORRIDOR_EKF_H
#include <stdint.h>
#include <stdbool.h>
/* 先引入消息定义 (含 EKF_STATE_DIM/EKF_OBS_DIM 宏) */
#include "preproc/corridor_msgs.h"
#ifdef __cplusplus
extern "C" {
#endif
/* =========================================================
* EKF 配置参数
* ========================================================= */
typedef struct {
/* 过程噪声协方差 Q */
float q_ey; // e_y 过程噪声方差
float q_eth; // e_th 过程噪声方差
float q_s; // s 过程噪声方差
/* 观测噪声协方差 R */
float r_ey; // 横向观测噪声方差 (侧墙)
float r_eth; // 航向观测噪声方差 (侧墙)
float r_eth_imu; // 航向观测噪声方差 (IMU yaw)，建议远大于 r_eth
/* 初始协方差 */
float P0_diag[3]; // 初始 P 对角线
/* χ² 检验门限 */
float chi2_1dof; // 1 自由度门限 (默认 3.84)
float chi2_2dof; // 2 自由度门限 (默认 5.99)
/* 走廊几何参数 */
float sensor_base_length; // 同侧前后雷达间距 L_s
float corridor_width; // 走廊标准宽度
float y_offset; // 期望偏置
float side_sensor_inset; // VL53L0X 侧向传感器内缩距离 (传感器面到车体外边缘)
float robot_width; // 车体外轮廓宽度 (用于单侧退化时的精确定位)
} CorridorEKFConfig_t;
/* =========================================================
* EKF 内部状态
* ========================================================= */
typedef struct {
float x[EKF_STATE_DIM]; // 状态向量
float P[EKF_STATE_DIM][EKF_STATE_DIM]; // 状态协方差
float K[EKF_STATE_DIM][EKF_OBS_DIM]; // 卡尔曼增益
float S[EKF_OBS_DIM][EKF_OBS_DIM]; // 新息协方差
float S_inv[EKF_OBS_DIM][EKF_OBS_DIM]; // S 的逆
} CorridorEKFState_t;
/* =========================================================
* API 接口
* ========================================================= */
/**
* @brief 初始化 EKF 滤波器
* @param config 配置参数
*/
void CorridorEKF_Init(const CorridorEKFConfig_t *config);
/**
* @brief EKF 预测步 (时间更新)
* @param odom_vx 里程计线速度 (m/s)
* @param imu_wz IMU 角速度 (rad/s)
* @param dt 时间间隔 (s)
*/
void CorridorEKF_Predict(float odom_vx, float imu_wz, float dt);
/**
* @brief EKF 观测步 (测量更新) - 鲁棒版本
* @param obs 预处理后的观测快照
* @param out_state 输出状态 (含马氏距离、拒绝掩码等)
* @return 成功更新的观测数
*/
int CorridorEKF_Update(const CorridorObs_t *obs, CorridorState_t *out_state);
/**
* @brief EKF IMU 航向观测更新 (独立于侧墙观测，在 Update 后调用)
*
* 将 IMU 连续 yaw 的变化量作为 e_th 的额外标量观测，执行 1DOF EKF 更新。
* 在侧墙观测丢失时 (转弯/单侧退化) 提供航向约束，防止 e_th 漂移。
*
* @param imu_yaw_rad IMU 连续 yaw 当前值 (rad)
* @param imu_yaw_ref_rad 进入走廊时锁定的 IMU yaw 参考值 (rad)
* z_eth_imu = imu_yaw_rad - imu_yaw_ref_rad
* @param valid IMU 数据是否有效
*/
void CorridorEKF_UpdateIMUYaw(float imu_yaw_rad, float imu_yaw_ref_rad, bool valid);
/**
* @brief 获取当前状态估计
*/
void CorridorEKF_GetState(CorridorState_t *out);
/**
* @brief 重置 EKF 状态
*/
void CorridorEKF_Reset(void);
/**
* @brief 设置过程噪声 (运行时可调)
*/
void CorridorEKF_SetProcessNoise(float q_ey, float q_eth, float q_s);
/**
* @brief 设置观测噪声 (运行时可调，用于自适应)
*/
void CorridorEKF_SetMeasurementNoise(float r_ey, float r_eth);
#ifdef __cplusplus
}
#endif
#endif /* CORRIDOR_EKF_H */

108
App/est/corridor_filter.c Normal file
View File

@@ -0,0 +1,108 @@
/**
* @file corridor_filter.c
* @brief 走廊相对定位滤波器 - 鲁棒 EKF 实现
*
* 本文件是对外统一接口层，内部调用 corridor_ekf 模块
* 保持与原有互补滤波接口兼容，方便无缝替换
*/
#include "corridor_filter.h"
#include "corridor_ekf.h"
#include "preproc/corridor_msgs.h" /* 引入 CHI2_THRESHOLD 宏 */
#include "robot_params.h" /* 引入 PARAM_EKF_* 宏 */
#include <math.h>
#include <string.h>
/* =========================================================
* 兼容层配置
* ========================================================= */
static CorridorFilterConfig_t s_cfg;
static bool s_initialized = false;
/* IMU yaw 参考值管理：进入走廊时锁定，用于计算相对走廊方向的航向偏差 */
static float s_imu_yaw_ref_rad = 0.0f; // 走廊参考 yaw (rad)
static bool s_imu_yaw_ref_set = false; // 参考值是否已锁定
/* =========================================================
* 初始化
* ========================================================= */
void CorridorFilter_Init(const CorridorFilterConfig_t *config)
{
s_cfg = *config;
/* 转换为 EKF 配置 */
CorridorEKFConfig_t ekf_cfg;
memset(&ekf_cfg, 0, sizeof(ekf_cfg));
/* 走廊几何参数 */
ekf_cfg.sensor_base_length = config->sensor_base_length;
ekf_cfg.corridor_width = config->corridor_width;
ekf_cfg.y_offset = config->y_offset;
ekf_cfg.side_sensor_inset = config->side_sensor_inset;
ekf_cfg.robot_width = config->robot_width;
/* 过程噪声 Q —— 统一从 robot_params.h 读取，改参数只改那一个文件 */
ekf_cfg.q_ey = PARAM_EKF_Q_EY; /* 横向过程噪声 */
ekf_cfg.q_eth = PARAM_EKF_Q_ETH; /* 航向过程噪声 */
ekf_cfg.q_s = PARAM_EKF_Q_S; /* 里程过程噪声 */
/* 观测噪声 R */
ekf_cfg.r_ey = PARAM_EKF_R_EY; /* 横向观测噪声 (侧墙) */
ekf_cfg.r_eth = PARAM_EKF_R_ETH; /* 航向观测噪声 (侧墙) */
ekf_cfg.r_eth_imu = PARAM_EKF_R_ETH_IMU; /* 航向观测噪声 (IMU yaw)，较大 */
/* 初始协方差 */
ekf_cfg.P0_diag[0] = PARAM_EKF_P0_EY; /* e_y 初始不确定度 */
ekf_cfg.P0_diag[1] = PARAM_EKF_P0_ETH; /* e_th 初始不确定度 */
ekf_cfg.P0_diag[2] = 0.0f; /* s 初始确定（已知从 0 出发） */
/* χ² 门限 */
ekf_cfg.chi2_1dof = PARAM_CHI2_1DOF;
ekf_cfg.chi2_2dof = PARAM_CHI2_2DOF;
CorridorEKF_Init(&ekf_cfg);
s_initialized = true;
}
/* =========================================================
* 核心更新函数
* ========================================================= */
void CorridorFilter_Update(const CorridorObs_t *obs, float imu_wz, float odom_vx,
float dt_s, float imu_yaw_continuous_rad, bool imu_yaw_valid,
CorridorState_t *out_state)
{
if (!s_initialized || dt_s <= 0.0f) {
return;
}
/* 预测步 */
CorridorEKF_Predict(odom_vx, imu_wz, dt_s);
/* 侧墙观测更新步 (含马氏距离异常检测) */
CorridorEKF_Update(obs, out_state);
/* ---- IMU yaw 航向观测更新 ---- *
* 在侧墙观测之后独立执行 1DOF 标量更新。
*
* 参考值管理策略：
* 首次收到有效 IMU yaw 且 EKF 置信度足够时锁定参考值。
* 此时 e_th ≈ 0 (刚被侧墙观测修正过)，
* 所以 yaw_ref = imu_yaw_current → z_eth_imu = 0，
* 后续 yaw 的变化量就等于 e_th 的变化量。
*/
if (imu_yaw_valid) {
if (!s_imu_yaw_ref_set && out_state->conf >= 0.5f) {
/* 首次锁定：此刻 e_th 已被侧墙修正到接近 0 */
s_imu_yaw_ref_rad = imu_yaw_continuous_rad - out_state->e_th;
s_imu_yaw_ref_set = true;
}
if (s_imu_yaw_ref_set) {
CorridorEKF_UpdateIMUYaw(imu_yaw_continuous_rad, s_imu_yaw_ref_rad, true);
/* 更新输出 (IMU 观测可能微调了 e_th) */
CorridorEKF_GetState(out_state);
out_state->t_ms = obs->t_ms;
}
}
}

46
App/est/corridor_filter.h Normal file
View File

@@ -0,0 +1,46 @@
#ifndef CORRIDOR_FILTER_H
#define CORRIDOR_FILTER_H
#include "preproc/corridor_preproc.h"
#include "preproc/corridor_msgs.h"
/* 滤波器配置参数结构体 */
typedef struct {
float sensor_base_length; // 同侧前后雷达的纵向安装间距 L_s (m)
float corridor_width; // 走廊标准宽度 (m)，比赛规则为 0.4m
float y_offset; // 期望的偏置行走量 (m)，0 表示绝对居中
float side_sensor_inset; // VL53L0X 传感器内缩距离 (传感器面到车体外边缘) (m)
float robot_width; // 车体外轮廓宽度 (m)
float alpha_theta; // 航向互补滤波系数 (0~1)
// 【注】因为您的IMU极好，此值建议设为 0.98~0.995
float alpha_y; // 横向低通滤波系数 (0~1)，建议设为 0.6~0.8 防止地毯颠簸
} CorridorFilterConfig_t;
#ifdef __cplusplus
extern "C" {
#endif
/**
* @brief 初始化走廊滤波器
* @param config 滤波器调参配置
*/
void CorridorFilter_Init(const CorridorFilterConfig_t *config);
/**
* @brief 核心函数：执行一次走廊状态估计 (建议与 CAN 发送同频，如 50Hz/20ms运行)
* @param obs 预处理模块输出的、清洗干净的雷达观测快照
* @param imu_wz 当前的 IMU Z轴角速度 (rad/s)，左转为正
* @param odom_vx 底盘反馈的当前里程计线速度 (m/s)
* @param dt_s 距离上一次调用的时间间隔 (s)
* @param imu_yaw_continuous_rad IMU unwrap 后的连续偏航角 (rad)
* @param imu_yaw_valid IMU 数据是否有效
* @param out_state 输出平滑后的 e_y (横向误差), e_th (航向误差) 和健康度
*/
void CorridorFilter_Update(const CorridorObs_t *obs, float imu_wz, float odom_vx,
float dt_s, float imu_yaw_continuous_rad, bool imu_yaw_valid,
CorridorState_t *out_state);
#ifdef __cplusplus
}
#endif
#endif // CORRIDOR_FILTER_H

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

83
App/nav/corridor_ctrl.c Normal file
View File

@@ -0,0 +1,83 @@
#include "corridor_ctrl.h"
#include <math.h>
#include <stdbool.h>
/* ====================== 内部状态 ====================== */
static CorridorCtrlConfig_t s_cfg;
static bool s_initialized = false;
/* 辅助：浮点数限幅 */
static inline float clampf(float val, float lo, float hi)
{
if (val < lo) return lo;
if (val > hi) return hi;
return val;
}
void CorridorCtrl_Init(const CorridorCtrlConfig_t *config)
{
s_cfg = *config;
s_initialized = true;
}
void CorridorCtrl_Compute(const CorridorState_t *state,
const CorridorObs_t *obs,
float imu_wz,
RawCmd_t *out_cmd)
{
if (!s_initialized) {
out_cmd->v = 0.0f;
out_cmd->w = 0.0f;
out_cmd->flags = 0U;
return;
}
/* ========================================================
* 核心控制律:
* w_cmd = kp_theta * e_th + kd_theta * (-imu_wz) + kp_y * e_y
*
* - kp_theta * e_th : 比例项，车头偏了就回转
* - kd_theta * (-imu_wz) : 微分阻尼，等价于"阻止车头继续转"
* 用 IMU 直接读数做微分项，比差分 e_th 更丝滑无噪声
* - kp_y * e_y : 横向纠偏，车身偏了就产生角速度拉回来
* ======================================================== */
float w_cmd = s_cfg.kp_theta * state->e_th
+ s_cfg.kd_theta * (-imu_wz)
+ s_cfg.kp_y * state->e_y;
/* 角速度限幅：防止 PD 溢出导致原地打转 */
w_cmd = clampf(w_cmd, -s_cfg.w_max, s_cfg.w_max);
/* ========================================================
* 线速度策略:
* 基础巡航速度 v_cruise，
* 当角速度偏大时适当降速（弯道减速），保持运动学协调
* 公式: v = v_cruise * (1 - k * |w/w_max|)
* k 取 0.3~0.5 较保守
* ======================================================== */
float speed_reduction = s_cfg.speed_reduction_k * fabsf(w_cmd) / s_cfg.w_max;
float v_cmd = s_cfg.v_cruise * (1.0f - speed_reduction);
/* 线速度限幅：不允许倒车，不允许超速 */
v_cmd = clampf(v_cmd, 0.0f, s_cfg.v_max);
/* ========================================================
* 置信度降级保护:
* 当滤波器健康度 conf 过低（两边雷达全瞎），
* 说明走廊参照完全丢失，降低线速度防止盲飞
* ======================================================== */
if (state->conf < 0.3f) {
/* 健康度极低：速度打三折，保持航向惯性滑行 */
v_cmd *= 0.3f;
} else if (state->conf < 0.6f) {
/* 健康度较低（单侧退化）：速度打七折 */
v_cmd *= 0.7f;
}
/* 输出赋值 */
out_cmd->t_ms = state->t_ms;
out_cmd->v = v_cmd;
out_cmd->w = w_cmd;
out_cmd->flags = 0U;
}

47
App/nav/corridor_ctrl.h Normal file
View File

@@ -0,0 +1,47 @@
#ifndef CORRIDOR_CTRL_H
#define CORRIDOR_CTRL_H
#include "preproc/corridor_msgs.h"
/**
* @brief 走廊控制器调参配置
* @note 所有增益参数都应在调试阶段"从小往大调"，防止震荡
*/
typedef struct {
float kp_theta; // 航向偏差比例增益 (rad/s per rad)
float kd_theta; // 航向偏差微分增益 (rad/s per rad/s) [阻尼项]
float kp_y; // 横向偏差比例增益 (rad/s per m)
float v_cruise; // 走廊内巡航线速度 (m/s)
float w_max; // 角速度输出硬限幅 (rad/s)，超过此值一律削峰
float v_max; // 线速度输出硬限幅 (m/s)
float speed_reduction_k; // 弯道减速系数 (0~1)，公式: v = v_cruise*(1-k*|w/w_max|)
} CorridorCtrlConfig_t;
#ifdef __cplusplus
extern "C" {
#endif
/**
* @brief 初始化走廊控制器
* @param config 控制器增益与限幅配置
*/
void CorridorCtrl_Init(const CorridorCtrlConfig_t *config);
/**
* @brief 核心函数：根据滤波器输出的走廊状态计算控制量
* @param state 滤波器输出的走廊状态 (e_y, e_th, conf)
* @param obs 预处理层的观测快照 (用于获取 d_front 等辅助信息)
* @param imu_wz 当前的 IMU Z轴角速度 (rad/s)，用于微分阻尼
* @param out_cmd 输出的控制指令 (v, w)
*/
void CorridorCtrl_Compute(const CorridorState_t *state,
const CorridorObs_t *obs,
float imu_wz,
RawCmd_t *out_cmd);
#ifdef __cplusplus
}
#endif
#endif // CORRIDOR_CTRL_H

322
App/nav/nav_script.c Normal file
View File

@@ -0,0 +1,322 @@
/**
* @file nav_script.c
* @brief 段脚本执行器实现：编排完整比赛流程
*/
#include "nav_script.h"
#include <stdint.h>
#include <stdbool.h>
#include <math.h>
#include <string.h>
/* =========================================================
* 内部状态
* ========================================================= */
static NavScriptConfig_t s_cfg;
static NavScriptStage_t s_stage = SCRIPT_STAGE_IDLE;
static bool s_initialized = false;
static bool s_running = false;
/* 阶段内部状态变量 */
static struct {
uint32_t entry_align_start_ms; // 入口对准开始时间
float turn_start_e_th; // 转向开始时的 EKF 航向 (保留作后备)
float turn_start_imu_yaw_deg; // 转向开始时的 IMU 连续偏航角 (deg)
bool turn_started; // 转向是否已开始
float corridor_s_entry; // 进入垄沟时的 s 里程
int pass_count; // 已走过的垄沟数
float exit_start_s; // 离开垄沟瞬间的 s 里程 (0=未触发)
} s_internal;
/* =========================================================
* 内部辅助函数
* ========================================================= */
static inline float clampf(float val, float lo, float hi)
{
if (val < lo) return lo;
if (val > hi) return hi;
return val;
}
/* =========================================================
* API 实现
* ========================================================= */
void NavScript_Init(const NavScriptConfig_t *config)
{
s_cfg = *config;
s_stage = SCRIPT_STAGE_IDLE;
s_running = false;
s_initialized = true;
/* 初始化内部状态 */
memset(&s_internal, 0, sizeof(s_internal));
}
void NavScript_Start(void)
{
if (s_initialized) {
s_stage = SCRIPT_STAGE_ENTRY_ALIGN;
s_running = true;
s_internal.entry_align_start_ms = 0;
s_internal.pass_count = 0;
}
}
NavScriptStage_t NavScript_GetStage(void)
{
return s_stage;
}
const char* NavScript_GetStageName(NavScriptStage_t stage)
{
switch (stage) {
case SCRIPT_STAGE_IDLE: return "IDLE";
case SCRIPT_STAGE_ENTRY_ALIGN: return "ENTRY_ALIGN";
case SCRIPT_STAGE_CORRIDOR_FORWARD: return "CORRIDOR_FWD";
case SCRIPT_STAGE_TURN_AT_END: return "TURN_AT_END";
case SCRIPT_STAGE_CORRIDOR_BACKWARD: return "CORRIDOR_BWD";
case SCRIPT_STAGE_EXIT: return "EXIT";
case SCRIPT_STAGE_FINISHED: return "FINISHED";
default: return "UNKNOWN";
}
}
void NavScript_Reset(void)
{
s_stage = SCRIPT_STAGE_IDLE;
s_running = false;
memset(&s_internal, 0, sizeof(s_internal));
}
void NavScript_Update(const CorridorObs_t *obs,
const CorridorState_t *state,
float imu_yaw_continuous_deg,
NavScriptOutput_t *out)
{
if (!s_initialized) {
out->stage = SCRIPT_STAGE_IDLE;
out->stage_name = "IDLE";
out->active = false;
out->request_corridor = false;
out->override_v = 0.0f;
out->override_w = 0.0f;
out->use_override = true;
return;
}
/* 默认输出 */
out->stage = s_stage;
out->stage_name = NavScript_GetStageName(s_stage);
out->active = s_running;
out->request_corridor = true; // 默认请求走廊控制器
out->use_override = false; // 默认不覆盖
if (!s_running) {
out->request_corridor = false;
out->override_v = 0.0f;
out->override_w = 0.0f;
out->use_override = true;
return;
}
/* ========================================================
* 核心状态机：按比赛流程执行
* ======================================================== */
switch (s_stage) {
/* ==============================================
* STAGE 1: 入口对准
* ============================================== */
case SCRIPT_STAGE_ENTRY_ALIGN: {
if (s_internal.entry_align_start_ms == 0) {
s_internal.entry_align_start_ms = state->t_ms;
}
/* 检查是否已进入垄沟：两侧都有距离数据 */
bool left_ok = ((obs->valid_mask & CORRIDOR_OBS_MASK_LF) != 0U) &&
((obs->valid_mask & CORRIDOR_OBS_MASK_LR) != 0U);
bool right_ok = ((obs->valid_mask & CORRIDOR_OBS_MASK_RF) != 0U) &&
((obs->valid_mask & CORRIDOR_OBS_MASK_RR) != 0U);
if (left_ok && right_ok && state->conf >= 0.8f) {
/* 两侧雷达都有数据，且置信度高 -> 进入垄沟，开始跟踪 */
s_internal.corridor_s_entry = state->s;
s_internal.pass_count = 1;
s_stage = SCRIPT_STAGE_CORRIDOR_FORWARD;
out->request_corridor = true;
} else {
/* 入口对准策略：慢速前进，让侧向雷达找墙 */
out->override_v = s_cfg.entry_align_v; // 慢速前进 (由 PARAM_SCRIPT_ENTRY_V 配置)
out->override_w = 0.0f;
out->use_override = true;
out->request_corridor = false;
/* P1 修复: 超时保护使用配置参数 s_cfg.entry_align_timeout，
* 而非硬编码 30000 ms */
uint32_t elapsed = state->t_ms - s_internal.entry_align_start_ms;
if (elapsed > (uint32_t)s_cfg.entry_align_timeout) {
/* 超时强制进入走廊跟踪：必须补齐与正常入沟一致的内部状态，
* 否则 pass_count 停留在 0，导致后续 TURN_AT_END 判定时
* 多跑一趟走廊（三趟而非文档描述的两趟）。 */
s_internal.corridor_s_entry = state->s;
s_internal.pass_count = 1;
s_stage = SCRIPT_STAGE_CORRIDOR_FORWARD;
}
}
break;
}
/* ==============================================
* STAGE 2: 向前走垄沟
* ============================================== */
case SCRIPT_STAGE_CORRIDOR_FORWARD: {
/* 使用走廊控制器 */
out->request_corridor = true;
/* 检查是否到端 */
bool front_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_FRONT) != 0U;
if (front_ok && obs->d_front <= s_cfg.d_entry_exit_front) {
/* 前向距离足够近 -> 到达垄沟末端，准备转向 */
s_internal.turn_start_e_th = state->e_th;
s_internal.turn_start_imu_yaw_deg = imu_yaw_continuous_deg;
s_internal.turn_started = false;
s_stage = SCRIPT_STAGE_TURN_AT_END;
out->request_corridor = false;
}
break;
}
/* ==============================================
* STAGE 3: 到端原地转向
* ============================================== */
case SCRIPT_STAGE_TURN_AT_END: {
out->request_corridor = false;
if (!s_internal.turn_started) {
s_internal.turn_start_e_th = state->e_th;
s_internal.turn_start_imu_yaw_deg = imu_yaw_continuous_deg;
s_internal.turn_started = true;
}
/* 原地 180 度转向策略 */
float target_delta = s_cfg.turn_target_angle; // 默认 PI (180度)
/* ---- 使用 IMU 连续 yaw 判定转角 (deg → rad) ---- *
* 优点：IMU 内部维护的 yaw 经过模块校准，比外部积分 wz 更稳定，
* 且 unwrap 后不存在 ±180° 跳变问题。
* 后备：如果 IMU 离线，退化回 EKF e_th 差值判定。 */
float delta_turned;
float imu_delta_deg = imu_yaw_continuous_deg - s_internal.turn_start_imu_yaw_deg;
delta_turned = imu_delta_deg * 0.01745329252f; // deg → rad
float remaining = target_delta - fabsf(delta_turned);
if (remaining <= 0.1f) {
/* 转向完成 -> 决定下一步 */
if (s_internal.pass_count < 2) {
/* 只走了一遍，往回走 */
s_internal.pass_count++;
s_stage = SCRIPT_STAGE_CORRIDOR_BACKWARD;
} else {
/* 走了两遍，退出场地 */
s_stage = SCRIPT_STAGE_EXIT;
}
out->override_v = 0.0f;
out->override_w = 0.0f;
out->use_override = true;
} else {
/* 继续转向：根据目标方向选择角速度 */
float turn_dir = (target_delta > 0.0f) ? 1.0f : -1.0f;
/* 接近目标时减速 */
float turn_speed = s_cfg.turn_omega;
if (remaining < 0.5f) {
turn_speed *= (remaining / 0.5f); // 线性减速
turn_speed = clampf(turn_speed, 0.3f, s_cfg.turn_omega);
}
out->override_v = 0.0f; // 线速度为 0，原地转
out->override_w = turn_dir * turn_speed;
out->use_override = true;
}
break;
}
/* ==============================================
* STAGE 4: 返程走垄沟 (原地转 180° 后继续正向行驶)
* ============================================== */
case SCRIPT_STAGE_CORRIDOR_BACKWARD: {
/* 使用走廊控制器，正向行驶 */
out->request_corridor = true;
/* P1 修复: 原地转 180° 后车头已调转，返回方向即"向前"，
* 因此到端检测应使用前向雷达 d_front，而非后向雷达 d_back */
bool front_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_FRONT) != 0U;
if (front_ok && obs->d_front <= s_cfg.d_entry_exit_front) {
/* 前向距离足够近 -> 到达垄沟起始端，转向或退出 */
s_internal.turn_start_e_th = state->e_th;
s_internal.turn_start_imu_yaw_deg = imu_yaw_continuous_deg;
s_internal.turn_started = false;
s_stage = SCRIPT_STAGE_TURN_AT_END;
out->request_corridor = false;
}
break;
}
/* ==============================================
* STAGE 5: 退出场地
* ============================================== */
case SCRIPT_STAGE_EXIT: {
/* P1 修复: 退出速度使用独立的 exit_v 参数，
* 而非误用角速度参数 turn_omega * 0.5f */
out->request_corridor = false;
out->override_v = s_cfg.exit_v; // 直线冲出速度 (由 PARAM_SCRIPT_EXIT_V 配置)
out->override_w = 0.0f;
out->use_override = true;
/* 检测是否已离开：两侧雷达都丢失 */
bool left_ok = ((obs->valid_mask & CORRIDOR_OBS_MASK_LF) != 0U) &&
((obs->valid_mask & CORRIDOR_OBS_MASK_LR) != 0U);
bool right_ok = ((obs->valid_mask & CORRIDOR_OBS_MASK_RF) != 0U) &&
((obs->valid_mask & CORRIDOR_OBS_MASK_RR) != 0U);
if (!left_ok && !right_ok) {
/* 两侧都丢了 -> 已离开垄沟，再往前冲 exit_runout_m 后停车 */
if (s_internal.exit_start_s == 0.0f) {
s_internal.exit_start_s = state->s;
}
if (state->s - s_internal.exit_start_s >= s_cfg.exit_runout_m) {
/* 冲够了，停车 */
s_stage = SCRIPT_STAGE_FINISHED;
s_internal.exit_start_s = 0.0f;
}
}
break;
}
/* ==============================================
* STAGE 6: 比赛完成，安全停车
* ============================================== */
case SCRIPT_STAGE_FINISHED: {
out->request_corridor = false;
out->override_v = 0.0f;
out->override_w = 0.0f;
out->use_override = true;
s_running = false;
break;
}
/* ==============================================
* DEFAULT/IDLE: 安全停车
* ============================================== */
case SCRIPT_STAGE_IDLE:
default: {
out->request_corridor = false;
out->override_v = 0.0f;
out->override_w = 0.0f;
out->use_override = true;
break;
}
}
}

133
App/nav/nav_script.h Normal file
View File

@@ -0,0 +1,133 @@
/**
* @file nav_script.h
* @brief 段脚本执行器：编排完整比赛流程
*
* 比赛流程模型：
* ┌─────────────┐
* │ ENTRY_ALIGN │ 入口对准：启动区 -> 第一垄沟入口
* └──────┬──────┘
* │
* v
* ┌─────────────┐
* │ CORRIDOR_A │ 第一垄沟跟踪
* └──────┬──────┘
* │ (到端)
* v
* ┌─────────────┐
* │ TURN_180 │ 原地 180 度转向
* └──────┬──────┘
* │
* v
* ┌─────────────┐
* │ CORRIDOR_B │ 返回垄沟跟踪
* └──────┬──────┘
* │ (到端)
* v
* ┌─────────────┐
* │ EXIT │ 退出比赛场地
* └──────┬──────┘
* │
* v
* ┌─────────────┐
* │ FINISH │ 停在启动区
* └─────────────┘
*/
#ifndef NAV_SCRIPT_H
#define NAV_SCRIPT_H
#include <stdint.h>
#include <stdbool.h>
#include "preproc/corridor_msgs.h"
#include "preproc/corridor_preproc.h"
#ifdef __cplusplus
extern "C" {
#endif
/* =========================================================
* 段脚本阶段枚举
* ========================================================= */
typedef enum {
SCRIPT_STAGE_IDLE = 0, // 待命阶段，不输出任何指令
SCRIPT_STAGE_ENTRY_ALIGN, // 入口对准：从启动区到第一垄沟
SCRIPT_STAGE_CORRIDOR_FORWARD, // 向前走垄沟
SCRIPT_STAGE_TURN_AT_END, // 到端原地转向
SCRIPT_STAGE_CORRIDOR_BACKWARD,// 向后走垄沟
SCRIPT_STAGE_EXIT, // 退出场地
SCRIPT_STAGE_FINISHED, // 比赛完成，安全停车
} NavScriptStage_t;
/* =========================================================
* 段脚本配置参数
* ========================================================= */
typedef struct {
float turn_target_angle; // 原地转向目标角度 (rad)，默认 PI (180度)
float turn_omega; // 原地转向角速度 (rad/s)
float corridor_length; // 垄沟长度估计 (m)，用于触发到端（备用）
float entry_align_timeout; // 入口对准超时 (ms)
float d_entry_exit_front; // 出入口前向距离阈值 (m)
float entry_align_v; // 入口对准慢速前进速度 (m/s)
float exit_runout_m; // 离开走廊后继续冲出的距离 (m)
float exit_v; // 退出场地直线冲出速度 (m/s)
} NavScriptConfig_t;
/* =========================================================
* 段脚本输出
* ========================================================= */
typedef struct {
NavScriptStage_t stage; // 当前执行阶段
const char *stage_name; // 阶段名称字符串
bool active; // 脚本是否正在运行
bool request_corridor; // 是否请求走廊控制器
float override_v; // 覆盖输出的 v (m/s)
float override_w; // 覆盖输出的 w (rad/s)
bool use_override; // 是否使用覆盖值
} NavScriptOutput_t;
/* =========================================================
* API 接口
* ========================================================= */
/**
* @brief 初始化段脚本执行器
*/
void NavScript_Init(const NavScriptConfig_t *config);
/**
* @brief 开始运行比赛脚本
*/
void NavScript_Start(void);
/**
* @brief 核心执行函数：每个导航周期调用一次
* @param obs 预处理层的观测快照
* @param state 滤波器的走廊状态
* @param imu_yaw_continuous_deg IMU unwrap 后的连续偏航角 (度)，用于转弯判定
* @param out 段脚本输出
*/
void NavScript_Update(const CorridorObs_t *obs,
const CorridorState_t *state,
float imu_yaw_continuous_deg,
NavScriptOutput_t *out);
/**
* @brief 获取当前执行阶段
*/
NavScriptStage_t NavScript_GetStage(void);
/**
* @brief 获取阶段名称
*/
const char* NavScript_GetStageName(NavScriptStage_t stage);
/**
* @brief 强制重置脚本到 IDLE
*/
void NavScript_Reset(void);
#ifdef __cplusplus
}
#endif
#endif // NAV_SCRIPT_H

156
App/nav/segment_fsm.c Normal file
View File

@@ -0,0 +1,156 @@
#include "segment_fsm.h"
#include <string.h>
/* ====================== 内部状态 ====================== */
static SegFsmConfig_t s_cfg;
static SegFsmState_t s_state = SEG_STATE_IDLE;
static bool s_initialized = false;
/* 辅助：浮点数限幅 */
static inline float seg_clampf(float val, float lo, float hi)
{
if (val < lo) return lo;
if (val > hi) return hi;
return val;
}
void SegFsm_Init(const SegFsmConfig_t *config)
{
s_cfg = *config;
s_state = SEG_STATE_IDLE;
s_initialized = true;
}
void SegFsm_Start(void)
{
if (s_initialized) {
s_state = SEG_STATE_CORRIDOR;
}
}
SegFsmState_t SegFsm_GetState(void)
{
return s_state;
}
const char* SegFsm_GetStateName(SegFsmState_t s)
{
switch (s) {
case SEG_STATE_IDLE: return "IDLE";
case SEG_STATE_CORRIDOR: return "CORRIDOR";
case SEG_STATE_APPROACH: return "APPROACH";
case SEG_STATE_STOP: return "STOP";
case SEG_STATE_ESTOP: return "E-STOP";
default: return "UNKNOWN";
}
}
void SegFsm_Update(const RawCmd_t *raw_cmd,
const CorridorObs_t *obs,
const CorridorState_t *state,
SegFsmOutput_t *out)
{
if (!s_initialized) {
out->state = SEG_STATE_IDLE;
out->safe_v = 0.0f;
out->safe_w = 0.0f;
return;
}
/* ========================================================
* 1. 全局安全断言：无论在什么状态，满足条件直接超驰
* ======================================================== */
/* 1a. 置信度极低 → 紧急停车 */
if (state->conf < s_cfg.conf_estop_thresh) {
s_state = SEG_STATE_ESTOP;
}
/* ========================================================
* 2. 待命状态：什么都不做
* ======================================================== */
if (s_state == SEG_STATE_IDLE) {
out->state = SEG_STATE_IDLE;
out->safe_v = 0.0f;
out->safe_w = 0.0f;
return;
}
/* ========================================================
* 3. 紧急停车状态：等待置信度恢复
* ======================================================== */
if (s_state == SEG_STATE_ESTOP) {
out->state = SEG_STATE_ESTOP;
out->safe_v = 0.0f;
out->safe_w = 0.0f;
/* 自动恢复开关：当置信度重新回到安全区且前方有路，恢复巡航 */
if (state->conf >= 0.5f) {
s_state = SEG_STATE_CORRIDOR;
}
return;
}
/* ========================================================
* 4. 前向距离判定：决定 CORRIDOR / APPROACH / STOP
* ======================================================== */
bool front_valid = (obs->valid_mask & CORRIDOR_OBS_MASK_FRONT) != 0U;
float d_front = obs->d_front;
if (front_valid) {
if (d_front <= s_cfg.d_front_stop) {
/* 距离过近：到端停车 */
s_state = SEG_STATE_STOP;
} else if (d_front <= s_cfg.d_front_approach) {
/* 进入减速预警区 */
s_state = SEG_STATE_APPROACH;
} else {
/* 前方安全：正常走廊跟踪 */
if (s_state == SEG_STATE_STOP || s_state == SEG_STATE_APPROACH) {
s_state = SEG_STATE_CORRIDOR;
}
}
} else {
/* 前向雷达失效：保守策略，保持当前状态但降速
* (不往 STOP 跳，避免前雷达偶发掉线就急停) */
}
/* ========================================================
* 5. 根据当前状态输出最终安全速度
* ======================================================== */
switch (s_state) {
case SEG_STATE_CORRIDOR: {
/* 正常放行控制器输出 */
out->safe_v = raw_cmd->v;
out->safe_w = raw_cmd->w;
break;
}
case SEG_STATE_APPROACH: {
/* 线性插值减速:
* 当 d_front 从 approach 距离滑向 stop 距离时，
* 速度从 raw_cmd->v 线性衰减至 approach_min_v
*/
float range = s_cfg.d_front_approach - s_cfg.d_front_stop;
float ratio = 1.0f;
if (range > 0.001f) {
ratio = (d_front - s_cfg.d_front_stop) / range;
ratio = seg_clampf(ratio, 0.0f, 1.0f);
}
float v_scaled = s_cfg.approach_min_v + ratio * (raw_cmd->v - s_cfg.approach_min_v);
out->safe_v = seg_clampf(v_scaled, 0.0f, raw_cmd->v);
out->safe_w = raw_cmd->w; // 角速度不限，仍然允许纠偏
break;
}
case SEG_STATE_STOP:
default: {
/* 到端停车 / 紧急停车：零速 */
out->safe_v = 0.0f;
out->safe_w = 0.0f;
break;
}
}
out->state = s_state;
}

78
App/nav/segment_fsm.h Normal file
View File

@@ -0,0 +1,78 @@
#ifndef SEGMENT_FSM_H
#define SEGMENT_FSM_H
#include "preproc/corridor_msgs.h"
#include "preproc/corridor_preproc.h"
/**
* @brief 段状态机的运行阶段枚举
*/
typedef enum {
SEG_STATE_IDLE = 0, // 待命：不输出任何指令
SEG_STATE_CORRIDOR, // 走廊跟踪中：放行控制器输出
SEG_STATE_APPROACH, // 接近端墙：降速保护
SEG_STATE_STOP, // 到端停车：强制零速
SEG_STATE_ESTOP, // 紧急停车：传感器全部失效或异常
} SegFsmState_t;
/**
* @brief 段状态机配置参数
*/
typedef struct {
float d_front_stop; // 前向停车距离阈值 (m)，低于此值直接停车
float d_front_approach; // 前向减速预警距离 (m)，低于此值开始线性减速
float approach_min_v; // 减速区内最低速度 (m/s)，防止爬行太慢永远到不了
float conf_estop_thresh; // 置信度紧急停车阈值，低于此值触发 E-Stop
} SegFsmConfig_t;
/**
* @brief 段状态机对外暴露的完整快照 (供日志或上位机读取)
*/
typedef struct {
SegFsmState_t state; // 当前状态
float safe_v; // 经安全仲裁后的最终线速度 (m/s)
float safe_w; // 经安全仲裁后的最终角速度 (rad/s)
} SegFsmOutput_t;
#ifdef __cplusplus
extern "C" {
#endif
/**
* @brief 初始化段状态机
* @param config 距离阈值与降级参数
*/
void SegFsm_Init(const SegFsmConfig_t *config);
/**
* @brief 启动走廊跟踪（从 IDLE 切换到 CORRIDOR）
*/
void SegFsm_Start(void);
/**
* @brief 核心函数：输入控制器的期望指令，输出安全仲裁后的最终指令
* @param raw_cmd 走廊控制器的原始输出 (v, w)
* @param obs 预处理层的观测快照 (提供 d_front 和 valid_mask)
* @param state 滤波器的状态输出 (提供 conf 置信度)
* @param out 安全仲裁后的最终输出
*/
void SegFsm_Update(const RawCmd_t *raw_cmd,
const CorridorObs_t *obs,
const CorridorState_t *state,
SegFsmOutput_t *out);
/**
* @brief 获取当前状态机状态 (用于日志打印)
*/
SegFsmState_t SegFsm_GetState(void);
/**
* @brief 获取状态名称字符串 (用于日志打印)
*/
const char* SegFsm_GetStateName(SegFsmState_t s);
#ifdef __cplusplus
}
#endif
#endif // SEGMENT_FSM_H

58
App/preproc/corridor_msgs.h Normal file
View File

@@ -0,0 +1,58 @@
#ifndef CORRIDOR_MSGS_H
#define CORRIDOR_MSGS_H
#include <stdint.h>
#include <stdbool.h>
/* =========================================================
* EKF 状态维度定义
* ========================================================= */
#define EKF_STATE_DIM 3 // [e_y, e_th, s]
#define EKF_OBS_DIM 3 // [z_ey, z_eth_L, z_eth_R]
/* χ² 检验门限 (95% 置信度) */
/* 1 DOF: 3.84, 2 DOF: 5.99, 3 DOF: 7.81 */
#define CHI2_THRESHOLD_1DOF 3.84f
#define CHI2_THRESHOLD_2DOF 5.99f
#define CHI2_THRESHOLD_3DOF 7.81f
/**
* @brief 走廊观测快照 (由 Blackboard 数据转化清洗而来)
* @note 所有距离单位统一为 米 (m)
*/
typedef struct {
uint32_t t_ms; // 观测时间戳
float d_lf, d_lr; // 左侧前后距离 (m)
float d_rf, d_rr; // 右侧前后距离 (m)
float d_front, d_back; // 前后防撞/到端距离 (m)
uint8_t valid_mask; // 位域掩码：标记哪些雷达数据是当前存活且合法的
} CorridorObs_t;
/**
* @brief 走廊相对定位状态 (EKF 的输出结果)
*/
typedef struct {
uint32_t t_ms; // 状态更新时间戳
float e_y; // 横向偏差 (m)，向左偏为正
float e_th; // 航向偏差 (rad)，车头偏左为正
float s; // 沿走廊进度里程 (m)
float conf; // 置信度/健康度 (0.0~1.0)，用于触发降级
/* EKF 扩展输出 */
float P[EKF_STATE_DIM][EKF_STATE_DIM]; // 状态协方差矩阵
float innovation[EKF_OBS_DIM]; // 新息向量 (观测残差)
float mahalanobis_d2; // 马氏距离平方
uint8_t obs_reject_mask; // 被拒绝的观测位掩码
} CorridorState_t;
/**
* @brief 纯控制指令 (准备交给安全层仲裁)
*/
typedef struct {
uint32_t t_ms;
float v; // 期望线速度 (m/s)
float w; // 期望角速度 (rad/s)
uint8_t flags; // 控制特殊标志位
} RawCmd_t;
#endif // CORRIDOR_MSGS_H

119
App/preproc/corridor_preproc.c Normal file
View File

@@ -0,0 +1,119 @@
#include "corridor_preproc.h"
/**
* @brief 内部辅助函数：处理单个侧向 VL53L0X 测距
* @return true 表示数据合法且在量程内，false 表示异常或超量程飞点
*/
static bool process_side_laser(const SensorItem_t *item, float *out_m)
{
// 驱动层报异常，直接抛弃
if (!item->is_valid) {
return false;
}
// 毫米转米
float dist_m = item->value / 1000.0f;
// 剔除硬件量程外的死区或异常噪点
if (dist_m > PREPROC_MAX_SIDE_RANGE_M || dist_m < PREPROC_MIN_SIDE_RANGE_M) {
return false;
}
*out_m = dist_m;
return true;
}
/**
* @brief 内部辅助函数：处理前后向互补雷达 (STP 远距离 + ATK 近盲区)
* @note 完美解决 STP-23L 的 7cm 物理盲区问题，防止“幽灵撞墙”
*/
static bool process_complementary_laser(const SensorItem_t *stp, const SensorItem_t *atk, float *out_m)
{
bool stp_ok = stp->is_valid && (stp->value > 0.0f);
bool atk_ok = atk->is_valid && (atk->value > 0.0f);
float stp_m = stp->value / 1000.0f;
float atk_m = atk->value / 1000.0f;
/* 1. 近距离补盲优先：如果 ATK 存活，且测距进入 8cm 内 (7cm盲区+1cm裕量) */
/* 此时 STP 的数据大概率是垃圾数据（卡死在7cm或报错），无条件信任 ATK */
if (atk_ok && atk_m <= PREPROC_BLIND_ZONE_M) {
*out_m = atk_m;
return true;
}
/* 2. 正常中远距离：两者都存活，取保守值防撞 */
if (stp_ok && atk_ok) {
// 如果 STP 读数等于 7cm 极限值，它可能已经瞎了，直接用 ATK 的值
if (stp_m <= 0.07f) {
*out_m = atk_m;
} else {
*out_m = (stp_m < atk_m) ? stp_m : atk_m; // 正常情况取两者中更近的
}
return true;
}
/* 3. 硬件单体降级：只剩下一个雷达活着 */
if (stp_ok) {
// 只有 STP 活着，但读数在盲区内。这数据极其危险（骗人的），宁可报失效触发急停！
if (stp_m <= 0.07f) {
return false;
}
*out_m = stp_m;
return true;
}
if (atk_ok) {
*out_m = atk_m;
return true;
}
/* 全挂了，通知上层失明 */
return false;
}
void CorridorPreproc_ExtractObs(const RobotBlackboard_t *board, uint32_t now_ms, CorridorObs_t *out_obs)
{
// 初始化输出结构体，清空掩码
out_obs->t_ms = now_ms;
out_obs->valid_mask = 0U;
out_obs->d_lf = 0.0f;
out_obs->d_lr = 0.0f;
out_obs->d_rf = 0.0f;
out_obs->d_rr = 0.0f;
out_obs->d_front = 0.0f;
out_obs->d_back = 0.0f;
/* --- 1. 独立清洗侧向 4 个 VL53L0X 数据 --- */
if (process_side_laser(&board->dist_left_f, &out_obs->d_lf)) {
out_obs->valid_mask |= CORRIDOR_OBS_MASK_LF;
}
if (process_side_laser(&board->dist_left_r, &out_obs->d_lr)) {
out_obs->valid_mask |= CORRIDOR_OBS_MASK_LR;
}
if (process_side_laser(&board->dist_right_f, &out_obs->d_rf)) {
out_obs->valid_mask |= CORRIDOR_OBS_MASK_RF;
}
if (process_side_laser(&board->dist_right_r, &out_obs->d_rr)) {
out_obs->valid_mask |= CORRIDOR_OBS_MASK_RR;
}
/* --- 2. 前向雷达：远近互补融合 --- */
if (process_complementary_laser(&board->dist_front_stp, &board->dist_front_atk, &out_obs->d_front)) {
/* 补偿传感器内缩：将"传感器到墙"转换为"车体前端到墙"
* d_body = d_sensor - front_laser_offset
* 当 offset > 0 时表示传感器在车体内部，车体前端比传感器更接近墙 */
out_obs->d_front -= PARAM_FRONT_LASER_OFFSET;
if (out_obs->d_front < 0.0f) out_obs->d_front = 0.0f; /* 安全下限 */
out_obs->valid_mask |= CORRIDOR_OBS_MASK_FRONT;
}
/* --- 3. 后向雷达：远近互补融合 --- */
if (process_complementary_laser(&board->dist_rear_stp, &board->dist_rear_atk, &out_obs->d_back)) {
/* 补偿传感器内缩：同前向逻辑 */
out_obs->d_back -= PARAM_REAR_LASER_OFFSET;
if (out_obs->d_back < 0.0f) out_obs->d_back = 0.0f;
out_obs->valid_mask |= CORRIDOR_OBS_MASK_BACK;
}
}

43
App/preproc/corridor_preproc.h Normal file
View File

@@ -0,0 +1,43 @@
#ifndef CORRIDOR_PREPROC_H
#define CORRIDOR_PREPROC_H
#include <stdint.h>
#include <stdbool.h>
#include "Contract/robot_blackboard.h"
#include "corridor_msgs.h"
/* 传感器有效性掩码定义 (对应 CorridorObs_t.valid_mask) */
#define CORRIDOR_OBS_MASK_LF (1U << 0) // 左前有效
#define CORRIDOR_OBS_MASK_LR (1U << 1) // 左后有效
#define CORRIDOR_OBS_MASK_RF (1U << 2) // 右前有效
#define CORRIDOR_OBS_MASK_RR (1U << 3) // 右后有效
#define CORRIDOR_OBS_MASK_FRONT (1U << 4) // 前方互补测距有效
#define CORRIDOR_OBS_MASK_BACK (1U << 5) // 后方互补测距有效
/* VL53L0X 侧向雷达的物理有效探测区间 (m) */
#define PREPROC_MAX_SIDE_RANGE_M 2.0f
#define PREPROC_MIN_SIDE_RANGE_M 0.02f
/* 前后向雷达近战盲区阈值 (m) (STP 7cm盲区 + 1cm工程裕量) */
#define PREPROC_BLIND_ZONE_M 0.08f
/* 前后激光传感器内缩补偿 (从 robot_params.h 引入) */
#include "robot_params.h"
#ifdef __cplusplus
extern "C" {
#endif
/**
* @brief 从黑板快照中提取、仲裁并清洗走廊观测数据
* @param board 输入的黑板数据快照
* @param now_ms 当前的系统时间戳
* @param out_obs 输出的清洗后观测值 (仅保留合法数据，单位统一为 m)
*/
void CorridorPreproc_ExtractObs(const RobotBlackboard_t *board, uint32_t now_ms, CorridorObs_t *out_obs);
#ifdef __cplusplus
}
#endif
#endif // CORRIDOR_PREPROC_H

99
App/retarget.c Normal file
View File

@@ -0,0 +1,99 @@
#include "retarget.h"
#include <stdint.h>
#include <stdio.h>
#include "cmsis_os.h"
#include "usbd_cdc_if.h"
/*
* 说明：
* 1. 这里默认使用 FreeRTOS 的 CMSIS-RTOS2 接口
* 2. printf 最终会走到 _write()
* 3. _write() 内部调用 CDC_Transmit_FS() 发送到 USB 虚拟串口
* 4. 为避免多个任务同时 printf 打花，这里提供互斥锁
*/
static osMutexId_t s_retarget_mutex = NULL;
static uint8_t s_inited = 0U;
/* 互斥锁属性 */
static const osMutexAttr_t s_retarget_mutex_attr = {
.name = "retargetMutex"
};
void Retarget_Init(void)
{
if (s_inited != 0U)
{
return;
}
/* 只有在内核启动后创建互斥量才有意义 */
if (osKernelGetState() == osKernelRunning)
{
s_retarget_mutex = osMutexNew(&s_retarget_mutex_attr);
}
s_inited = 1U;
}
void Retarget_Lock(void)
{
if ((s_retarget_mutex != NULL) && (osKernelGetState() == osKernelRunning))
{
(void)osMutexAcquire(s_retarget_mutex, osWaitForever);
}
}
void Retarget_Unlock(void)
{
if ((s_retarget_mutex != NULL) && (osKernelGetState() == osKernelRunning))
{
(void)osMutexRelease(s_retarget_mutex);
}
}
/*
* GNU 工具链 printf 重定向
* 注意：
* - 不要在中断里调用 printf
* - USB CDC 忙时最多等待 20ms
* - 等待期间 osDelay(1) 让出 CPU
*/
int _write(int file, char *ptr, int len)
{
uint32_t start_tick;
uint8_t result;
(void)file;
if ((ptr == NULL) || (len <= 0))
{
return 0;
}
start_tick = HAL_GetTick();
while (1)
{
result = CDC_Transmit_FS((uint8_t *)ptr, (uint16_t)len);
if (result == USBD_OK)
{
return len;
}
/* USB 还没准备好或者正忙，最多等 20ms */
if ((HAL_GetTick() - start_tick) > 20U)
{
return 0;
}
/* 在 FreeRTOS 下主动让出 CPU */
if (osKernelGetState() == osKernelRunning)
{
osDelay(1);
}
}
}

21
App/retarget.h Normal file
View File

@@ -0,0 +1,21 @@
#ifndef __RETARGET_H
#define __RETARGET_H
#ifdef __cplusplus
extern "C" {
#endif
/* 初始化日志输出模块 */
void Retarget_Init(void);
/* 日志输出加锁 */
void Retarget_Lock(void);
/* 日志输出解锁 */
void Retarget_Unlock(void);
#ifdef __cplusplus
}
#endif
#endif

406
App/robot_params.h Normal file
View File

@@ -0,0 +1,406 @@
/**
* @file robot_params.h
* @brief ARES 机器人全局参数配置中心
*
* ============================================
* 实车调参前必读
* ============================================
* 1. 所有长度单位默认为 米 (m)，角度为 弧度 (rad)，速度为 m/s
* 2. 带"实测"标记的参数必须用卷尺/编码器实际测量后填写
* 3. 带"调优"标记的参数需要根据实车运行效果从小往大调整
* 4. 修改本文件后需要重新编译烧录
*
* 调参顺序建议:
* P0: 几何参数 (传感器位置、轮子尺寸) -> 必须实测
* P1: 里程计参数 -> 实车跑 10 米对比编码器/实际距离
* P2: 走廊滤波器 -> 从保守值开始逐步调优
* P3: 控制器增益 -> 实车走廊测试，从低增益开始
* P4: 安全阈值 -> 根据实际场地微调
*/
#ifndef ROBOT_PARAMS_H
#define ROBOT_PARAMS_H
#include <stdint.h>
#include <stdbool.h>
#ifdef __cplusplus
extern "C" {
#endif
/* =========================================================
* 【P0】机械/几何参数 (必须实测，开机前填写!)
* ========================================================= */
/* ------------------- 车体几何 ------------------- */
/** @brief [实测] 车体外轮廓宽度 [m] (左右方向，含外壳)
* 测量方法：卷尺测量车体最宽处
* 典型值：20cm -> 0.20
* 用途：侧向安全裕量计算，传感器偏移补偿
*/
#define PARAM_ROBOT_WIDTH 0.200f
/** @brief [实测] 车体外轮廓长度 [m] (前后方向，含外壳)
* 测量方法：卷尺测量车体最长处
* 典型值：20cm -> 0.20
* 用途：前后安全距离补偿
*/
#define PARAM_ROBOT_LENGTH 0.200f
/** @brief [实测] 驱动轮标称直径 [m]
* 测量方法：卷尺测量轮胎外径 (非轮毂)
* 典型值：60mm 轮胎 -> 0.060
*/
#define PARAM_WHEEL_DIAMETER 0.080f
/** @brief [实测] 左右轮中心距 [m]
* 测量方法：卷尺测量左右驱动轮中心之间的距离
* 典型值：车体 20cm 宽，轮胎每边厚 3cm -> 约 0.14m
*/
#define PARAM_WHEEL_TRACK 0.140f
/** @brief [实测] 同侧前后 VL53L0X 雷达纵向间距 [m]
* 测量方法：卷尺测量同侧前后两个 VL53L0X 的中心距离
* 典型值：根据机械安装，约 0.10~0.15m
* 影响：航向观测精度 z_eth = atan2((d_back-d_front), L_s)
*/
#define PARAM_SENSOR_BASE_LENGTH 0.120f
/** @brief [实测] 比赛走廊标准宽度 [m]
* 测量方法：卷尺测量赛场垄沟实际宽度 (规则 40cm，允许±5% 误差)
* 典型值：0.40 (但建议实测后填写)
*/
#define PARAM_CORRIDOR_WIDTH 0.40f
/** @brief [实测] 前向激光雷达安装中心偏置 [m]
* 测量方法：雷达发射面到车头最前端的距离 (内缩为正)
* 典型值：根据机械安装，若与车头齐平则为 0
* 用途：前向到端距离补偿 d_body_front = d_sensor - offset
* 注意：这里定义的是传感器到车头前缘的距离，不是到几何中心
*/
#define PARAM_FRONT_LASER_OFFSET 0.0f
/** @brief [实测] 后向激光雷达安装中心偏置 [m]
* 测量方法：雷达发射面到车尾最后端的距离 (内缩为正)
* 典型值：根据机械安装，若与车尾齐平则为 0
* 用途：后向到端距离补偿 d_body_rear = d_sensor - offset
*/
#define PARAM_REAR_LASER_OFFSET 0.0f
/** @brief [实测] 侧向 VL53L0X 传感器内缩距离 [m]
* 测量方法：VL53L0X 发射面到车体侧面最外边缘的距离 (向内为正)
* 典型值：传感器与外壳齐平则为 0，嵌入 1cm 则为 0.01
* 用途：侧向测距补偿，d_actual_to_wall = d_sensor - sensor_inset
* 即传感器测到的距离减去这个内缩量才是车体边缘到墙的真实距离
* ⚠ 极其重要：沟道40cm - 车体20cm = 每边仅10cm，1cm的偏差就是10%的误差！
*/
#define PARAM_VL53_SIDE_INSET 0.0f
/* ------------------- 编码器参数 ------------------- */
/** @brief [实测] 编码器每转脉冲数 (CPR)
* 测量方法：查阅 F407 固件配置，或实车转动一圈数脉冲
* 典型值：500 (F407 默认固件)
*/
#define PARAM_ENCODER_CPR 500
/* =========================================================
* 【P1】里程计参数 (实车标定)
* ========================================================= */
/** @brief [调优] 里程计过程噪声 [m²/s²]
* 含义：对里程计信任程度，越小越信任
* 调优方法：实车直线跑 10 米，对比 EKF 输出的 s 与实际距离
* 典型值：0.01~0.1
* 过大：EKF 过度依赖里程计，轮滑时误差累积
* 过小：EKF 不信任里程计，沿程 s 估计不准
*/
#define PARAM_ODOM_PROCESS_NOISE 0.1f
/** @brief [调优] 里程计速度低通滤波系数
* 含义：alpha=0.8 表示新采样占 20%，历史值占 80%
* 典型值：0.7~0.9
*/
#define PARAM_ODOM_LOWPASS_ALPHA 0.8f
/* =========================================================
* 【P2】走廊 EKF 滤波器参数 (调优)
* ========================================================= */
/* --- EKF 过程噪声 Q (对模型信任度) --- */
/** @brief [调优] 横向偏差 e_y 过程噪声方差 [m²]
* 含义：横向状态的自然发散速度，越小表示横向越稳定
* 调优方法：从 0.001 开始，逐步增大直到走廊跟踪平滑
* 典型值：0.001~0.02
* 过大：EKF 认为横向很不稳定，过度依赖观测 -> 震荡
* 过小：EKF 不信任观测，响应慢
*/
#define PARAM_EKF_Q_EY 0.01f
/** @brief [调优] 航向偏差 e_th 过程噪声方差 [rad²]
* 含义：航向状态的自然发散速度
* 调优方法：IMU 短时漂移小，可以设很小
* 典型值：0.0001~0.005
* 过大：EKF 认为航向不稳定，IMU 作用被削弱
* 过小：EKF 过度信任 IMU，雷达观测不起作用
*/
#define PARAM_EKF_Q_ETH 0.001f
/** @brief [调优] 沿程进度 s 过程噪声方差 [m²]
* 含义：里程估计的不确定度
* 调优方法：地毯轮滑严重时调大
* 典型值：0.05~0.2
*/
#define PARAM_EKF_Q_S 0.1f
/* --- EKF 观测噪声 R (对传感器信任度) --- */
/** @brief [调优] 横向观测噪声方差 [m²]
* 含义：VL53L0X 测距的可靠程度
* 调优方法：从 0.001 开始，根据走廊跟踪平滑度调整
* 典型值：0.001~0.005
* 过大：EKF 不信任侧向雷达，横向响应慢
* 过小：EKF 过度信任雷达，对跳变敏感
*/
#define PARAM_EKF_R_EY 0.002f
/** @brief [调优] 航向观测噪声方差 [rad²]
* 含义：同侧前后雷达差分测角的可程靠度
* 调优方法：从 0.001 开始
* 典型值：0.0005~0.003
*/
#define PARAM_EKF_R_ETH 0.001f
/** @brief [调优] IMU 航向观测噪声方差 [rad²]
* 含义：IMU yaw 作为 EKF 航向观测时的噪声
* 设计意图：IMU yaw 用于长时航向约束 (防止 wz 积分漂移)，
* R 值应明显大于侧墙航向观测 R_ETH，避免干扰侧墙短时精确修正
* 调优方法：从 0.01 开始，如果 IMU yaw 非常稳定可减小
* 典型值：0.005~0.05
* 过大：IMU 航向观测作用弱，等于没接入
* 过小：IMU 航向主导，侧墙观测被压制
*/
#define PARAM_EKF_R_ETH_IMU 0.01f
/* --- EKF 初始状态 --- */
/** @brief [调优] e_y 初始不确定度 [m²]
* 含义：开机时横向位置的先验不确定度
* 典型值：0.01~0.1 (对应 10~30cm 不确定)
*/
#define PARAM_EKF_P0_EY 0.1f
/** @brief [调优] e_th 初始不确定度 [rad²]
* 含义：开机时航向的先验不确定度
* 典型值：0.05~0.2 (对应 13°~26°不确定)
*/
#define PARAM_EKF_P0_ETH 0.1f
/* --- χ² 检验门限 (鲁棒拒绝) --- */
/** @brief [固定] 1 自由度 χ² 检验门限 (95% 置信度)
* 含义：单路观测的马氏距离超过此值则拒绝
* 理论值：3.84 (95%), 6.63 (99%)
* 建议：不要修改，除非传感器质量极差
*/
#define PARAM_CHI2_1DOF 3.84f
/** @brief [固定] 2 自由度 χ² 检验门限 (95% 置信度)
* 理论值：5.99
*/
#define PARAM_CHI2_2DOF 5.99f
/* =========================================================
* 【P3】走廊控制器参数 (实车调优)
* ========================================================= */
/** @brief [调优] 航向比例增益 kp_theta [1/s]
* 含义：航向偏差 1 rad 时产生的角速度
* 调优方法：从 0.5 开始逐步增加，直到走廊跟踪响应快但不震荡
* 典型值：1.0~3.0
* 过大：车头左右摆动 (震荡)
* 过小：纠偏太慢，容易撞墙
*/
#define PARAM_CTRL_KP_THETA 2.0f
/** @brief [调优] 航向微分增益 kd_theta [s]
* 含义：IMU 角速度阻尼项，抑制车头转动速度
* 调优方法：从 0.05 开始
* 典型值：0.05~0.2
* 过大：车头转动被过度抑制，响应迟钝
* 过小：阻尼不足，容易超调震荡
*/
#define PARAM_CTRL_KD_THETA 0.1f
/** @brief [调优] 横向比例增益 kp_y [1/(m·s)]
* 含义：横向偏差 1m 时产生的角速度修正
* 调优方法：从 1.0 开始
* 典型值：1.5~4.0
* 过大：横向纠偏过猛，引起震荡
* 过小：偏了拉不回来
*/
#define PARAM_CTRL_KP_Y 3.0f
/** @brief [调优] 走廊巡航速度 [m/s]
* 含义：走廊内正常行驶速度
* 调优方法：从 0.1 m/s 开始逐步增加
* 典型值：0.15~0.4 m/s
* 注意：速度越高，对滤波器带宽和控制器增益要求越高
*/
#define PARAM_CTRL_V_CRUISE 0.15f
/** @brief [调优] 角速度输出限幅 [rad/s]
* 含义：最大允许转角速度
* 典型值：1.0~2.0 (约 57°/s ~ 115°/s)
*/
#define PARAM_CTRL_W_MAX 1.5f
/** @brief [调优] 线速度输出限幅 [m/s]
* 含义：最大允许线速度
* 典型值：0.3~0.5
*/
#define PARAM_CTRL_V_MAX 0.3f
/** @brief [调优] 弯道减速系数 [0~1]
* 含义：角速度大时线速度打折程度
* 公式：v = v_cruise * (1 - k * |w/w_max|)
* 典型值：0.3~0.5
*/
#define PARAM_CTRL_SPEED_REDUCTION 0.4f
/* =========================================================
* 【P4】安全阈值与状态机参数
* ========================================================= */
/* --- 到端检测 --- */
/** @brief [调优] 前向停车距离阈值 [m]
* 含义：前向雷达 (传感器到墙的距离) 低于此值强制停车
* 计算方法：PARAM_FRONT_LASER_OFFSET (传感器内缩) + 安全裕量 (2~5cm)
* 示例：传感器与车头齐平(offset=0) + 3cm安全裕量 = 0.03m
* 传感器内缩5cm(offset=0.05) + 3cm安全裕量 = 0.08m
* ⚠ 此阈值应 >= PARAM_FRONT_LASER_OFFSET，否则传感器到墙的距离
* 还没到阈值，车体前端就已经撞墙了！
* 过小：可能撞墙
* 过大：离墙很远就停车，走不完走廊
*/
#define PARAM_SAFE_D_FRONT_STOP 0.08f
/** @brief [调优] 前向减速预警距离 [m]
* 含义：前向雷达低于此值开始线性减速
* 典型值：0.20~0.40
*/
#define PARAM_SAFE_D_FRONT_APPROACH 0.25f
/** @brief [调优] 减速区最低速度 [m/s]
* 含义：到端前爬行速度
* 典型值：0.03~0.08
*/
#define PARAM_SAFE_APPROACH_MIN_V 0.05f
/* --- 置信度降级 --- */
/** @brief [调优] E-Stop 置信度阈值
* 含义：EKF 置信度低于此值触发紧急停车
* 典型值：0.05~0.15
*/
#define PARAM_SAFE_CONF_ESTOP 0.1f
/** @brief [调优] 单侧退化置信度
* 含义：单侧 VL53L0X 失效时，置信度降为多少
* 典型值：0.6~0.8
*/
#define PARAM_SAFE_CONF_DEGRADED 0.7f
/* --- 入口/退出 --- */
/** @brief [调优] 入口对准速度 [m/s]
* 含义：从启动区进入走廊的慢速对准速度
* 典型值：0.05~0.10
*/
#define PARAM_SCRIPT_ENTRY_V 0.08f
/** @brief [调优] 入口对准超时 [ms]
* 含义：超过此时间强制进入走廊模式 (保护)
* 典型值：20000~40000 (20~40 秒)
*/
#define PARAM_SCRIPT_ENTRY_TIMEOUT 30000U
/** @brief [调优] 原地转向角速度 [rad/s]
* 含义：到端 180 度转向的角速度
* 典型值：0.8~1.5
*/
#define PARAM_SCRIPT_TURN_OMEGA 1.0f
/** @brief [调优] 退出场地后冲距离 [m]
* 含义：检测到离开垄沟后再往前冲的距离
* 典型值：1.5~2.5
*/
#define PARAM_SCRIPT_EXIT_RUNOUT 2.0f
/** @brief [调优] 退出场地直线冲出速度 [m/s]
* 含义：检测到离开垄沟后直线冲出的速度
* 典型值：0.3~0.5
* 注意：与 turn_omega (转向角速度) 无关，两者独立
*/
#define PARAM_SCRIPT_EXIT_V 0.3f
/* =========================================================
* 【P5】传感器驱动参数
* ========================================================= */
/* --- VL53L0X --- */
/** @brief [实测] VL53L0X 测距预算 [μs]
* 含义：单次测距的时间预算，越大精度越高但频率越低
* 选项：20000(20ms, 高速), 33000(33ms), 100000(100ms), 200000(200ms, 高精度)
* 典型值：100000 (100ms，折中)
* 调优：走廊跟踪时用 100ms，原地转向时可切 200ms 提高精度
*/
#define PARAM_VL53_TIMING_BUDGET 100000U
/** @brief [调优] VL53L0X 卡尔曼滤波参数 Q
* 含义：测距过程噪声
* 典型值：5.0~20.0
*/
#define PARAM_VL53_KALMAN_Q 10.0f
/** @brief [调优] VL53L0X 卡尔曼滤波参数 R
* 含义：测距观测噪声
* 典型值：10.0~30.0
*/
#define PARAM_VL53_KALMAN_R 14.1f
/* --- IMU --- */
/** @brief [实测] HWT101 IMU 安装偏置 (航向) [rad]
* 含义：IMU 零位与车头方向的偏差
* 测量：车头对准正北，读取 IMU 航向作为偏置
* 典型值：需要通过校准确定
*/
#define PARAM_IMU_YAW_OFFSET 0.0f
/* =========================================================
* 宏工具函数
* ========================================================= */
#define PARAM_RAD2DEG(rad) ((rad) * 57.2957795131f)
#define PARAM_DEG2RAD(deg) ((deg) * 0.01745329252f)
/* =========================================================
* 快速参考表
* =========================================================
*
* 调参流程:
* 1. 实测几何参数 (P0 组) -> 卷尺测量
* 2. 里程计标定 (P1 组) -> 跑 10 米对比
* 3. 原地测试控制器 -> 只调 kp_theta, kd_theta
* 4. 走廊低速测试 -> 调 kp_y, v_cruise
* 5. 逐步提速 -> 微调各增益
* 6. 安全阈值 -> 根据实际场地
*
* 常见问题诊断:
* 车头左右摆 -> 减小 kp_theta 或增大 kd_theta
* 横向纠偏慢 -> 增大 kp_y
* 到端刹车不住 -> 减小 d_front_stop 或增大 approach 距离
* 出口冲不出 -> 减小 exit_runout 或增大 v_cruise
* EKF 发散 -> 增大 Q 或减小 R
*
* =========================================================
*/
#ifdef __cplusplus
}
#endif
#endif /* ROBOT_PARAMS_H */