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 21:19:41 +08:00
parent 1ecdf6c187
commit e18a7c949c
5 changed files with 98 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
App/VL53L0X_API/platform/vl53_board.c
View File

@@ -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;

4
App/VL53L0X_API/platform/vl53_calibration_config.h
View File

@@ -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,

14
App/app_tasks.c
View File

@@ -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

87
App/nav/nav_script.c
View File

@@ -5,6 +5,7 @@
#include "nav_script.h"
#include "est/corridor_filter.h"
#include "robot_params.h"
#include <stdint.h>
#include <stdbool.h>
#include <math.h>
@@ -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;

6
App/robot_params.h
View File

@@ -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
* 含义：测距过程噪声