288 lines
9.3 KiB
C
288 lines
9.3 KiB
C
|
/**
|
|||
|
|
* @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);
|
|||
|
|
}
|