From e18a7c949c77cd90a059596dd270185a12f3523c Mon Sep 17 00:00:00 2001 From: nitiantuhao <2062405236@qq.com> Date: Sat, 4 Apr 2026 21:19:41 +0800 Subject: [PATCH] 1.0 --- App/VL53L0X_API/platform/vl53_board.c | 5 ++ .../platform/vl53_calibration_config.h | 4 +- App/app_tasks.c | 14 ++- App/nav/nav_script.c | 87 +++++++++++++++++-- App/robot_params.h | 6 +- 5 files changed, 98 insertions(+), 18 deletions(-) diff --git a/App/VL53L0X_API/platform/vl53_board.c b/App/VL53L0X_API/platform/vl53_board.c index 40d61ec..4a8a284 100644 --- a/App/VL53L0X_API/platform/vl53_board.c +++ b/App/VL53L0X_API/platform/vl53_board.c @@ -87,6 +87,11 @@ static VL53L0X_Error vl53_do_static_init(VL53L0X_DEV dev, uint32_t timing_budget status = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(dev, timing_budget_us); if (status != VL53L0X_ERROR_NONE) return status; + /* 连续测量采用 back-to-back 模式，让下一次测量在上一帧完成后立即开始。 + * 这样任务轮询周期变快时，能尽可能拿到最新帧，而不是被固定间隔再次拖慢。 */ + status = VL53L0X_SetInterMeasurementPeriodMilliSeconds(dev, 0u); + if (status != VL53L0X_ERROR_NONE) return status; + status = VL53L0X_SetLimitCheckEnable(dev, VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 1u); if (status != VL53L0X_ERROR_NONE) return status; diff --git a/App/VL53L0X_API/platform/vl53_calibration_config.h b/App/VL53L0X_API/platform/vl53_calibration_config.h index a4ed731..fb640bd 100644 --- a/App/VL53L0X_API/platform/vl53_calibration_config.h +++ b/App/VL53L0X_API/platform/vl53_calibration_config.h @@ -37,13 +37,13 @@ static const Vl53RuntimeCalibration_t k_vl53_left_calibration[2] = { static const Vl53RuntimeCalibration_t k_vl53_right_calibration[2] = { { - .offset_micro_meters = 5000, + .offset_micro_meters = 9000, .xtalk_compensation_rate_mcps = 0, .offset_calibrated = 1, .xtalk_calibrated = 0, }, { - .offset_micro_meters = 10000, + .offset_micro_meters = 13000, .xtalk_compensation_rate_mcps = 0, .offset_calibrated = 1, .xtalk_calibrated = 0, diff --git a/App/app_tasks.c b/App/app_tasks.c index 4d35871..74e9e8c 100644 --- a/App/app_tasks.c +++ b/App/app_tasks.c @@ -282,8 +282,8 @@ void AppTasks_RunVl53Task_Impl(void *argument) // 统一推送到黑板 Blackboard_UpdateVl53(&combined_snap); - // 【修改 3】：休眠 100ms 匹配 VL53L0X 的新测距周期 - osDelay(100); + // 与 VL53 约 33ms 的连续测量节奏匹配，避免高频空轮询旧数据。 + osDelay(33); } } @@ -390,9 +390,15 @@ void AppTasks_RunNavTask_Impl(void *argument) raw_cmd.flags = 0U; } - /* --- Step 6: 安全仲裁 (CORRIDOR 模式，兼容旧行为) --- */ + /* --- Step 6: 安全仲裁 (按脚本阶段切换模式) --- */ SegFsmOutput_t fsm_out; - SegFsm_Update(&raw_cmd, &obs, &corridor_state, SAFETY_MODE_CORRIDOR, &fsm_out); + SafetyMode_t safety_mode = SAFETY_MODE_CORRIDOR; + if (!script_out.active) { + safety_mode = SAFETY_MODE_IDLE; + } else if (script_out.stage == SCRIPT_STAGE_TURN_AT_END) { + safety_mode = SAFETY_MODE_TURN; + } + SegFsm_Update(&raw_cmd, &obs, &corridor_state, safety_mode, &fsm_out); #endif diff --git a/App/nav/nav_script.c b/App/nav/nav_script.c index 9271176..702e24d 100644 --- a/App/nav/nav_script.c +++ b/App/nav/nav_script.c @@ -5,6 +5,7 @@ #include "nav_script.h" #include "est/corridor_filter.h" +#include "robot_params.h" #include #include #include @@ -24,6 +25,8 @@ static struct { float turn_start_e_th; // 转向开始时的 EKF 航向 (保留作后备) float turn_start_imu_yaw_deg; // 转向开始时的 IMU 连续偏航角 (deg) bool turn_started; // 转向是否已开始 + bool turn_coarse_done; // 180° IMU 粗转是否已完成 + uint32_t turn_settle_start_ms; // 进入侧墙精调阶段的时刻 float corridor_s_entry; // 进入垄沟时的 s 里程 float end_rearm_s; // 掉头后到端检测重新使能的起始里程 bool end_armed; // 到端检测是否已重新使能 @@ -33,6 +36,10 @@ static struct { } s_internal; #define SCRIPT_END_REARM_DIST_M 0.12f +#define SCRIPT_TURN_SETTLE_KP 1.4f +#define SCRIPT_TURN_SETTLE_W_MAX 0.6f +#define SCRIPT_TURN_SETTLE_TOL_RAD 0.05f +#define SCRIPT_TURN_SETTLE_TIMEOUT_MS 1200u /* ========================================================= * 内部辅助函数 @@ -44,6 +51,35 @@ static inline float clampf(float val, float lo, float hi) return val; } +static bool compute_wall_heading_error(const CorridorObs_t *obs, float *out_heading_rad) +{ + float heading_sum = 0.0f; + int valid_count = 0; + const float sensor_base = PARAM_SENSOR_BASE_LENGTH; + + bool left_ok = ((obs->valid_mask & CORRIDOR_OBS_MASK_LF) != 0U) && + ((obs->valid_mask & CORRIDOR_OBS_MASK_LR) != 0U); + bool right_ok = ((obs->valid_mask & CORRIDOR_OBS_MASK_RF) != 0U) && + ((obs->valid_mask & CORRIDOR_OBS_MASK_RR) != 0U); + + if (left_ok) { + heading_sum += atan2f(obs->d_lr - obs->d_lf, sensor_base); + valid_count++; + } + + if (right_ok) { + heading_sum += atan2f(obs->d_rf - obs->d_rr, sensor_base); + valid_count++; + } + + if (valid_count == 0) { + return false; + } + + *out_heading_rad = heading_sum / (float)valid_count; + return true; +} + /* ========================================================= * API 实现 * ========================================================= */ @@ -213,6 +249,8 @@ void NavScript_Update(const CorridorObs_t *obs, s_internal.turn_start_e_th = state->e_th; s_internal.turn_start_imu_yaw_deg = imu_yaw_continuous_deg; s_internal.turn_started = true; + s_internal.turn_coarse_done = false; + s_internal.turn_settle_start_ms = 0U; } /* 原地 180 度转向策略 */ @@ -228,15 +266,46 @@ void NavScript_Update(const CorridorObs_t *obs, float remaining = target_delta - fabsf(delta_turned); - if (remaining <= 0.1f) { - /* 转向完成 -> 决定下一步 */ - CorridorFilter_RebaseAfterTurnaround(imu_yaw_continuous_deg * 0.01745329252f); - s_internal.end_rearm_s = state->s; - s_internal.end_armed = false; - s_stage = s_internal.post_turn_stage; - out->override_v = 0.0f; - out->override_w = 0.0f; - out->use_override = true; + if (!s_internal.turn_coarse_done && remaining <= 0.1f) { + s_internal.turn_coarse_done = true; + s_internal.turn_settle_start_ms = obs->t_ms; + } + + if (s_internal.turn_coarse_done) { + float wall_heading = 0.0f; + bool wall_heading_ok = compute_wall_heading_error(obs, &wall_heading); + bool settle_timeout = (obs->t_ms - s_internal.turn_settle_start_ms) >= SCRIPT_TURN_SETTLE_TIMEOUT_MS; + + if (wall_heading_ok) { + if (fabsf(wall_heading) <= SCRIPT_TURN_SETTLE_TOL_RAD) { + CorridorFilter_RebaseAfterTurnaround(imu_yaw_continuous_deg * 0.01745329252f); + s_internal.end_rearm_s = state->s; + s_internal.end_armed = false; + s_stage = s_internal.post_turn_stage; + out->override_v = 0.0f; + out->override_w = 0.0f; + out->use_override = true; + } else { + out->override_v = 0.0f; + out->override_w = clampf(-SCRIPT_TURN_SETTLE_KP * wall_heading, + -SCRIPT_TURN_SETTLE_W_MAX, + SCRIPT_TURN_SETTLE_W_MAX); + out->use_override = true; + } + } else if (settle_timeout) { + /* 极端情况下侧墙暂时不可用，超时后接受 IMU 粗转结果，避免卡死。 */ + CorridorFilter_RebaseAfterTurnaround(imu_yaw_continuous_deg * 0.01745329252f); + s_internal.end_rearm_s = state->s; + s_internal.end_armed = false; + s_stage = s_internal.post_turn_stage; + out->override_v = 0.0f; + out->override_w = 0.0f; + out->use_override = true; + } else { + out->override_v = 0.0f; + out->override_w = 0.0f; + out->use_override = true; + } } else { /* 继续转向：根据目标方向选择角速度 */ float turn_dir = (target_delta > 0.0f) ? 1.0f : -1.0f; diff --git a/App/robot_params.h b/App/robot_params.h index 8fa8432..19630f3 100644 --- a/App/robot_params.h +++ b/App/robot_params.h @@ -346,10 +346,10 @@ extern "C" { /** @brief [实测] VL53L0X 测距预算 [μs] * 含义：单次测距的时间预算，越大精度越高但频率越低 * 选项：20000(20ms, 高速), 33000(33ms), 100000(100ms), 200000(200ms, 高精度) - * 典型值：100000 (100ms，折中) - * 调优：走廊跟踪时用 100ms，原地转向时可切 200ms 提高精度 + * 典型值：33000 (33ms，折中) + * 调优：本车实测稳定上限按 33ms 配置，若噪声偏大再逐步提高 */ -#define PARAM_VL53_TIMING_BUDGET 100000U +#define PARAM_VL53_TIMING_BUDGET 33000U /** @brief [调优] VL53L0X 卡尔曼滤波参数 Q * 含义：测距过程噪声