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

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;