919 lines
69 KiB
C
919 lines
69 KiB
C
/**
|
||
* @file global_nav.c
|
||
* @brief 璧涢亾绾ф€绘帶 鈥?娣峰悎瀵艰埅鐘舵€佹満瀹炵幇
|
||
*
|
||
* 鍦哄湴缁撴瀯: 鍨勬矡娌縓杞?妯悜)鍒嗗竷锛屽乏鍙充袱绔悇鏈夌旱鍚戠閮ㄩ€氶亾
|
||
* 鍚姩鍖哄湪宸︿笅瑙掞紝鍏ュ彛瀵归綈宸︾閫氶亾
|
||
*
|
||
* S 鍨嬮亶鍘?
|
||
* 鍏ュ満(鍖楄)鈫掑彸杞叆C1(鈫掍笢)鈫掑彸绔埌绔啋宸﹁浆鈫掑寳琛屸啋宸﹁浆鍏2(鈫愯タ)
|
||
* 鈫掑乏绔埌绔啋鍙宠浆鈫掑寳琛屸啋鍙宠浆鍏3(鈫掍笢)鈫?..鈫扖6(鈫愯タ)
|
||
* 鈫掑乏绔埌绔啋宸﹁浆(鏈濆崡)鈫掑崡琛屽嚭鍦衡啋鍥炲仠鍚姩鍖? *
|
||
* 绔儴閫氶亾鐗圭偣:
|
||
* - 涓€渚ц创鍥存爮(VL53鑳芥祴鍒?, 鍙︿竴渚т氦鏇垮嚭鐜板瀯鑳岀闈㈠拰鍨勬矡寮€鍙? * - 涓嶈兘渚濊禆鍙屼晶VL53鍋欵KF, 蹇呴』鐢↖MU鑸悜淇濇寔+鍓嶅悗婵€鍏夊埌绔娴? */
|
||
|
||
#include "global_nav.h"
|
||
#include "est/corridor_filter.h"
|
||
#include "preproc/corridor_preproc.h"
|
||
#include "robot_params.h"
|
||
#include <math.h>
|
||
#include <string.h>
|
||
|
||
/* =========================================================
|
||
* 鍐呴儴鐘舵€? * ========================================================= */
|
||
static struct {
|
||
GlobalNavStage_t stage;
|
||
bool running;
|
||
bool initialized;
|
||
|
||
/* 璧涢亾绾?*/
|
||
uint8_t current_corridor_id;
|
||
uint8_t corridors_completed;
|
||
|
||
/* 杞悜 */
|
||
float turn_start_yaw_deg; /* IMU yaw_continuous at turn start */
|
||
float turn_target_delta_deg; /* 90掳 */
|
||
int8_t turn_sign; /* +1 (CCW) or -1 (CW) */
|
||
|
||
/* 閲岀▼ (鐢ㄩ噷绋嬭绉垎璺濈) */
|
||
float stage_entry_odom_vx_accum; /* 杩涘叆闃舵鏃剁殑閲岀▼璁＄疮璁¤窛绂?*/
|
||
float odom_distance_accum; /* 杩愯涓寔缁Н鍒嗙殑閲岀▼ */
|
||
|
||
/* 瓒呮椂 */
|
||
uint32_t stage_start_ms;
|
||
|
||
/* 閲嶆崟鑾?*/
|
||
uint8_t reacquire_ok_count;
|
||
|
||
/* 鍘熷湴瀵规 */
|
||
uint8_t align_ok_count;
|
||
|
||
/* 鍑哄満 */
|
||
bool exit_vl53_lost;
|
||
float exit_lost_distance;
|
||
|
||
/* 鑸悜淇濇寔 */
|
||
float heading_ref_deg;
|
||
|
||
/* 杩炴帴娈? 澶氫紶鎰熷櫒杈呭姪 */
|
||
float link_d_front_start; /* 杩涘叆杩炴帴娈垫椂鍓嶆縺鍏夎鏁?(m) */
|
||
bool link_d_front_valid; /* 杩涘叆鏃跺墠婵€鍏夋槸鍚︽湁鏁?*/
|
||
uint8_t link_gap_count; /* 闈炲洿鏍忎晶 VL53 杩炵画涓㈠け璁℃暟 (娌熷彛纭) */
|
||
|
||
/* EKF 杩涘害淇濆瓨 */
|
||
float corridor_entry_s;
|
||
|
||
/* 閰嶇疆 */
|
||
GlobalNavConfig_t cfg;
|
||
} s_nav;
|
||
|
||
/* 涓婁竴涓懆鏈熺殑閲岀▼璁￠€熷害锛岀敤浜庣Н鍒?*/
|
||
static float s_last_odom_vx = 0.0f;
|
||
static uint32_t s_last_update_ms = 0;
|
||
|
||
/* =========================================================
|
||
* 杈呭姪鍑芥暟
|
||
* ========================================================= */
|
||
static inline float gnav_clampf(float val, float lo, float hi)
|
||
{
|
||
if (val < lo) return lo;
|
||
if (val > hi) return hi;
|
||
return val;
|
||
}
|
||
|
||
static inline float gnav_fabsf(float x)
|
||
{
|
||
return x < 0.0f ? -x : x;
|
||
}
|
||
|
||
/** 绠€鍗?P 鎺у埗鑸悜淇濇寔锛岃緭鍏ュ亸宸?(deg)锛岃緭鍑鸿閫熷害 (rad/s) */
|
||
static float heading_hold_pd(float current_yaw_deg, float ref_yaw_deg, float kp)
|
||
{
|
||
float err_deg = ref_yaw_deg - current_yaw_deg;
|
||
float w = kp * err_deg; /* kp 鎶?掳 鏄犲皠鍒?rad/s */
|
||
return gnav_clampf(w, -1.0f, 1.0f);
|
||
}
|
||
|
||
/** 妫€鏌ヤ晶鍚?VL53 鏄惁鎺㈠埌澹?(鑷冲皯鏈変竴渚х殑鍓嶅悗閮芥湁鏁? 鈥?浠呯敤浜庨噸鎹曡幏 */
|
||
static bool side_walls_detected(const CorridorObs_t* obs)
|
||
{
|
||
bool left_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_LF) &&
|
||
(obs->valid_mask & CORRIDOR_OBS_MASK_LR);
|
||
bool right_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_RF) &&
|
||
(obs->valid_mask & CORRIDOR_OBS_MASK_RR);
|
||
return left_ok || right_ok;
|
||
}
|
||
|
||
/**
|
||
* @brief 妫€娴嬮潪鍥存爮渚?VL53 鏄惁鍙戠幇鍨勬矡寮€鍙?(杩炴帴娈垫矡鍙ｈ緟鍔╁垽瀹?
|
||
*
|
||
* 鍦ㄧ旱鍚戠閮ㄩ€氶亾闈㈡湞鍖楄椹舵椂锛? * - 鍒氳蛋瀹屽悜涓?EAST)鐨勬矡 鈫?褰撳墠鍦ㄥ彸绔€氶亾 鈫?鍙充晶璐村洿鏍?鈫?妫€鏌ュ乏渚L53
|
||
* - 鍒氳蛋瀹屽悜瑗?WEST)鐨勬矡 鈫?褰撳墠鍦ㄥ乏绔€氶亾 鈫?宸︿晶璐村洿鏍?鈫?妫€鏌ュ彸渚L53
|
||
*
|
||
* 缁忚繃鍨勮儗绔潰鏃? VL53 娴嬪埌绾?(閫氶亾瀹?2 - 杞﹀/2 - VL53鍐呯缉) 鈮?10cm 鈫?鏈夋晥
|
||
* 缁忚繃鍨勬矡寮€鍙ｆ椂: VL53 灏勫叆娌熷唴 220cm+ 鈫?瓒呭嚭鏈夋晥璺濈 鈫?鏃犳晥鎴栬鏁?> 1.2m
|
||
*
|
||
* 鍥犱负 VL53 鍓嶅悗涓ら闂磋窛 12cm锛岃溅韬?20cm 瀹斤紝娌熷彛 40cm 瀹斤紝
|
||
* 褰撹溅韬腑蹇冨鍑嗘矡鍙ｆ椂锛屽墠鍚?VL53 閮藉凡杩涘叆寮€鍙ｅ尯鍩燂紝涓ら鍧囧簲涓㈠け銆? * 浣嗚繃娓″尯鏈夎竟缂樻晥搴旓紝鎵€浠ュ彧瑕佹眰 "鍓嶅悗鑷冲皯涓€棰椾涪澶? 鍗宠涓烘帰鍒版矡鍙ｃ€? *
|
||
* @param obs 褰撳墠瑙傛祴
|
||
* @param prev_travel_dir 鍒氳蛋瀹岀殑閭ｆ潯娌熺殑琛岄┒鏂瑰悜 (EAST/WEST)
|
||
* @return true = 闈炲洿鏍忎晶妫€娴嬪埌寮€鍙? */
|
||
static bool gap_detected_on_open_side(const CorridorObs_t* obs,
|
||
TravelDirection_t prev_travel_dir)
|
||
{
|
||
if (prev_travel_dir == TRAVEL_DIR_EAST) {
|
||
/* 鍦ㄥ彸绔€氶亾锛屽彸渚ц创鍥存爮 鈫?妫€鏌ュ乏渚?VL53 */
|
||
bool lf_lost = !(obs->valid_mask & CORRIDOR_OBS_MASK_LF)
|
||
|| (obs->d_lf > 0.5f); /* >50cm 瑙嗕负娌熷彛 (姝ｅ父璐村绾?0cm) */
|
||
bool lr_lost = !(obs->valid_mask & CORRIDOR_OBS_MASK_LR)
|
||
|| (obs->d_lr > 0.5f);
|
||
return lf_lost || lr_lost;
|
||
} else {
|
||
/* 鍦ㄥ乏绔€氶亾锛屽乏渚ц创鍥存爮 鈫?妫€鏌ュ彸渚?VL53 */
|
||
bool rf_lost = !(obs->valid_mask & CORRIDOR_OBS_MASK_RF)
|
||
|| (obs->d_rf > 0.5f);
|
||
bool rr_lost = !(obs->valid_mask & CORRIDOR_OBS_MASK_RR)
|
||
|| (obs->d_rr > 0.5f);
|
||
return rf_lost || rr_lost;
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief [Phase-1] 鑾峰彇鍥存爮渚?VL53 骞冲潎璺濈
|
||
*
|
||
* 鍦ㄧ閮ㄩ€氶亾鍐咃紝鍥存爮渚х敱鍒氳蛋瀹岀殑娌熺殑琛岄┒鏂瑰悜鍐冲畾锛? * EAST 鈫?鍦ㄥ彸绔€氶亾 鈫?鍥存爮鍦ㄥ彸渚? * WEST 鈫?鍦ㄥ乏绔€氶亾 鈫?鍥存爮鍦ㄥ乏渚? *
|
||
* 鍥存爮渚т袱棰?VL53 鍙栧钩鍧囧彲灏?卤2cm 涓綋璇樊闄嶄綆鍒?卤1.4cm銆? * 浠呬竴棰楁湁鏁堟椂涔熻繑鍥炶棰楄鏁帮紙绮惧害绋嶄綆浣嗘湁鑳滀簬鏃狅級銆? *
|
||
* @param obs 褰撳墠瑙傛祴
|
||
* @param travel_dir 鍒氳蛋瀹岀殑娌熺殑琛岄┒鏂瑰悜
|
||
* @param[out] d_avg 鍥存爮渚у钩鍧囪窛绂?(m)
|
||
* @param[out] fence_sign 妯悜鏍℃鏂瑰悜绗﹀彿: +1 = 鍋忕鍥存爮鏃跺簲鍚戝乏杞? -1 = 搴斿悜鍙宠浆
|
||
* @return true = 鑷冲皯涓€棰楀洿鏍忎晶 VL53 鏈夋晥
|
||
*/
|
||
static bool get_fence_side_distance(const CorridorObs_t* obs,
|
||
TravelDirection_t travel_dir,
|
||
float* d_avg,
|
||
float* fence_sign)
|
||
{
|
||
float df, dr;
|
||
bool f_ok, r_ok;
|
||
|
||
if (travel_dir == TRAVEL_DIR_EAST) {
|
||
/* 鍙崇閫氶亾 鈫?鍥存爮鍦ㄥ彸渚?涓滀晶X=31) */
|
||
df = obs->d_rf; dr = obs->d_rr;
|
||
f_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_RF) != 0U;
|
||
r_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_RR) != 0U;
|
||
/* 鎺у埗寰? w = fence_sign 脳 kp 脳 (d_actual - d_target)
|
||
* 鍙充晶绂诲洿鏍忓お杩?d_actual > d_target) 鈫?搴斿悜鍙宠浆(-w)闈犺繎
|
||
* 鎵€浠?fence_sign = -1 */
|
||
*fence_sign = -1.0f;
|
||
} else {
|
||
/* 宸︾閫氶亾 鈫?鍥存爮鍦ㄥ乏渚?瑗夸晶X=0) */
|
||
df = obs->d_lf; dr = obs->d_lr;
|
||
f_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_LF) != 0U;
|
||
r_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_LR) != 0U;
|
||
/* 鎺у埗寰? w = fence_sign 脳 kp 脳 (d_actual - d_target)
|
||
* 宸︿晶绂诲洿鏍忓お杩?d_actual > d_target) 鈫?搴斿悜宸﹁浆(+w)闈犺繎
|
||
* 鎵€浠?fence_sign = +1 */
|
||
*fence_sign = +1.0f;
|
||
}
|
||
|
||
if (f_ok && r_ok) {
|
||
*d_avg = (df + dr) * 0.5f; /* 鍙岄骞冲潎锛屽櫔澹?卤2cm 鈫?卤1.4cm */
|
||
return true;
|
||
} else if (f_ok) {
|
||
*d_avg = df;
|
||
return true;
|
||
} else if (r_ok) {
|
||
*d_avg = dr;
|
||
return true;
|
||
}
|
||
return false;
|
||
}
|
||
|
||
/** 妫€鏌ヤ晶鍚?VL53 鏄惁鍏ㄤ涪 */
|
||
static bool all_side_lost(const CorridorObs_t* obs)
|
||
{
|
||
uint8_t side_mask = CORRIDOR_OBS_MASK_LF | CORRIDOR_OBS_MASK_LR |
|
||
CORRIDOR_OBS_MASK_RF | CORRIDOR_OBS_MASK_RR;
|
||
return (obs->valid_mask & side_mask) == 0U;
|
||
}
|
||
|
||
/** 妫€鏌ラ噸鎹曡幏鏉′欢 */
|
||
static bool check_reacquire(const CorridorObs_t* obs, const CorridorState_t* state)
|
||
{
|
||
/* ----------------------------------------------------------------
|
||
* 鏉′欢 0 (鏂板): 鍚庢縺鍏夊叆娌熺‘璁? *
|
||
* 鐗╃悊鍘熺悊锛? * 杞集瀹屾垚鍚庤溅澶存湞鍚戞矡鍐咃紝鍚庢縺鍏夋湞鍚戠閮ㄩ€氶亾绾靛悜鏂瑰悜銆? * - 杞︽湭杩囨矡鍙ｏ細鍚庢縺鍏夋祴鍒扮閮ㄩ€氶亾瀵归潰鐨勫洿鏍忥紝璺濈杈冪煭锛堚増20~35cm锛? * - 杞﹁韩瓒婅繃娌熷彛锛氬悗婵€鍏夎绾胯繘鍏ユ矡鍐咃紝璺濈浠庤繎绔洿鏍忚烦鍙樺埌娌熼暱
|
||
* 鏂瑰悜锛?20cm+閫氶亾瀹斤級锛岃鏁?>> 40cm 鎴栬秴閲忕▼鐩存帴鏃犳晥
|
||
*
|
||
* 鍥犳 d_back > 闃堝€?鏄?杞﹁韩宸茶秺杩囨矡鍙?鐨勭洿鎺ョ墿鐞嗚瘉鎹紝
|
||
* 鍖哄垎搴︽瀬澶э紙浠?~30cm 璺冲埌 >120cm锛夛紝杩滀紭浜庨噷绋嬭绉垎銆? *
|
||
* 闃堝€奸€夊彇锛? * 绔儴閫氶亾瀹?40cm锛岃溅瀹?20cm锛岃溅韬眳涓椂涓や晶鍚?10cm銆? * 鍚庢縺鍏夊埌瑗夸晶鍥存爮鏈€杩滅害 40cm锛堥€氶亾鍏ㄥ锛夈€? * 鍙?0.45m 浣滀负闃堝€硷細楂樹簬閫氶亾瀹界殑鏈€澶у€硷紝浣庝簬娌熷唴鏈€鐭祴閲忓€笺€? * 鑻ュ悗婵€鍏夋棤鏁堬紙瓒呴噺绋嬶級鍒欏悓鏍锋弧瓒虫潯浠讹紙璇绘暟=0涓攊nvalid锛夆€斺€? * 浣嗘垜浠敤 MASK_BACK 妫€鏌ユ湁鏁堟€э紝鏃犳晥鏃舵鏉′欢涓嶆垚绔嬶紝閫€鍖栦负閲岀▼淇濇姢銆? * ---------------------------------------------------------------- */
|
||
bool back_valid = (obs->valid_mask & CORRIDOR_OBS_MASK_BACK) != 0U;
|
||
if (back_valid && obs->d_back <= s_nav.cfg.reacquire_back_thresh) {
|
||
/* 鍚庢縺鍏変粛鑳芥祴鍒扮閮ㄩ€氶亾杩戝 鈫?杞﹁韩灏氭湭瓒婅繃娌熷彛 鈫?鎷掔粷 */
|
||
return false;
|
||
}
|
||
/* 鑻ュ悗婵€鍏夋棤鏁堬紙瓒呭嚭閲忕▼锛夆啋 璇存槑瑙嗙嚎宸叉繁鍏ユ矡鍐咃紝瑙嗕负宸插叆娌燂紝璺宠繃姝ら棬妲?*/
|
||
|
||
/* 鏉′欢 1: 鑷冲皯 3 涓晶鍚戜紶鎰熷櫒鏈夋晥 */
|
||
uint8_t side_mask = CORRIDOR_OBS_MASK_LF | CORRIDOR_OBS_MASK_LR |
|
||
CORRIDOR_OBS_MASK_RF | CORRIDOR_OBS_MASK_RR;
|
||
uint8_t active = obs->valid_mask & side_mask;
|
||
int count = 0;
|
||
for (int i = 0; i < 4; i++) {
|
||
if (active & (1U << i)) count++;
|
||
}
|
||
if (count < 3) return false;
|
||
|
||
/* 鏉′欢 2: 宸﹀彸璺濈鍜?鈮?璧板粖瀹藉害 */
|
||
bool left_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_LF) &&
|
||
(obs->valid_mask & CORRIDOR_OBS_MASK_LR);
|
||
bool right_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_RF) &&
|
||
(obs->valid_mask & CORRIDOR_OBS_MASK_RR);
|
||
|
||
if (left_ok && right_ok) {
|
||
float d_left = (obs->d_lf + obs->d_lr) * 0.5f;
|
||
float d_right = (obs->d_rf + obs->d_rr) * 0.5f;
|
||
float total_width = d_left + d_right + PARAM_ROBOT_WIDTH;
|
||
float err = gnav_fabsf(total_width - s_nav.cfg.corridor_width);
|
||
if (err > s_nav.cfg.reacquire_width_tol) return false;
|
||
}
|
||
|
||
/* 鏉′欢 3: EKF 缃俊搴?*/
|
||
if (state->conf < s_nav.cfg.reacquire_conf_thresh) return false;
|
||
|
||
return true;
|
||
}
|
||
|
||
/** 鑾峰彇褰撳墠闃舵鐨勫凡琛岄┒閲岀▼ */
|
||
static float odom_since_entry(void)
|
||
{
|
||
return s_nav.odom_distance_accum - s_nav.stage_entry_odom_vx_accum;
|
||
}
|
||
|
||
/** 闃舵杞Щ 鈥?閫氱敤鍒濆鍖?*/
|
||
static void transition_to(GlobalNavStage_t next, const RobotBlackboard_t* board);
|
||
|
||
/* =========================================================
|
||
* 闃舵鍚嶇О琛? * ========================================================= */
|
||
static const char* const s_stage_names[] = {
|
||
"IDLE",
|
||
"ENTRY_STRAIGHT",
|
||
"TURN_INTO_CORRIDOR",
|
||
"REACQUIRE",
|
||
"ALIGN",
|
||
"CORRIDOR_TRACK",
|
||
"TURN_OUT",
|
||
"LINK_STRAIGHT",
|
||
"TURN_INTO_NEXT",
|
||
"EXIT_STRAIGHT",
|
||
"DOCK",
|
||
"FINISHED",
|
||
"ERROR"
|
||
};
|
||
|
||
/* =========================================================
|
||
* 杞悜鎵ц (缁熶竴鐨?90掳 杞悜閫昏緫)
|
||
* ========================================================= */
|
||
static void execute_turn(const CorridorObs_t* obs,
|
||
const CorridorState_t* state,
|
||
const RobotBlackboard_t* board,
|
||
uint32_t now_ms,
|
||
GlobalNavOutput_t* out)
|
||
{
|
||
/* [鏀硅繘G] IMU 澶辨晥瀹夊叏淇濇姢: 娌℃湁 IMU 鏁版嵁鏃剁珛鍗冲仠杞︼紝涓嶇洸杞€? * 瓒呮椂鍚庝細琚閮ㄨ秴鏃朵繚鎶ゆ崟鑾凤紝杩涘叆 GNAV_ERROR銆?*/
|
||
if (!board->imu_yaw_continuous.is_valid) {
|
||
out->override_v = 0.0f;
|
||
out->override_w = 0.0f;
|
||
out->use_override = true;
|
||
out->request_corridor = false;
|
||
out->safety_mode = SAFETY_MODE_IDLE;
|
||
return;
|
||
}
|
||
|
||
float imu_yaw = board->imu_yaw_continuous.value;
|
||
|
||
/* 宸茶浆杩囩殑瑙掑害 (鍙栫粷瀵瑰€? */
|
||
float delta = (imu_yaw - s_nav.turn_start_yaw_deg) * s_nav.turn_sign;
|
||
float target = s_nav.turn_target_delta_deg;
|
||
float remaining_deg = target - delta;
|
||
|
||
float omega = s_nav.cfg.turn_omega;
|
||
|
||
/* 鎺ヨ繎鐩爣鏃跺噺閫?*/
|
||
float decel_zone_deg = PARAM_RAD2DEG(s_nav.cfg.turn_decel_zone_rad);
|
||
if (remaining_deg < decel_zone_deg && decel_zone_deg > 0.01f) {
|
||
float ratio = remaining_deg / decel_zone_deg;
|
||
if (ratio < 0.0f) ratio = 0.0f;
|
||
omega = s_nav.cfg.turn_min_omega +
|
||
ratio * (s_nav.cfg.turn_omega - s_nav.cfg.turn_min_omega);
|
||
}
|
||
|
||
out->override_v = 0.0f;
|
||
out->override_w = (float)s_nav.turn_sign * omega;
|
||
out->use_override = true;
|
||
out->request_corridor = false;
|
||
out->safety_mode = SAFETY_MODE_TURN;
|
||
|
||
/* 杞悜瀹屾垚鍒ゅ畾 */
|
||
float tolerance_deg = PARAM_RAD2DEG(s_nav.cfg.turn_tolerance_rad);
|
||
if (delta >= target - tolerance_deg) {
|
||
switch (s_nav.stage) {
|
||
case GNAV_TURN_INTO_CORRIDOR:
|
||
case GNAV_TURN_INTO_NEXT:
|
||
transition_to(GNAV_REACQUIRE, board);
|
||
break;
|
||
case GNAV_TURN_OUT_OF_CORRIDOR:
|
||
if (TrackMap_IsLastCorridor(s_nav.current_corridor_id)) {
|
||
transition_to(GNAV_EXIT_STRAIGHT, board);
|
||
} else {
|
||
transition_to(GNAV_LINK_STRAIGHT, board);
|
||
}
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
return;
|
||
}
|
||
|
||
/* 瓒呮椂淇濇姢 */
|
||
if (now_ms - s_nav.stage_start_ms > s_nav.cfg.turn_timeout_ms) {
|
||
transition_to(GNAV_ERROR, board);
|
||
}
|
||
}
|
||
|
||
/* =========================================================
|
||
* 闃舵杞Щ
|
||
* ========================================================= */
|
||
static void transition_to(GlobalNavStage_t next, const RobotBlackboard_t* board)
|
||
{
|
||
float imu_yaw = (board != NULL && board->imu_yaw_continuous.is_valid)
|
||
? board->imu_yaw_continuous.value : 0.0f;
|
||
|
||
/* 閫氱敤: 璁板綍杩涘叆鏃堕棿鍜岄噷绋?*/
|
||
s_nav.stage_start_ms = s_last_update_ms;
|
||
s_nav.stage_entry_odom_vx_accum = s_nav.odom_distance_accum;
|
||
s_nav.reacquire_ok_count = 0;
|
||
|
||
switch (next) {
|
||
case GNAV_ENTRY_STRAIGHT:
|
||
s_nav.heading_ref_deg = imu_yaw;
|
||
s_nav.current_corridor_id = TrackMap_Get()->entry_corridor_id;
|
||
break;
|
||
|
||
case GNAV_TURN_INTO_CORRIDOR: {
|
||
const CorridorDescriptor_t* cd = TrackMap_GetCorridor(s_nav.current_corridor_id);
|
||
s_nav.turn_sign = (int8_t)cd->entry_turn_dir;
|
||
s_nav.turn_start_yaw_deg = imu_yaw;
|
||
s_nav.turn_target_delta_deg = 90.0f;
|
||
break;
|
||
}
|
||
case GNAV_TURN_OUT_OF_CORRIDOR: {
|
||
const CorridorDescriptor_t* cd = TrackMap_GetCorridor(s_nav.current_corridor_id);
|
||
s_nav.turn_sign = (int8_t)cd->exit_turn_dir;
|
||
s_nav.turn_start_yaw_deg = imu_yaw;
|
||
s_nav.turn_target_delta_deg = 90.0f;
|
||
break;
|
||
}
|
||
case GNAV_TURN_INTO_NEXT: {
|
||
uint8_t next_id = TrackMap_GetNextCorridorId(s_nav.current_corridor_id);
|
||
const CorridorDescriptor_t* cd = TrackMap_GetCorridor(next_id);
|
||
s_nav.turn_sign = (int8_t)cd->entry_turn_dir;
|
||
s_nav.turn_start_yaw_deg = imu_yaw;
|
||
s_nav.turn_target_delta_deg = 90.0f;
|
||
s_nav.current_corridor_id = next_id;
|
||
break;
|
||
}
|
||
case GNAV_REACQUIRE:
|
||
/* EKF 閲嶇疆: 鏂版矡鐨?e_y 鍙傝€冧笉鍚岋紝蹇呴』閲嶅缓 */
|
||
CorridorFilter_Reset();
|
||
s_nav.heading_ref_deg = imu_yaw;
|
||
break;
|
||
|
||
case GNAV_ALIGN:
|
||
/* 鍋滆溅瀵规锛氭竻闆惰鏁板櫒锛孍KF 姝ゆ椂宸叉湁鍒濇浼拌 */
|
||
s_nav.align_ok_count = 0;
|
||
break;
|
||
|
||
case GNAV_CORRIDOR_TRACK:
|
||
s_nav.corridor_entry_s = 0.0f; /* EKF 宸?reset, s 浠?0 寮€濮?*/
|
||
break;
|
||
|
||
case GNAV_LINK_STRAIGHT:
|
||
s_nav.heading_ref_deg = imu_yaw;
|
||
s_nav.link_d_front_start = 0.0f;
|
||
s_nav.link_d_front_valid = false; /* 棣栨媿鍐嶈褰?*/
|
||
s_nav.link_gap_count = 0;
|
||
break;
|
||
|
||
case GNAV_EXIT_STRAIGHT:
|
||
s_nav.heading_ref_deg = imu_yaw;
|
||
s_nav.exit_vl53_lost = false;
|
||
s_nav.exit_lost_distance = 0.0f;
|
||
break;
|
||
|
||
case GNAV_DOCK:
|
||
break;
|
||
|
||
case GNAV_FINISHED:
|
||
s_nav.running = false;
|
||
break;
|
||
|
||
case GNAV_ERROR:
|
||
break;
|
||
|
||
default:
|
||
break;
|
||
}
|
||
|
||
s_nav.stage = next;
|
||
}
|
||
|
||
/* =========================================================
|
||
* 鍏紑 API
|
||
* ========================================================= */
|
||
|
||
void GlobalNav_Init(const GlobalNavConfig_t* cfg)
|
||
{
|
||
memset(&s_nav, 0, sizeof(s_nav));
|
||
s_nav.cfg = *cfg;
|
||
s_nav.stage = GNAV_IDLE;
|
||
s_nav.running = false;
|
||
s_nav.initialized = true;
|
||
s_last_odom_vx = 0.0f;
|
||
s_last_update_ms = 0;
|
||
}
|
||
|
||
void GlobalNav_Start(void)
|
||
{
|
||
if (!s_nav.initialized) return;
|
||
s_nav.running = true;
|
||
s_nav.corridors_completed = 0;
|
||
s_nav.odom_distance_accum = 0.0f;
|
||
s_nav.stage_entry_odom_vx_accum = 0.0f;
|
||
s_nav.current_corridor_id = TrackMap_Get()->entry_corridor_id;
|
||
/* 涓嶅湪杩欓噷 transition_to锛屽洜涓鸿繕娌℃湁 board 鏁版嵁銆? 涓嬩竴涓?Update 鍛ㄦ湡閲屽彂鐜?running && IDLE 鏃跺啀 transition銆?*/
|
||
s_nav.stage = GNAV_IDLE;
|
||
}
|
||
|
||
void GlobalNav_Stop(void)
|
||
{
|
||
s_nav.running = false;
|
||
s_nav.stage = GNAV_FINISHED;
|
||
}
|
||
|
||
void GlobalNav_Reset(void)
|
||
{
|
||
s_nav.stage = GNAV_IDLE;
|
||
s_nav.running = false;
|
||
s_nav.corridors_completed = 0;
|
||
s_nav.odom_distance_accum = 0.0f;
|
||
}
|
||
|
||
GlobalNavStage_t GlobalNav_GetStage(void)
|
||
{
|
||
return s_nav.stage;
|
||
}
|
||
|
||
const char* GlobalNav_GetStageName(GlobalNavStage_t stage)
|
||
{
|
||
if (stage <= GNAV_ERROR) {
|
||
return s_stage_names[stage];
|
||
}
|
||
return "UNKNOWN";
|
||
}
|
||
|
||
/* =========================================================
|
||
* 鏍稿績 Update
|
||
* ========================================================= */
|
||
void GlobalNav_Update(const CorridorObs_t* obs,
|
||
const CorridorState_t* state,
|
||
const RobotBlackboard_t* board,
|
||
uint32_t now_ms,
|
||
GlobalNavOutput_t* out)
|
||
{
|
||
if (!s_nav.initialized) {
|
||
memset(out, 0, sizeof(*out));
|
||
return;
|
||
}
|
||
|
||
/* 閲岀▼璁＄Н鍒? 螖d = vx * dt
|
||
* now_ms 鐢辫皟鐢ㄦ柟浼犲叆 (HAL_GetTick)锛屼笌浠讳綍浼犳劅鍣ㄦ椂闂存埑鏃犲叧锛? * 閬垮厤 IMU 鏃堕棿鎴冲仠鏇存椂閲岀▼鍜岃秴鏃跺弻鍙屽喕缁?(鍘?TODO-1 淇) */
|
||
{
|
||
float odom_vx = board->odom_vx;
|
||
if (s_last_update_ms > 0) {
|
||
float dt = (float)(now_ms - s_last_update_ms) * 0.001f;
|
||
if (dt > 0.0f && dt < 0.5f) {
|
||
s_nav.odom_distance_accum += gnav_fabsf(odom_vx) * dt;
|
||
}
|
||
}
|
||
s_last_update_ms = now_ms;
|
||
s_last_odom_vx = odom_vx;
|
||
}
|
||
|
||
/* [鏀硅繘G] IMU yaw 鎻愬彇: 澶辨晥鏃朵娇鐢ㄥ弬鑰冨€硷紝淇濇寔鑸悜涓嶅彉鑰屼笉鏄烦鍒?0 */
|
||
float imu_yaw_deg = board->imu_yaw_continuous.is_valid
|
||
? board->imu_yaw_continuous.value : s_nav.heading_ref_deg;
|
||
|
||
/* 榛樿杈撳嚭 */
|
||
out->use_override = true;
|
||
out->request_corridor = false;
|
||
out->override_v = 0.0f;
|
||
out->override_w = 0.0f;
|
||
out->safety_mode = SAFETY_MODE_IDLE;
|
||
out->stage = s_nav.stage;
|
||
out->corridor_id = s_nav.current_corridor_id;
|
||
out->corridors_done = s_nav.corridors_completed;
|
||
out->active = s_nav.running;
|
||
out->stage_name = GlobalNav_GetStageName(s_nav.stage);
|
||
|
||
if (!s_nav.running) return;
|
||
|
||
/* IDLE 鈫?鑷姩杩涘叆 ENTRY_STRAIGHT */
|
||
if (s_nav.stage == GNAV_IDLE) {
|
||
transition_to(GNAV_ENTRY_STRAIGHT, board);
|
||
out->stage = s_nav.stage;
|
||
out->stage_name = GlobalNav_GetStageName(s_nav.stage);
|
||
}
|
||
|
||
uint32_t elapsed_ms = now_ms - s_nav.stage_start_ms;
|
||
|
||
switch (s_nav.stage) {
|
||
|
||
/* ============================================================
|
||
* 鍏ュ満鐩寸嚎 (浠庡惎鍔ㄥ尯娌垮乏绔旱鍚戦€氶亾鍖楄)
|
||
*
|
||
* 浼犳劅鍣ㄦ儏鍐碉細
|
||
* - 宸︿晶 VL53 璐村洿鏍忥紝濮嬬粓鏈夋晥 鈫?鐢ㄤ簬妯悜璐村鎺у埗
|
||
* - 鍙充晶 VL53 涓€鍑哄惎鍔ㄥ尯灏卞鐫€C1寮€鍙?260cm)锛屾祴涓嶅埌
|
||
* - 鍏ュ満璺濈鏋佺煭锛堝惎鍔ㄥ尯鍏ュ彛鍒癈1鍏ュ彛浠呯害 10~30cm锛? * - 涓昏闈犻噷绋嬭鎺ㄨ繘瓒冲璺濈鍚庡嵆鍙浆鍚? * ============================================================ */
|
||
case GNAV_ENTRY_STRAIGHT: {
|
||
out->override_v = s_nav.cfg.entry_v;
|
||
out->safety_mode = SAFETY_MODE_STRAIGHT;
|
||
|
||
/* 鍏ュ満娈靛湪宸︾閫氶亾鍖楄锛氬乏渚ц创鍥存爮锛屼笌 LINK_STRAIGHT 鐩稿悓閫昏緫
|
||
* ENTRY 鏂瑰悜绛夋晥浜?TRAVEL_DIR_WEST锛堝湪宸︾閫氶亾锛屽洿鏍忓湪宸︿晶锛?/
|
||
{
|
||
float w_imu = heading_hold_pd(imu_yaw_deg, s_nav.heading_ref_deg,
|
||
s_nav.cfg.heading_kp);
|
||
float d_fence_avg = 0.0f;
|
||
float fence_sign = 0.0f;
|
||
bool fence_valid = get_fence_side_distance(obs, TRAVEL_DIR_WEST,
|
||
&d_fence_avg, &fence_sign);
|
||
if (fence_valid) {
|
||
float ey_wall = d_fence_avg - s_nav.cfg.link_wall_target_d;
|
||
float w_wall = fence_sign * s_nav.cfg.link_wall_kp_y * ey_wall;
|
||
w_wall = gnav_clampf(w_wall, -1.0f, 1.0f);
|
||
float blend = s_nav.cfg.link_wall_blend;
|
||
out->override_w = blend * w_wall + (1.0f - blend) * w_imu;
|
||
} else {
|
||
out->override_w = w_imu;
|
||
}
|
||
out->override_w = gnav_clampf(out->override_w, -1.0f, 1.0f);
|
||
}
|
||
|
||
/* 閲岀▼璁?+ 瓒呮椂鍒ゅ畾 */
|
||
if (odom_since_entry() >= s_nav.cfg.entry_distance ||
|
||
elapsed_ms > s_nav.cfg.entry_timeout_ms)
|
||
{
|
||
transition_to(GNAV_TURN_INTO_CORRIDOR, board);
|
||
}
|
||
break;
|
||
}
|
||
|
||
/* ============================================================
|
||
* 涓夌杞悜鐘舵€佺粺涓€澶勭悊
|
||
* ============================================================ */
|
||
case GNAV_TURN_INTO_CORRIDOR:
|
||
case GNAV_TURN_OUT_OF_CORRIDOR:
|
||
case GNAV_TURN_INTO_NEXT:
|
||
execute_turn(obs, state, board, now_ms, out);
|
||
break;
|
||
|
||
/* ============================================================
|
||
* 閲嶆崟鑾疯蛋寤? *
|
||
* 闂鑳屾櫙锛? * 杞集鍒氬畬鎴愭椂锛岃溅韬彲鑳借繕鍦ㄦ矡鍙ｅ锛堢閮ㄩ€氶亾鍐咃級锛? * 浣嗘鏃朵袱渚L53鍚屾牱鑳芥祴鍒颁袱渚у瀯鑳岀闈?鈮?0cm)锛? * 瀹藉害鍜?= 10+10+20 = 40cm锛屼笌鐪熸鍦ㄦ矡鍐呭畬鍏ㄤ竴鑷淬€? * 杩欏鑷村皻鏈叆娌熷氨婊¤冻閲嶆崟鑾锋潯浠讹紝鍒囧埌CORRIDOR_TRACK鍚? * EKF鐨別_y鍙兘鏄ぇ鍋忓樊锛宑orridor_ctrl杈撳嚭澶锛岃溅鍗℃鍦ㄥ叆鍙ｃ€? *
|
||
* 淇鏂规锛? * 寮曞叆 reacquire_min_depth 闂ㄦ锛? * 鍙湁閲岀▼璁℃樉绀鸿溅韬凡娣卞叆娌熷唴瓒冲璺濈鍚庯紝鎵嶅厑璁稿紑濮嬭鏁般€? * 杩欑‘淇濆洓棰梀L53鍏ㄩ儴鑴辩娌熷彛杈圭紭杩涘叆绋冲畾渚у鍖哄煙銆? * ============================================================ */
|
||
case GNAV_REACQUIRE:
|
||
out->override_v = s_nav.cfg.reacquire_v;
|
||
out->override_w = 0.0f; /* 涓嶅仛鑸悜鎺у埗锛岃杞﹁嚜鐒惰繘娌燂紝ALIGN闃舵鍐嶆憜姝?*/
|
||
out->safety_mode = SAFETY_MODE_STRAIGHT;
|
||
|
||
{
|
||
/* 鍏ユ矡娣卞害瀹堝崼锛氭湭杈惧埌鏈€灏忔繁搴﹀墠锛岀姝㈠紑濮嬬‘璁よ鏁?*/
|
||
bool depth_ok = (odom_since_entry() >= s_nav.cfg.reacquire_min_depth);
|
||
|
||
if (depth_ok && check_reacquire(obs, state)) {
|
||
s_nav.reacquire_ok_count++;
|
||
} else {
|
||
/* 娣卞害涓嶈冻 鎴?鏉′欢鏈弧瓒?鈫?璁℃暟涓ユ牸娓呴浂
|
||
* 娉ㄦ剰锛氭繁搴︿笉瓒虫椂涔熸竻闆讹紝闃叉鍦ㄦ矡鍙ｅ绉疮浜嗛儴鍒嗚鏁? * 鐒跺悗杩涙矡鍚庡彧闇€寰堝皯鍑犳媿灏辫Е鍙?*/
|
||
s_nav.reacquire_ok_count = 0;
|
||
}
|
||
}
|
||
|
||
if (s_nav.reacquire_ok_count >= s_nav.cfg.reacquire_confirm_ticks) {
|
||
/* 閲嶆崟鑾风‘璁?鈫?杩涘叆鍘熷湴瀵规闃舵锛屽仠杞︾敤VL53鎽嗘鍚庡啀鍓嶈繘 */
|
||
transition_to(GNAV_ALIGN, board);
|
||
}
|
||
if (elapsed_ms > s_nav.cfg.reacquire_timeout_ms) {
|
||
transition_to(GNAV_ERROR, board);
|
||
}
|
||
break;
|
||
|
||
/* ============================================================
|
||
* 鍘熷湴瀵规 (ALIGN)
|
||
*
|
||
* 瑙﹀彂鏃舵満锛歊EACQUIRE 纭鎴愬姛鍚庯紝鍋滆溅鍘熷湴鐢?EKF 鎽嗘杞﹁韩鍐嶅墠杩涖€? *
|
||
* 鎺у埗寰嬶細v = 0锛寃 = -(kp_th路e_th + kp_y路e_y)
|
||
*
|
||
* 涓轰粈涔堢敤 EKF 鐨?e_th/e_y 鑰屼笉鏄師濮?VL53 宸垎锛? * 鍘熷 VL53 鍓嶅悗宸及瑙掞細卤2cm / 12cm 鍩虹嚎 鈮?卤18掳 鍣０锛屽畬鍏ㄤ笉鍙敤銆? * EKF 鐨?e_th 鏄?IMU 楂樼簿搴︾Н鍒?+ 渚у妯悜鑱斿悎绾︽潫鐨勪骇鐗╋紝鍙俊銆? *
|
||
* 瀹屾垚鏉′欢锛殀e_th| < th_tol AND |e_y| < y_tol锛岃繛缁?N 鎷嶃€? * 瓒呮椂锛氱洿鎺ヨ繘娌燂紝涓嶉樆濉炴祦绋嬶紙瓒呮椂璇存槑VL53璐ㄩ噺宸紝浣嗕篃瑕佺户缁級銆? * ============================================================ */
|
||
case GNAV_ALIGN: {
|
||
out->override_v = 0.0f; /* 鍋滆溅 */
|
||
out->use_override = true;
|
||
out->request_corridor = false;
|
||
out->safety_mode = SAFETY_MODE_TURN; /* TURN妯″紡锛氬厑璁稿師鍦版棆杞紝璺宠繃鍓嶅悜纰版挒妫€鏌?*/
|
||
|
||
/* 瀵规鎺у埗寰嬶細浠呰閫熷害锛屾棤绾块€熷害 */
|
||
float w_align = -(s_nav.cfg.align_kp_th * state->e_th
|
||
+ s_nav.cfg.align_kp_y * state->e_y);
|
||
out->override_w = gnav_clampf(w_align, -1.0f, 1.0f);
|
||
|
||
/* 瀹屾垚鍒ゅ畾锛氬Э鎬佸拰浣嶇疆閮藉湪瀹瑰樊鍐?*/
|
||
bool th_ok = gnav_fabsf(state->e_th) < s_nav.cfg.align_th_tol_rad;
|
||
bool y_ok = gnav_fabsf(state->e_y) < s_nav.cfg.align_y_tol_m;
|
||
bool ekf_trusted = (state->conf >= s_nav.cfg.reacquire_conf_thresh);
|
||
|
||
if (th_ok && y_ok && ekf_trusted) {
|
||
s_nav.align_ok_count++;
|
||
} else {
|
||
s_nav.align_ok_count = 0;
|
||
}
|
||
|
||
if (s_nav.align_ok_count >= s_nav.cfg.align_confirm_ticks) {
|
||
/* 瀵规瀹屾垚锛岄『渚跨敤姝ゆ椂鐨?e_th 鏇存柊 IMU 鍙傝€冭 */
|
||
s_nav.heading_ref_deg += PARAM_RAD2DEG(state->e_th);
|
||
transition_to(GNAV_CORRIDOR_TRACK, board);
|
||
}
|
||
|
||
/* 瓒呮椂淇濇姢锛氫笉姝荤瓑锛岀洿鎺ヨ繘娌?*/
|
||
if (elapsed_ms > s_nav.cfg.align_timeout_ms) {
|
||
transition_to(GNAV_CORRIDOR_TRACK, board);
|
||
}
|
||
break;
|
||
}
|
||
|
||
/* ============================================================
|
||
* 娌熷唴闂幆璺熻釜 (浜ょ粰 corridor_ctrl)
|
||
* ============================================================ */
|
||
case GNAV_CORRIDOR_TRACK:
|
||
out->use_override = false;
|
||
out->request_corridor = true;
|
||
out->safety_mode = SAFETY_MODE_CORRIDOR;
|
||
|
||
/* [Phase-3] 娌熷唴 IMU 鍙傝€冩柟鍚戞參鏍℃
|
||
*
|
||
* 鍘熺悊锛欵KF 鐨?e_th 鍙嶆槧浜嗗綋鍓嶈埅鍚戠浉瀵逛簬 IMU 鍙傝€冭鐨勫亸宸€? * 濡傛灉 e_th 闀挎湡涓嶄负闆讹紙渚嬪鎸佺画 +2掳锛夛紝璇存槑 IMU 鍙傝€冭鏈夌郴缁熸€у亸宸€? * 鎺у埗鍣ㄨ櫧鐒惰兘閫氳繃 kp_theta 鎶?e_th 鍘嬩笅鍘伙紝浣嗚繖鎰忓懗鐫€鎺у埗鍣ㄤ竴鐩村湪
|
||
* "鎶楀亸宸?锛岃溅澶存寔缁亸鍚戜竴渚э紝妯悜浼氱紦鎱㈡紓绉汇€? *
|
||
* 瑙ｅ喅鏂规硶锛氭瘡鎷嶇敤鏋佸皬鐨?alpha 灏?e_th 鍙嶉鍒?heading_ref_deg 涓婏紝
|
||
* 璁╁弬鑰冭缂撴參瀵归綈鐪熷疄娌熻酱銆? *
|
||
* 涓轰粈涔堜笉鐢ㄤ晶澧欏墠鍚庡樊锛圴L53 宸垎娴嬭锛夛細
|
||
* VL53 鏈?卤2cm 涓綋璇樊锛屽熀绾?12cm
|
||
* 鏈€鍧忓櫔澹?= atan2(4cm, 12cm) 鈮?18掳
|
||
* 鍗充娇鍙栧钩鍧囦篃鏈?~9掳 鍣０锛屼笉閫傚悎鍋氳搴︿及璁? *
|
||
* 涓轰粈涔堢敤 EKF e_th锛? * e_th 鏄?IMU 楂樼簿搴︾Н鍒?+ 渚у妯悜瑙傛祴鑱斿悎绾︽潫鐨勪骇鐗? * 濡傛灉鍙屼晶瑙傛祴绋冲畾锛坈onf 楂橈級锛宔_th 鐨勮秼鍔挎槸鍙俊鐨? *
|
||
* 闂ㄦ帶鏉′欢锛? * - EKF conf >= corridor_ref_correct_conf锛堥粯璁?0.75锛? * - alpha 鏋佸皬锛?.001锛夛紝鏃堕棿甯告暟 ~1s锛屼笉浼氳鐬椂鍣０甯﹁窇
|
||
*/
|
||
if (s_nav.cfg.corridor_ref_correct_alpha > 0.0f &&
|
||
state->conf >= s_nav.cfg.corridor_ref_correct_conf)
|
||
{
|
||
float eth_deg = PARAM_RAD2DEG(state->e_th);
|
||
s_nav.heading_ref_deg += s_nav.cfg.corridor_ref_correct_alpha * eth_deg;
|
||
}
|
||
|
||
/* 鍒扮妫€娴?*/
|
||
{
|
||
bool front_valid = (obs->valid_mask & CORRIDOR_OBS_MASK_FRONT) != 0U;
|
||
if (front_valid && obs->d_front <= s_nav.cfg.corridor_end_detect_dist) {
|
||
/* 閲岀▼涓嬮檺淇濇姢: 鑷冲皯璧颁簡 1.0m 鎵嶅厑璁歌瀹氬埌绔紝閬垮厤鍋囬槼鎬?*/
|
||
float corridor_odom = odom_since_entry();
|
||
if (corridor_odom > 1.0f) {
|
||
s_nav.corridors_completed++;
|
||
transition_to(GNAV_TURN_OUT_OF_CORRIDOR, board);
|
||
}
|
||
}
|
||
}
|
||
/* 閲岀▼瓒呴暱淇濇姢 */
|
||
if (odom_since_entry() > s_nav.cfg.corridor_length_max) {
|
||
s_nav.corridors_completed++;
|
||
transition_to(GNAV_TURN_OUT_OF_CORRIDOR, board);
|
||
}
|
||
break;
|
||
|
||
/* ============================================================
|
||
* 杩炴帴娈电洿琛?(绾靛悜绔儴閫氶亾鍖楄, 鏂规2: 涓変俊鍙疯仈鍚堝垽瀹?
|
||
*
|
||
* 浼犳劅鍣ㄦ儏鍐碉紙杞悜瀹屾垚鍚庨潰鏈濆寳锛夛細
|
||
* - 鍓嶆縺鍏夛紙鏈濆寳锛夆啋 涓婂洿鏍忥紝d_front 闅忓寳琛岄€掑噺 [绮惧害楂榏
|
||
* - 鍚庢縺鍏夛紙鏈濆崡锛夆啋 涓嬪洿鏍忥紝d_back 闅忓寳琛岄€掑 [绮惧害楂榏
|
||
* - 鍥存爮渚?VL53 鈫?濮嬬粓鏈夋晥 (~10cm) [涓嶇敤浜庡垽瀹歖
|
||
* - 闈炲洿鏍忎晶 VL53 鈫?璐村瀯鑳岀闈㈡椂鏈夋晥锛屽埌鍨勬矡寮€鍙ｆ椂涓㈠け [娌熷彛鏍囧織]
|
||
*
|
||
* 淇″彿瀹氫箟锛? * A: 閲岀▼璁? odom >= link_distance * 0.85 (鎵撴粦琛板噺, 鏉冮噸浣?
|
||
* B: 鍓嶆縺鍏夊彉鍖? d_front缂╁皬 >= link_distance * 0.85 (鏉冮噸楂?
|
||
* C: 闈炲洿鏍忎晶VL53 涓㈠け/璺冲埌>50cm锛岃繛缁?鎷嶇‘璁? (鐩存帴鎺㈡祴娌熷彛, 鏉冮噸涓?
|
||
*
|
||
* 瑙﹀彂閫昏緫: B || (A && C)
|
||
* - 鍓嶆縺鍏夊彉鍖栭噺瓒冲 鈫?鐩存帴瑙﹀彂锛堟渶鍙潬鐨勫崟涓€淇″彿锛? * - 閲岀▼璁″埌浣?+ VL53鎺㈠埌娌熷彛 鈫?鑱斿悎瑙﹀彂锛堜簰鐩告牎楠岋級
|
||
* ============================================================ */
|
||
case GNAV_LINK_STRAIGHT:
|
||
out->override_v = s_nav.cfg.link_v;
|
||
out->safety_mode = SAFETY_MODE_STRAIGHT;
|
||
|
||
/* [Phase-1] 鍥存爮渚у崟杈硅创澧?+ IMU 娣峰悎鎺у埗
|
||
*
|
||
* 鍘熸潵锛氱函 IMU heading_hold_pd()
|
||
* 鐜板湪锛氬鏋滃洿鏍忎晶 VL53 鏈夋晥锛岀敤鍥存爮璺濈鍋氭í鍚戠害鏉燂紝涓?IMU 娣峰悎
|
||
* 濡傛灉鍥存爮渚?VL53 鍏ㄩ儴鏃犳晥锛岄€€鍖栦负鍘熸潵鐨勭函 IMU
|
||
*
|
||
* 涓轰粈涔堜笉鐢ㄥ洿鏍忓墠鍚庡樊鍋氳埅鍚戠害鏉燂細
|
||
* VL53 鏈?卤2cm 涓綋璇樊锛屽熀绾夸粎 12cm 鈫?atan2(卤4cm, 12cm) = 卤18掳
|
||
* 鍣０澶ぇ锛屼笉閫傚悎鍋氳埅鍚戞帶鍒躲€傚彧鐢ㄥ钩鍧囪窛绂诲仛妯悜绾︽潫銆? */
|
||
{
|
||
float w_imu = heading_hold_pd(imu_yaw_deg, s_nav.heading_ref_deg,
|
||
s_nav.cfg.heading_kp);
|
||
|
||
const CorridorDescriptor_t* cd = TrackMap_GetCorridor(s_nav.current_corridor_id);
|
||
float d_fence_avg = 0.0f;
|
||
float fence_sign = 0.0f;
|
||
bool fence_valid = get_fence_side_distance(obs, cd->travel_dir,
|
||
&d_fence_avg, &fence_sign);
|
||
|
||
if (fence_valid) {
|
||
/* 妯悜鍋忓樊 = 瀹為檯璺濈 - 鐩爣璺濈
|
||
* 姝ｅ€?= 绂诲洿鏍忓お杩?鈫?fence_sign 鍐冲畾鏍℃鏂瑰悜 */
|
||
float ey_wall = d_fence_avg - s_nav.cfg.link_wall_target_d;
|
||
float w_wall = fence_sign * s_nav.cfg.link_wall_kp_y * ey_wall;
|
||
w_wall = gnav_clampf(w_wall, -1.0f, 1.0f);
|
||
|
||
/* 娣峰悎锛氬洿鏍忕害鏉熶负涓?+ IMU 骞虫粦涓鸿緟 */
|
||
float blend = s_nav.cfg.link_wall_blend;
|
||
out->override_w = blend * w_wall + (1.0f - blend) * w_imu;
|
||
} else {
|
||
/* 鍥存爮渚?VL53 鍏ㄩ儴鏃犳晥 鈫?閫€鍖栦负绾?IMU锛堜笌鍘熻涓轰竴鑷达級 */
|
||
out->override_w = w_imu;
|
||
}
|
||
|
||
out->override_w = gnav_clampf(out->override_w, -1.0f, 1.0f);
|
||
|
||
bool front_valid = (obs->valid_mask & CORRIDOR_OBS_MASK_FRONT) != 0U;
|
||
|
||
/* 棣栨媿璁板綍鍓嶆縺鍏夊熀鍑嗗€?*/
|
||
if (!s_nav.link_d_front_valid && front_valid) {
|
||
s_nav.link_d_front_start = obs->d_front;
|
||
s_nav.link_d_front_valid = true;
|
||
}
|
||
|
||
/* ---- 淇″彿 A: 閲岀▼璁?(鑰冭檻鎵撴粦, 鐢?85% 瀹瑰樊) ---- */
|
||
bool odom_ok = odom_since_entry() >= s_nav.cfg.link_distance * 0.85f;
|
||
|
||
/* ---- 淇″彿 B: 鍓嶆縺鍏夊彉鍖栭噺 (楂樼簿搴? ---- */
|
||
bool laser_ok = false;
|
||
if (s_nav.link_d_front_valid && front_valid) {
|
||
float d_front_delta = s_nav.link_d_front_start - obs->d_front;
|
||
/* 杞﹁韩涓績鍒板墠婵€鍏夋湁鍋忕疆(FRONT_LASER_OFFSET)锛屼絾杩欓噷鐢ㄧ殑鏄樊鍊硷紝鍋忕疆鎶垫秷 */
|
||
laser_ok = (d_front_delta >= s_nav.cfg.link_distance * 0.85f);
|
||
}
|
||
|
||
/* ---- 淇″彿 C: 闈炲洿鏍忎晶 VL53 娌熷彛妫€娴?(闇€杩炵画2鎷嶇‘璁? ----
|
||
*
|
||
* 鍒ゅ畾闃堝€?0.5m 鐨勬潵婧?
|
||
* 姝ｅ父璐村瀯鑳岀闈㈡椂 VL53 璇绘暟 鈮?(閫氶亾瀹?2 - 杞﹀/2 - VL53鍐呯缉)
|
||
* = (0.40/2 - 0.20/2 - 0.0)
|
||
* = 0.10m
|
||
* 鍒板瀯娌熷紑鍙ｆ椂 VL53 璇绘暟 > 1.2m (瓒呭嚭鏈夋晥璺濈) 鎴栨棤鏁? * 闃堝€?0.5m 鍦ㄤ袱鑰呬箣闂达紝瓒冲鍖哄垎
|
||
*/
|
||
/* cd 宸插湪涓婃柟鍥存爮渚ч€昏緫涓幏鍙栵紝澶嶇敤 */
|
||
/* 鍦?LINK_STRAIGHT 闃舵锛宑urrent_corridor_id 浠嶆槸鍒氳蛋瀹岀殑娌?(灏氭湭鏇存柊)
|
||
鎵€浠?cd->travel_dir 灏辨槸鍒氳蛋瀹岄偅鏉℃矡鐨勬柟鍚戯紝鐩存帴鐢ㄦ潵鍒ゆ柇褰撳墠鍦ㄥ摢涓閮ㄩ€氶亾 */
|
||
bool gap_now = gap_detected_on_open_side(obs, cd->travel_dir);
|
||
|
||
if (gap_now) {
|
||
if (s_nav.link_gap_count < 255) s_nav.link_gap_count++;
|
||
} else {
|
||
s_nav.link_gap_count = 0;
|
||
}
|
||
bool gap_confirmed = (s_nav.link_gap_count >= 2); /* 杩炵画2鎷?(40ms @ 20ms鍛ㄦ湡) */
|
||
|
||
/* ---- 鑱斿悎鍒ゅ畾: B || (A && C) ---- */
|
||
if (laser_ok || (odom_ok && gap_confirmed))
|
||
{
|
||
transition_to(GNAV_TURN_INTO_NEXT, board);
|
||
}
|
||
}
|
||
if (elapsed_ms > s_nav.cfg.link_timeout_ms) {
|
||
transition_to(GNAV_ERROR, board);
|
||
}
|
||
break;
|
||
|
||
/* ============================================================
|
||
* 鍑哄満鐩磋
|
||
* ============================================================ */
|
||
case GNAV_EXIT_STRAIGHT:
|
||
out->override_v = s_nav.cfg.exit_v;
|
||
out->safety_mode = SAFETY_MODE_STRAIGHT;
|
||
|
||
/* [Phase-1] 鍑哄満娈典篃浣跨敤鍥存爮渚ц创澧欐帶鍒? * C6 鍑烘矡鍚庡湪宸︾閫氶亾鍗楄锛屽洿鏍忓湪宸︿晶锛堜笌 LINK_STRAIGHT 鍏辩敤閫昏緫锛? * 璺濈 ~3.9m锛屾瘮杩炴帴娈甸暱寰楀锛屽洿鏍忕害鏉熸洿鏈変环鍊? */
|
||
{
|
||
float w_imu = heading_hold_pd(imu_yaw_deg, s_nav.heading_ref_deg,
|
||
s_nav.cfg.heading_kp);
|
||
|
||
const CorridorDescriptor_t* cd_exit = TrackMap_GetCorridor(s_nav.current_corridor_id);
|
||
float d_fence_avg = 0.0f;
|
||
float fence_sign = 0.0f;
|
||
bool fence_valid = get_fence_side_distance(obs, cd_exit->travel_dir,
|
||
&d_fence_avg, &fence_sign);
|
||
|
||
if (fence_valid && !s_nav.exit_vl53_lost) {
|
||
float ey_wall = d_fence_avg - s_nav.cfg.link_wall_target_d;
|
||
float w_wall = fence_sign * s_nav.cfg.link_wall_kp_y * ey_wall;
|
||
w_wall = gnav_clampf(w_wall, -1.0f, 1.0f);
|
||
float blend = s_nav.cfg.link_wall_blend;
|
||
out->override_w = blend * w_wall + (1.0f - blend) * w_imu;
|
||
} else {
|
||
out->override_w = w_imu;
|
||
}
|
||
out->override_w = gnav_clampf(out->override_w, -1.0f, 1.0f);
|
||
}
|
||
|
||
/* 妫€娴嬩晶鍚戝叏涓?*/
|
||
if (!s_nav.exit_vl53_lost && all_side_lost(obs)) {
|
||
s_nav.exit_vl53_lost = true;
|
||
s_nav.exit_lost_distance = s_nav.odom_distance_accum;
|
||
}
|
||
if (s_nav.exit_vl53_lost) {
|
||
float since_lost = s_nav.odom_distance_accum - s_nav.exit_lost_distance;
|
||
if (since_lost >= s_nav.cfg.exit_runout) {
|
||
transition_to(GNAV_DOCK, board);
|
||
}
|
||
}
|
||
/* 閲岀▼涓婇檺淇濇姢 */
|
||
if (odom_since_entry() >= s_nav.cfg.exit_max_dist) {
|
||
transition_to(GNAV_DOCK, board);
|
||
}
|
||
if (elapsed_ms > s_nav.cfg.exit_timeout_ms) {
|
||
transition_to(GNAV_DOCK, board);
|
||
}
|
||
break;
|
||
|
||
/* ============================================================
|
||
* 鍥炲仠鍚姩鍖? * ============================================================ */
|
||
case GNAV_DOCK:
|
||
out->override_v = s_nav.cfg.dock_v;
|
||
out->override_w = 0.0f;
|
||
out->safety_mode = SAFETY_MODE_STRAIGHT;
|
||
|
||
if (odom_since_entry() >= s_nav.cfg.dock_distance ||
|
||
elapsed_ms > 5000U)
|
||
{
|
||
transition_to(GNAV_FINISHED, board);
|
||
}
|
||
break;
|
||
|
||
/* ============================================================
|
||
* 缁堟€? * ============================================================ */
|
||
case GNAV_FINISHED:
|
||
out->override_v = 0.0f;
|
||
out->override_w = 0.0f;
|
||
out->safety_mode = SAFETY_MODE_IDLE;
|
||
out->active = false;
|
||
break;
|
||
|
||
/* ============================================================
|
||
* 寮傚父鎬? * ============================================================ */
|
||
case GNAV_ERROR:
|
||
out->override_v = 0.0f;
|
||
out->override_w = 0.0f;
|
||
out->safety_mode = SAFETY_MODE_IDLE;
|
||
|
||
if (elapsed_ms > 2000U) {
|
||
transition_to(GNAV_FINISHED, board);
|
||
}
|
||
break;
|
||
|
||
default:
|
||
out->override_v = 0.0f;
|
||
out->override_w = 0.0f;
|
||
break;
|
||
}
|
||
|
||
/* 鏇存柊杈撳嚭闃舵 (鍙兘鍦?switch 鍐呭凡缁?transition) */
|
||
out->stage = s_nav.stage;
|
||
out->stage_name = GlobalNav_GetStageName(s_nav.stage);
|
||
out->corridor_id = s_nav.current_corridor_id;
|
||
out->corridors_done = s_nav.corridors_completed;
|
||
out->active = s_nav.running;
|
||
}
|