116 lines
4.5 KiB
C
116 lines
4.5 KiB
C
|
/**
|
|||
|
|
* @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 */
|