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-04-04 14:49:33 +08:00
parent 7afa32f7dc
commit 4d0c531344
3 changed files with 12 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
App/app_tasks.c
View File

@@ -344,7 +344,7 @@ void AppTasks_RunNavTask_Impl(void *argument)
/* --- Step 4: 赛道级导航 --- */ /* --- Step 4: 赛道级导航 --- */
GlobalNavOutput_t nav_out; GlobalNavOutput_t nav_out;
GlobalNav_Update(&obs, &corridor_state, &board, &nav_out); GlobalNav_Update(&obs, &corridor_state, &board, now_ms, &nav_out);
/* --- Step 5: 控制律 --- */ /* --- Step 5: 控制律 --- */
RawCmd_t raw_cmd; RawCmd_t raw_cmd;

16
App/nav/global_nav.c
View File

@@ -415,6 +415,7 @@ const char* GlobalNav_GetStageName(GlobalNavStage_t stage)
void GlobalNav_Update(const CorridorObs_t* obs, void GlobalNav_Update(const CorridorObs_t* obs,
const CorridorState_t* state, const CorridorState_t* state,
const RobotBlackboard_t* board, const RobotBlackboard_t* board,
uint32_t now_ms,
GlobalNavOutput_t* out) GlobalNavOutput_t* out)
{ {
if (!s_nav.initialized) { if (!s_nav.initialized) {
@@ -422,23 +423,16 @@ void GlobalNav_Update(const CorridorObs_t* obs,
return; return;
} }
uint32_t now_ms = s_last_update_ms; /* 里程计积分: Δd = vx * dt
/* 用 HAL_GetTick 代理 — 实际上 app_tasks 会传入 board 里的时间 */ * now_ms 由调用方传入 (HAL_GetTick)，与任何传感器时间戳无关，
* 避免 IMU 时间戳停更时里程和超时双双冻结 (原 TODO-1 修复) */
{ {
/* 里程计积分: Δd = vx * dt */
float odom_vx = board->odom_vx; float odom_vx = board->odom_vx;
if (s_last_update_ms > 0) { if (s_last_update_ms > 0) {
/* 使用 board 中的时间戳估计当前时间 */ float dt = (float)(now_ms - s_last_update_ms) * 0.001f;
uint32_t cur_ms = board->imu_wz.timestamp_ms;
if (cur_ms == 0) cur_ms = s_last_update_ms + 20U;
float dt = (float)(cur_ms - s_last_update_ms) * 0.001f;
if (dt > 0.0f && dt < 0.5f) { if (dt > 0.0f && dt < 0.5f) {
s_nav.odom_distance_accum += gnav_fabsf(odom_vx) * dt; s_nav.odom_distance_accum += gnav_fabsf(odom_vx) * dt;
} }
now_ms = cur_ms;
} else {
now_ms = board->imu_wz.timestamp_ms;
if (now_ms == 0) now_ms = 1;
} }
s_last_update_ms = now_ms; s_last_update_ms = now_ms;
s_last_odom_vx = odom_vx; s_last_odom_vx = odom_vx;

10
App/nav/global_nav.h
View File

@@ -118,14 +118,16 @@ void GlobalNav_Reset(void);
/** /**
* @brief 核心函数：每个导航周期调用一次 * @brief 核心函数：每个导航周期调用一次
* @param obs 预处理层的观测快照 * @param obs 预处理层的观测快照
* @param state EKF 走廊状态 * @param state EKF 走廊状态
* @param board 黑板快照 (读 IMU yaw_continuous, odom) * @param board 黑板快照 (读 IMU yaw_continuous, odom)
* @param out 导航输出 * @param now_ms 当前系统时间 (HAL_GetTick)，由调用方传入，避免内部依赖 IMU 时间戳
* @param out 导航输出
*/ */
void GlobalNav_Update(const CorridorObs_t* obs, void GlobalNav_Update(const CorridorObs_t* obs,
const CorridorState_t* state, const CorridorState_t* state,
const RobotBlackboard_t* board, const RobotBlackboard_t* board,
uint32_t now_ms,
GlobalNavOutput_t* out); GlobalNavOutput_t* out);
GlobalNavStage_t GlobalNav_GetStage(void); GlobalNavStage_t GlobalNav_GetStage(void);