76 lines
2.8 KiB
C
76 lines
2.8 KiB
C
#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]
|
|
|
|
/* [改进I] 观测维度 (与实际实现对齐):
|
|
* 侧墙观测: 1DOF 标量更新 (z_ey)
|
|
* IMU 航向: 独立 1DOF 标量更新 (z_eth_imu)，在 Update 后单独调用
|
|
* 侧墙航向 (z_eth_L/z_eth_R) 已取消 — VL53 差分噪声过大不适合做航向源
|
|
* 旧值 3 对应已取消的 [z_ey, z_eth_L, z_eth_R] 三维观测
|
|
*/
|
|
#define EKF_OBS_DIM 1 // 实际: 侧墙 1DOF (z_ey)
|
|
|
|
/* χ² 检验门限 (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;
|
|
|
|
/* =========================================================
|
|
* 安全模式枚举 (赛道级导航 → 安全状态机 的模式指示)
|
|
* ========================================================= */
|
|
typedef enum {
|
|
SAFETY_MODE_IDLE = 0, // 零速，不做任何裁剪
|
|
SAFETY_MODE_CORRIDOR, // 沟内: 前向减速/停车/E-STOP 全开
|
|
SAFETY_MODE_TURN, // 转向: 允许 v=0+w!=0, 前向不全停, 不检查 conf
|
|
SAFETY_MODE_STRAIGHT // 直行段: 前后激光防撞, 不检查 conf
|
|
} SafetyMode_t;
|
|
|
|
#endif // CORRIDOR_MSGS_H
|