ASER/App/preproc/corridor_msgs.h

#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