ASER/temp_promax_nav.c

<EFBFBD><EFBFBD>/**
 * @file   global_nav.c
 * @brief  <EFBFBD>t<EFBFBD>m<EFBFBD>N<EFBFBD>~D<EFBFBD> <EFBFBD>~6^ %<EFBFBD>?#Z<EFBFBD>\<EFBFBD>`5pp<EFBFBD><EFBFBD>W<EFBFBD>5<EFBFBD><EFBFBD> yO<EFBFBD>n9p<EFBFBD>pG^
 *
 *         f<EFBFBD><EFBFBD>Ttn
<EFBFBD>d/p: h<EFBFBD><EFBFBD>R<EFBFBD>wZ~^g?<EFBFBD>YD<EFBFBD><EFBFBD>`)R<EFBFBD><EFBFBD>U<EFBFBD>z<EFBFBD>}\ONY<EFBFBD>EQ<EFBFBD><EFBFBD><EFBFBD>~<EFBFBD><EFBFBD>`ȓY<EFBFBD>eZ<EFBFBD> blக)1<EFBFBD> 6l<EFBFBD>N
 *         Z<EFBFBD><EFBFBD><EFBFBD>YV<EFBFBD><EFBFBD>Tjn<EFBFBD>[?<EFBFBD>{Yt<EFBFBD>c}O<EFBFBD><EFBFBD>0[_5pR_<EFBFBD>}<EFBFBD>[><EFBFBD>l૕6l<EFBFBD>N
 *
 *         S h<EFBFBD>.[<EFBFBD>NX<EFBFBD>?
 *           O<EFBFBD><EFBFBD>0<EFBFBD>n(V<EFBFBD>Di<EFBFBD>)+<EFBFBD><EFBFBD>cx_^g<EFBFBD><EFBFBD>SC1(+<EFBFBD><EFBFBD>c"{)+<2B><>cx_<78>~<1A><>W<EFBFBD>~<17>KU<4B>[A<>Fm+<2B><>c<EFBFBD>[tx\KU<EFBFBD>[A<>FmO<6D>I<EFBFBD>2(+<2B>/a<>0)
 *           +<EFBFBD><EFBFBD>cON<EFBFBD>~<EFBFBD><EFBFBD>W<EFBFBD>~<EFBFBD>KUY<EFBFBD><EFBFBD>[Fm+<EFBFBD><EFBFBD>c<EFBFBD>[tx\KUY<EFBFBD><EFBFBD>[FmO<EFBFBD>I<EFBFBD>3(+<EFBFBD><EFBFBD>c"{)+<2B>?..+<2B>Vb6(+<2B>/a<>0)
 *           +<EFBFBD><EFBFBD>cON<EFBFBD>~<EFBFBD><EFBFBD>W<EFBFBD>~<EFBFBD>KU<EFBFBD>[A<EFBFBD>Fm(ȓ<EFBFBD>o!])+<EFBFBD><EFBFBD>c!]t}\<EFBFBD>Vf<EFBFBD>a<EFBFBD>KUe<EFBFBD><EFBFBD>p<EFBFBD>NZ<EFBFBD><EFBFBD><EFBFBD>YV<EFBFBD>? *
 *         <EFBFBD>~<EFBFBD>4Q<EFBFBD><EFBFBD>6l<EFBFBD>N<EFBFBD>-WcP:
 *           - <EFBFBD>m<EFBFBD> nFRe<EFBFBD>X[.r(VL53s<EFBFBD><EFBFBD><EFBFBD>tyR<EFBFBD>?, Y<EFBFBD>?<EFBFBD><EFBFBD>znB&lǓ<EFBFBD>W<EFBFBD>V<EFBFBD>g/ps<EFBFBD><EFBFBD>\l<EFBFBD>ȕ 2<EFBFBD>bh<EFBFBD><EFBFBD>R<EFBFBD>w<EFBFBD>[<EFBFBD> Y<EFBFBD>? *           - <EFBFBD>m<EFBFBD>]XQn<EFBFBD>o<EFBFBD>yY<EFBFBD>|\vfVL53K<EFBFBD>5kKF, G<EFBFBD>tT0"<EFBFBD><EFBFBD>!MUx<>D<EFBFBD><44>`<60>m<EFBFBD>o<EFBFBD>[+S<><53>]<5D>`uZ<75> O<>
Y<EFBFBD>W<EFBFBD>~<EFBFBD><EFBFBD><EFBFBD>4Z? */

#include "global_nav.h"
#include "est/corridor_filter.h"
#include "preproc/corridor_preproc.h"
#include "robot_params.h"
#include <math.h>
#include <string.h>

/* =========================================================
 * P<EFBFBD>tT4Q<EFBFBD>5<EFBFBD><EFBFBD> ? * ========================================================= */
static struct {
    GlobalNavStage_t stage;
    bool             running;
    bool             initialized;

    /* <EFBFBD>t<EFBFBD>m<EFBFBD>N<EFBFBD>~?*/
    uint8_t  current_corridor_id;
    uint8_t  corridors_completed;

    /* ^g<EFBFBD><EFBFBD>` */
    float    turn_start_yaw_deg;        /* IMU yaw_continuous at turn start */
    float    turn_target_delta_deg;     /* 90<EFBFBD>c */
    int8_t   turn_sign;                 /* +1 (CCW) or -1 (CW) */

    /* <EFBFBD><EFBFBD><EFBFBD>\<EFBFBD>% ("<EFBFBD>)1wV<77>~-[x<><78>~<1A><>W<EFBFBD>t<EFBFBD>o<EFBFBD><6F>) */
    float    stage_entry_odom_vx_accum; /* ig<EFBFBD>m<EFBFBD>SÕ5<EFBFBD><EFBFBD><EFBFBD>ÓAR<EFBFBD>k<EFBFBD><EFBFBD><EFBFBD>\<EFBFBD>%<EFBFBD>t<EFBFBD><EFBFBD>u<EFBFBD>t<EFBFBD><EFBFBD>z<EFBFBD>~?*/
    float    odom_distance_accum;       /* ig/a<EFBFBD><EFBFBD>m_<EFBFBD><EFBFBD>[
`<EFBFBD>R<EFBFBD><EFBFBD>U<EFBFBD>k<EFBFBD><EFBFBD><EFBFBD>\<EFBFBD>% */

    /* <EFBFBD>tnTi */
    uint32_t stage_start_ms;

    /* <EFBFBD><EFBFBD><EFBFBD>]]~<EFBFBD>?*/
    uint8_t  reacquire_ok_count;

    /* X<EFBFBD><EFBFBD>qtn5pĉ<EFBFBD> */
    uint8_t  align_ok_count;

    /* Q<EFBFBD><EFBFBD>T<EFBFBD>n */
    bool     exit_vl53_lost;
    float    exit_lost_distance;

    /* x<EFBFBD>D<EFBFBD><EFBFBD>`<EFBFBD>m<EFBFBD>o<EFBFBD>[ */
    float    heading_ref_deg;

    /* ig<EFBFBD>p4^Z? <EFBFBD>o+l6}<EFBFBD><EFBFBD><EFBFBD>q<EFBFBD>jHgmT<EFBFBD>Y */
    float    link_d_front_start;    /* ig<EFBFBD>m<EFBFBD>Sig<EFBFBD>p4^Z<EFBFBD>WiS<EFBFBD><EFBFBD>]:~O<EFBFBD>Y<EFBFBD><EFBFBD><EFBFBD><EFBFBD>?(m) */
    bool     link_d_front_valid;    /* ig<EFBFBD>m<EFBFBD>SÓ<EFBFBD><EFBFBD><EFBFBD>XuZ<EFBFBD> O<EFBFBD>Y<EFBFBD>iZ<EFBFBD>=<EFBFBD>An<EFBFBD><EFBFBD>?*/
    uint8_t  link_gap_count;        /* ȕ<EFBFBD>p?m͓<EFBFBD>_vf VL53 ig<EFBFBD>p;u<EFBFBD>m 2Q0<EFBFBD>t!<EFBFBD>f (Z<EFBFBD>q[_<EFBFBD>~<EFBFBD><EFBFBD>{<EFBFBD>) */

    /* EKF ig<EFBFBD>m<EFBFBD>[<EFBFBD>m<EFBFBD>o<EFBFBD>t */
    float    corridor_entry_s;

    /* <EFBFBD><EFBFBD><EFBFBD>]<EFBFBD>u */
    GlobalNavConfig_t cfg;
} s_nav;

/* <EFBFBD>mAZ<EFBFBD>z<EFBFBD>mD<EFBFBD><EFBFBD>aȓ<EFBFBD>q<EFBFBD>k<EFBFBD><EFBFBD><EFBFBD>\<EFBFBD>%<EFBFBD>t<EFBFBD><EFBFBD><EFBFBD> <EFBFBD>q<EFBFBD>[<EFBFBD><EFBFBD>\de\m<EFBFBD>^R<EFBFBD>?*/
static float s_last_odom_vx = 0.0f;
static uint32_t s_last_update_ms = 0;

/* =========================================================
 * HgmT<EFBFBD>YQ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>f
 * ========================================================= */
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;
}

/** <EFBFBD>~<EFBFBD> W<EFBFBD>?P <EFBFBD><EFBFBD>C<EFBFBD>Wx<EFBFBD>D<EFBFBD><EFBFBD>`<EFBFBD>m<EFBFBD>o<EFBFBD>[<EFBFBD><EFBFBD>\<EFBFBD>}O<EFBFBD><EFBFBD>0<EFBFBD>N<EFBFBD>[?(deg)<EFBFBD><EFBFBD>\<EFBFBD>}Q<EFBFBD>?<EFBFBD>W竕<EFBFBD>q<EFBFBD>[ (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 <EFBFBD><EFBFBD>?<EFBFBD>c ē<EFBFBD>r<EFBFBD>vR<EFBFBD>?rad/s */
    return gnav_clampf(w, -1.0f, 1.0f);
}

/** <EFBFBD>Y<EFBFBD> ̓<EFBFBD>0vfZ<EFBFBD>?VL53 ē<EFBFBD><EFBFBD>`<EFBFBD><EFBFBD> 2<EFBFBD>W<EFBFBD>o?(w<EFBFBD><EFBFBD>Q<EFBFBD>vȓ	Y<EFBFBD>znE<EFBFBD>kS<EFBFBD><EFBFBD>]<EFBFBD>`<EFBFBD><EFBFBD><EFBFBD><EFBFBD>An<EFBFBD><EFBFBD>? %<EFBFBD>?`moTde\m<EFBFBD>^xV<EFBFBD><EFBFBD><EFBFBD>fO^ */
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 <EFBFBD>Y<EFBFBD> 4Z.[joe<EFBFBD>X[.rn?VL53 ē<EFBFBD><EFBFBD>`Y<EFBFBD> bG^h<EFBFBD><EFBFBD>R<EFBFBD>w<EFBFBD>[<EFBFBD> Y<EFBFBD>?(ig<EFBFBD>p4^Z<EFBFBD>W<EFBFBD>wY<EFBFBD>H<EFBFBD><EFBFBD>}T<EFBFBD>A%<EFBFBD>W9p?
 *
 * f<EFBFBD>'1<EFBFBD>eZ<EFBFBD> blக)1<EFBFBD> 6l<EFBFBD>Nȕ!2^nV<EFBFBD>Di<EFBFBD>9i5<EFBFBD>i<EFBFBD>? *   - R<EFBFBD>3lˆ9p}\<EFBFBD>`<EFBFBD>m?EAST)(<EFBFBD><EFBFBD>R<EFBFBD>w +<EFBFBD>?0<EFBFBD><EFBFBD>d<EFBFBD>Xf<EFBFBD>%1x_<EFBFBD>~<EFBFBD><EFBFBD> 6l<EFBFBD>N +<EFBFBD>?Y<EFBFBD>EQvf<EFBFBD>tQg?m͓?+<EFBFBD>?<EFBFBD>Y<EFBFBD> ̓<EFBFBD>0ONn<EFBFBD>L53
 *   - R<EFBFBD>3lˆ9p}\<EFBFBD>`Wt?WEST)(<EFBFBD><EFBFBD>R<EFBFBD>w +<EFBFBD>?0<EFBFBD><EFBFBD>d<EFBFBD>Xf<EFBFBD>%1ON<EFBFBD>~<EFBFBD><EFBFBD> 6l<EFBFBD>N +<EFBFBD>?<EFBFBD>[?<EFBFBD>vf<EFBFBD>tQg?m͓?+<EFBFBD>?<EFBFBD>Y<EFBFBD> ̓<EFBFBD>0x_n<EFBFBD>L53
 *
 * 
<EFBFBD>_C~h<EFBFBD><EFBFBD>RQ<EFBFBD>~<EFBFBD>poÓ? VL53 4Z*[<EFBFBD>W<EFBFBD>~?(<EFBFBD><EFBFBD>6l<EFBFBD>N9p?2 - ^g@<EFBFBD><EFBFBD><EFBFBD>/2 - VL53P<EFBFBD>oT	) .<EFBFBD>?10cm +<EFBFBD>?ȓYef
 * 
<EFBFBD>_C~h<EFBFBD><EFBFBD>R<EFBFBD>w<EFBFBD>[<EFBFBD> Y<EFBFBD>F<EFBFBD>i: VL53 Op<EFBFBD>R<EFBFBD>SZ<EFBFBD>q4U 220cm+ +<EFBFBD>?<EFBFBD>tmT<EFBFBD>VȓYef<EFBFBD>t<EFBFBD>o<EFBFBD><EFBFBD> +<EFBFBD>?Ó<EFBFBD>ref<EFBFBD><EFBFBD>,h<EFBFBD><EFBFBD><EFBFBD><EFBFBD>?> 1.2m
 *
 * e<EFBFBD><EFBFBD>r<EFBFBD> VL53 S<EFBFBD><EFBFBD>]<EFBFBD>`<EFBFBD>m<EFBFBD>0|<EFBFBD>•<EFBFBD>x<EFBFBD>z 12cm<EFBFBD><EFBFBD>\<EFBFBD>n<EFBFBD><EFBFBD>?20cm 9p<EFBFBD>e}Z<EFBFBD>q[_ 40cm 9p<EFBFBD>e}
 * 0<EFBFBD><EFBFBD>d<EFBFBD>n엡<EFBFBD>Q<EFBFBD>G<EFBFBD><EFBFBD>Q<EFBFBD><EFBFBD>Q<EFBFBD><EFBFBD>U<EFBFBD>wY<EFBFBD>F<EFBFBD>i<EFBFBD>}\<EFBFBD>XZ<EFBFBD>?VL53 <EFBFBD><EFBFBD>Ʌ<EFBFBD>Qig<EFBFBD>m<EFBFBD>S<EFBFBD>[<EFBFBD> Y<EFBFBD>E<EFBFBD>/\i<EFBFBD><EFBFBD>q}<EFBFBD>m<EFBFBD>0|<EFBFBD>g<EFBFBD><EFBFBD>V2|<EFBFBD>m 2Q0<EFBFBD><EFBFBD>? * cm<EFBFBD>UC~Z3 /\ȓY<EFBFBD>z;jef4d<EFBFBD>e}<EFBFBD><EFBFBD><EFBFBD> `m<EFBFBD>0g_UtyO0w "S<><53>]<5D>`w<><77>Q<EFBFBD>v<EFBFBD>m<EFBFBD> <20>h>i<>m<EFBFBD>o? W<><57>[K<><4B>m<EFBFBD>p0^R<>Hr<48>wY<77>C<EFBFBD><43> ? *
 * @param obs     0<EFBFBD><EFBFBD>d<EFBFBD>XYt<EFBFBD>Pty
 * @param prev_travel_dir  R<EFBFBD>3lˆ9p<EFBFBD>\<EFBFBD>k<EFBFBD><EFBFBD>F<EFBFBD>ooZ<EFBFBD>q<EFBFBD>kt<EFBFBD>\%“pt<EFBFBD>` (EAST/WEST)
 * @return true = ȕ<EFBFBD>p?m͓<EFBFBD>_vf<EFBFBD>Y<EFBFBD> 4Z*[<EFBFBD>W<EFBFBD>[<EFBFBD> Y<EFBFBD>? */
static bool gap_detected_on_open_side(const CorridorObs_t* obs,
                                       TravelDirection_t prev_travel_dir)
{
    if (prev_travel_dir == TRAVEL_DIR_EAST) {
        /* f<EFBFBD>%1x_<EFBFBD>~<EFBFBD><EFBFBD> 6l<EFBFBD>N<EFBFBD>}\x_nFRe<EFBFBD>X[.r +<EFBFBD>?<EFBFBD>Y<EFBFBD> ̓<EFBFBD>0ONn?VL53 */
        bool lf_lost = !(obs->valid_mask & CORRIDOR_OBS_MASK_LF)
                       || (obs->d_lf > 0.5f);  /* >50cm Yt<EFBFBD>U<EFBFBD>Z<EFBFBD>q[_ (<EFBFBD>YE<EFBFBD>6r<EFBFBD>tQg<EFBFBD><EFBFBD><EFBFBD>~?0cm) */
        bool lr_lost = !(obs->valid_mask & CORRIDOR_OBS_MASK_LR)
                       || (obs->d_lr > 0.5f);
        return lf_lost || lr_lost;
    } else {
        /* f<EFBFBD>%1ON<EFBFBD>~<EFBFBD><EFBFBD> 6l<EFBFBD>N<EFBFBD>}\ONnFRe<EFBFBD>X[.r +<EFBFBD>?<EFBFBD>Y<EFBFBD> ̓<EFBFBD>0x_n?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] ~<EFBFBD><EFBFBD>\G_e<EFBFBD>X[.rn?VL53 <EFBFBD><EFBFBD><EFBFBD>QNo<EFBFBD>t<EFBFBD>o<EFBFBD><EFBFBD>
 *
 * f<EFBFBD>'1lக)1<EFBFBD> 6l<EFBFBD>NP<EFBFBD><EFBFBD>T}e<EFBFBD>X[.rnEqeR<EFBFBD>3lˆ9p<EFBFBD>\<EFBFBD>kZ<EFBFBD>q<EFBFBD>kt<EFBFBD>\%“pt<EFBFBD>`P<EFBFBD><EFBFBD>Q~u<EFBFBD>? *   EAST +<EFBFBD>?f<EFBFBD>%1x_<EFBFBD>~<EFBFBD><EFBFBD> 6l<EFBFBD>N +<EFBFBD>?e<EFBFBD>X[.rf<EFBFBD>%1x_n? *   WEST +<EFBFBD>?f<EFBFBD>%1ON<EFBFBD>~<EFBFBD><EFBFBD> 6l<EFBFBD>N +<EFBFBD>?e<EFBFBD>X[.rf<EFBFBD>%1ONn? *
 * e<EFBFBD>X[.rnB<EFBFBD><EFBFBD><EFBFBD>h?VL53 Y<EFBFBD>'h<EFBFBD><EFBFBD>g<EFBFBD><EFBFBD>Vr_Op?dS2cm <EFBFBD>mC<EFBFBD><EFBFBD>}<EFBFBD>t<EFBFBD>
jĕ<EFBFBD>]<EFBFBD>}R<EFBFBD>?dS1.4cm<EFBFBD><EFBFBD>? * `mlT<EFBFBD>z<EFBFBD>hAiAn<EFBFBD><EFBFBD>Xi<EFBFBD>m<EFBFBD>qQ~e<EFBFBD><EFBFBD>p<EFBFBD><EFBFBD><EFBFBD>hDi<EFBFBD><EFBFBD><EFBFBD><EFBFBD>.^}<EFBFBD>~<EFBFBD>`<EFBFBD>[<EFBFBD>~<EFBFBD>]<EFBFBD>}cm<EFBFBD>UAns<EFBFBD><EFBFBD>n,|Ó<EFBFBD>r}<EFBFBD><EFBFBD>? *
 * @param obs           0<EFBFBD><EFBFBD>d<EFBFBD>XYt<EFBFBD>Pty
 * @param travel_dir    R<EFBFBD>3lˆ9p<EFBFBD>\<EFBFBD>kZ<EFBFBD>q<EFBFBD>kt<EFBFBD>\%“pt<EFBFBD>`
 * @param[out] d_avg    e<EFBFBD>X[.rnC<EFBFBD><EFBFBD>g<EFBFBD><EFBFBD>V<EFBFBD>z<EFBFBD>~?(m)
 * @param[out] fence_sign  <EFBFBD>YD<EFBFBD><EFBFBD>`͓!<EFBFBD>“pt<EFBFBD>`<EFBFBD>~@<EFBFBD>_: +1 = K<EFBFBD><EFBFBD>_<EFBFBD><EFBFBD>e<EFBFBD>X[.rÓ<EFBFBD><EFBFBD>2|Z<EFBFBD>bON^g? -1 = 4d<EFBFBD>e<EFBFBD>`Y<EFBFBD><EFBFBD>[Fm
 * @return true = w<EFBFBD><EFBFBD>Q<EFBFBD>v<EFBFBD>m<EFBFBD> <EFBFBD>h@i?m͓<EFBFBD>_vf VL53 ȓYef
 */
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) {
        /* Y<EFBFBD>]l૕6l<EFBFBD>N +<EFBFBD>?e<EFBFBD>X[.rf<EFBFBD>%1x_n?<EFBFBD>m<EFBFBD>nvfX=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;
        /* <EFBFBD><EFBFBD>C<EFBFBD>W<EFBFBD>[? w = fence_sign 3<EFBFBD> kp 3<EFBFBD> (d_actual - d_target)
         * Y<EFBFBD>EQvf<EFBFBD>~<EFBFBD><EFBFBD>?m͓<EFBFBD>_J0ig?d_actual > d_target) +<EFBFBD>?4d<EFBFBD>e<EFBFBD>`Y<EFBFBD><EFBFBD>[Fm(-w)ȕ<EFBFBD>rN~
         * <EFBFBD><EFBFBD><EFBFBD> `m?fence_sign = -1 */
        *fence_sign = -1.0f;
    } else {
        /* <EFBFBD>[><EFBFBD>l૕6l<EFBFBD>N +<EFBFBD>?e<EFBFBD>X[.rf<EFBFBD>%1ONn?Wt8YvfX=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;
        /* <EFBFBD><EFBFBD>C<EFBFBD>W<EFBFBD>[? w = fence_sign 3<EFBFBD> kp 3<EFBFBD> (d_actual - d_target)
         * <EFBFBD>[?<EFBFBD>vf<EFBFBD>~<EFBFBD><EFBFBD>?m͓<EFBFBD>_J0ig?d_actual > d_target) +<EFBFBD>?4d<EFBFBD>e<EFBFBD>`<EFBFBD>[A<EFBFBD>Fm(+w)ȕ<EFBFBD>rN~
         * <EFBFBD><EFBFBD><EFBFBD> `m?fence_sign = +1 */
        *fence_sign = +1.0f;
    }

    if (f_ok && r_ok) {
        *d_avg = (df + dr) * 0.5f;  /* Y<EFBFBD><EFBFBD>\|垚<EFBFBD>QNo<EFBFBD>}\<EFBFBD>j<EFBFBD>o?dS2cm +<EFBFBD>?dS1.4cm */
        return true;
    } else if (f_ok) {
        *d_avg = df;
        return true;
    } else if (r_ok) {
        *d_avg = dr;
        return true;
    }
    return false;
}

/** <EFBFBD>Y<EFBFBD> ̓<EFBFBD>0vfZ<EFBFBD>?VL53 ē<EFBFBD><EFBFBD>`O<EFBFBD>$1<EFBFBD>m */
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;
}

/** <EFBFBD>Y<EFBFBD> ̓<EFBFBD>0xV<EFBFBD><EFBFBD><EFBFBD>fO^ɓ2 "k */
static bool check_reacquire(const CorridorObs_t* obs, const CorridorState_t* state)
{
    /* ----------------------------------------------------------------
     * ɓ2 "k 0 (“g<7F><67>): Z<><5A>^:~O<>
Y<EFBFBD>SZ<EFBFBD>q <EFBFBD>t?     *
     * <EFBFBD>C%<EFBFBD>`X<EFBFBD><EFBFBD>q<EFBFBD>`<EFBFBD>?     *   ^g<EFBFBD>Ɩ9p~\<EFBFBD>WZ<EFBFBD><EFBFBD>^<EFBFBD>n<EFBFBD>oX[^nZ<EFBFBD>b<EFBFBD>wP<EFBFBD><EFBFBD>T}Z<EFBFBD><EFBFBD>^:~O<EFBFBD>Y^nZ<EFBFBD> blக)1<EFBFBD> 6l<EFBFBD>N<EFBFBD>~[<EFBFBD><EFBFBD>`“pt<EFBFBD>`<EFBFBD><EFBFBD>?     *   - ^g=<EFBFBD>mnig<EFBFBD>V<EFBFBD>wY<EFBFBD>O<EFBFBD>0}Z<EFBFBD><EFBFBD>^:~O<EFBFBD>YtyR<EFBFBD>nblக)1<EFBFBD> 6l<EFBFBD>N5pR_po(<EFBFBD><EFBFBD>R?m͓<EFBFBD>_}<EFBFBD>t<EFBFBD>o<EFBFBD><EFBFBD>Hg<EFBFBD>Qmq<EFBFBD>X<EFBFBD>X20~35cm<EFBFBD>?     *   - ^gA<EFBFBD><EFBFBD><EFBFBD><EFBFBD>tEZC~Z<EFBFBD>q[_<EFBFBD>,l<EFBFBD>`uZ<EFBFBD> O<EFBFBD>YK<EFBFBD><EFBFBD>~<EFBFBD><EFBFBD>X~O<EFBFBD><EFBFBD>0<EFBFBD>wP<EFBFBD><EFBFBD>T}<EFBFBD>t<EFBFBD>o<EFBFBD><EFBFBD>`m<EFBFBD>^N~<EFBFBD>~<EFBFBD>?m͓<EFBFBD>_<EFBFBD>pY<EFBFBD>:j<EFBFBD>WZ<EFBFBD>q<EFBFBD>f
     *     “pt<EFBFBD>`<EFBFBD>?20cm+<EFBFBD><EFBFBD>6l<EFBFBD>N9p<EFBFBD>e}<EFBFBD><EFBFBD>\<EFBFBD><EFBFBD><EFBFBD><EFBFBD>?>> 40cm <EFBFBD><EFBFBD>,h<EFBFBD>y<EFBFBD><EFBFBD><EFBFBD>_<EFBFBD>%)<EFBFBD>X[4^Ó<EFBFBD>ref
     *
     *   e<EFBFBD><EFBFBD>r<EFBFBD> d_back > ÕX<EFBFBD> ?ē?^gA<EFBFBD>闸[6<EFBFBD><EFBFBD>yig<EFBFBD>V<EFBFBD>wY<EFBFBD>?(<EFBFBD><EFBFBD>R?m<EFBFBD><EFBFBD><EFBFBD>0<EFBFBD>X<EFBFBD><EFBFBD>U	v<EFBFBD><EFBFBD><EFBFBD><EFBFBD>}
     *   V<EFBFBD><EFBFBD>T<EFBFBD>W4d=<EFBFBD>,p<EFBFBD>oM}`m?~30cm <EFBFBD>t<EFBFBD>Q<EFBFBD>W >120cm<EFBFBD>Y}ig<EFBFBD>n-}\m<EFBFBD>^wV<EFBFBD>~-[x<EFBFBD><EFBFBD>~<EFBFBD><EFBFBD>W<EFBFBD><EFBFBD>?     *
     * ÕX<EFBFBD> xY<EFBFBD> 
YG_<EFBFBD>?     *   <EFBFBD>~<EFBFBD>4Q<EFBFBD><EFBFBD>6l<EFBFBD>N9p?40cm<EFBFBD><EFBFBD>\<EFBFBD>n9p?20cm<EFBFBD><EFBFBD>\<EFBFBD>n엢<EFBFBD>3w<EFBFBD>m_<EFBFBD>i<EFBFBD>m<EFBFBD>0vfZ<EFBFBD>?10cm<EFBFBD><EFBFBD>?     *   Z<EFBFBD><EFBFBD>^:~O<EFBFBD>
Y<EFBFBD>WWt8Yvfe<EFBFBD>X[.rȓ<EFBFBD> ig<EFBFBD>n<EFBFBD>[ 40cm<EFBFBD>%X<EFBFBD> 6l<EFBFBD>NO<EFBFBD>%1<EFBFBD><EFBFBD><EFBFBD>Y<EFBFBD> ?     *   Y<EFBFBD>?0.45m cm<EFBFBD>n<EFBFBD>ÕX<EFBFBD> wx0}Bi9j,|<EFBFBD><EFBFBD>6l<EFBFBD>N9pLu<EFBFBD>kȓ<EFBFBD> <EFBFBD>oC<EFBFBD> wx}cm<EFBFBD>^,|Z<EFBFBD>q4Uȓ<EFBFBD> -<EFBFBD>_<EFBFBD>ty<EFBFBD><EFBFBD><EFBFBD>_<EFBFBD> :{<EFBFBD> ?     *   {<EFBFBD><EFBFBD>0<EFBFBD>`uZ<EFBFBD> O<EFBFBD>Y<EFBFBD>h<EFBFBD><EFBFBD>,X}<EFBFBD>ttTzV<EFBFBD>~6[}R<EFBFBD>k<EFBFBD>`͓<EFBFBD>'_<EFBFBD>tk<EFBFBD>oo`m<EFBFBD><EFBFBD>}<EFBFBD>t<EFBFBD>~<EFBFBD>f=0<EFBFBD>m
envalid<EFBFBD>Y<EFBFBD> <EFBFBD>e<EFBFBD> ?     *   cm<EFBFBD>U<EFBFBD>W`m<EFBFBD>de MASK_BACK <EFBFBD>Y<EFBFBD> ̓<EFBFBD>0An<EFBFBD><EFBFBD>X<EFBFBD> M}Ó<EFBFBD>refÓ5<EFBFBD><EFBFBD>ɓ2 "k<EFBFBD>m<EFBFBD>]<5D>W<EFBFBD>~6[}<7D><><EFBFBD> V<>&h<1F><><EFBFBD><EFBFBD>\<EFBFBD>%<25>m<EFBFBD>o<EFBFBD>Y<EFBFBD><59>?     * ---------------------------------------------------------------- */
    bool back_valid = (obs->valid_mask & CORRIDOR_OBS_MASK_BACK) != 0U;
    if (back_valid && obs->d_back <= s_nav.cfg.reacquire_back_thresh) {
        /* Z<EFBFBD><EFBFBD>^:~O<EFBFBD>	Y<EFBFBD>|s<EFBFBD><EFBFBD><EFBFBD>tyR<EFBFBD>nblக)1<EFBFBD> 6l<EFBFBD>Nigb~<EFBFBD> +<EFBFBD>?^gA<EFBFBD><EFBFBD><EFBFBD>Op-lmn<EFBFBD>tEZC~Z<EFBFBD>q[_ +<EFBFBD>?<EFBFBD><EFBFBD><EFBFBD>c<EFBFBD>| */
        return false;
    }
    /* {<EFBFBD><EFBFBD>0<EFBFBD>`uZ<EFBFBD> O<EFBFBD>Y<EFBFBD>h<EFBFBD><EFBFBD>,X}<EFBFBD>tmT<EFBFBD>V<EFBFBD><EFBFBD><EFBFBD>_<EFBFBD>%<EFBFBD>YKU <EFBFBD>tX[<EFBFBD>iYt<EFBFBD>U<EFBFBD>V<EFBFBD>[<EFBFBD>SA~O<EFBFBD><EFBFBD>0<EFBFBD>wP<EFBFBD><EFBFBD>T}Yt<EFBFBD>U<EFBFBD><EFBFBD>[<EFBFBD>c<EFBFBD>SZ<EFBFBD>q}<EFBFBD>t<EFBFBD>[C~<EFBFBD>Y<EFBFBD>0<EFBFBD>h<EFBFBD>Y?*/

    /* ɓ2 "k 1: w<><77>Q<EFBFBD>v 3 <00>mC<6D>vfZ<66>b6}<7D><><EFBFBD>q<EFBFBD>jȓYef */
    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 "k 2: <00>[@<40>x_<78>t<EFBFBD>o<EFBFBD><6F>\<EFBFBD>?.<2E>?<00>tg<7F>|9pɅ<70>[ */
    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;
    }

    /* ɓ2 "k 3: EKF <><7F><EFBFBD>O4d?*/
    if (state->conf < s_nav.cfg.reacquire_conf_thresh) return false;

    return true;
}

/** ~<EFBFBD><EFBFBD>\G_0<EFBFBD><EFBFBD>d<EFBFBD>XÕ5<EFBFBD><EFBFBD><EFBFBD>(<EFBFBD><EFBFBD>R<EFBFBD>Qt<EFBFBD>\%<EFBFBD><EFBFBD><EFBFBD>\<EFBFBD>% */
static float odom_since_entry(void)
{
    return s_nav.odom_distance_accum - s_nav.stage_entry_odom_vx_accum;
}

/** Õ5<EFBFBD><EFBFBD><EFBFBD>^g<EFBFBD>) %<EFBFBD>?<EFBFBD><EFBFBD>1ldeR<EFBFBD><EFBFBD>oP<EFBFBD>V<EFBFBD>?*/
static void transition_to(GlobalNavStage_t next, const RobotBlackboard_t* board);

/* =========================================================
 * Õ5<EFBFBD><EFBFBD><EFBFBD>Z<EFBFBD><EFBFBD>]t? * ========================================================= */
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"
};

/* =========================================================
 * ^g<EFBFBD><EFBFBD>`<EFBFBD><EFBFBD>F<EFBFBD> (
<EFBFBD>q<EFBFBD>z(<EFBFBD>?90<EFBFBD>c ^g<EFBFBD><EFBFBD>`<EFBFBD><EFBFBD>f<EFBFBD>})
 * ========================================================= */
static void execute_turn(const CorridorObs_t* obs,
                         const CorridorState_t* state,
                         const RobotBlackboard_t* board,
                         uint32_t now_ms,
                         GlobalNavOutput_t* out)
{
    /* [<EFBFBD><EFBFBD>ExX~G] IMU <EFBFBD>o<EFBFBD><EFBFBD>ef9p
Y<EFBFBD>S<EFBFBD>m<EFBFBD>o<EFBFBD>Y: Z!An IMU <EFBFBD><EFBFBD>HrA]ÓAR<EFBFBD>sW<EFBFBD><EFBFBD>Q<EFBFBD>N^g<<EFBFBD>}<EFBFBD>m<EFBFBD>]8m^g<EFBFBD><EFBFBD><EFBFBD> ?     * <EFBFBD>tnTiZ<EFBFBD><EFBFBD>^0}t<EFBFBD><EFBFBD>;殕(1<EFBFBD>yÓ5gZ~<EFBFBD><EFBFBD><EFBFBD>0]~<EFBFBD><EFBFBD>Q}ig<EFBFBD>m<EFBFBD>S GNAV_ERROR<EFBFBD><EFBFBD>?*/
    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;

    /* <EFBFBD>[6<EFBFBD>Fmig<EFBFBD>V<EFBFBD>kYt<EFBFBD>c<EFBFBD>[ (Y<EFBFBD>+h<EFBFBD>|5ppt<EFBFBD> ? */
    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;

    /* <EFBFBD><EFBFBD><EFBFBD>0N~)<EFBFBD><EFBFBD><EFBFBD>#rÓ<EFBFBD><EFBFBD>zV<EFBFBD><EFBFBD>?*/
    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;

    /* ^g<EFBFBD><EFBFBD>`9p~\<EFBFBD>WR<EFBFBD><EFBFBD>0~u */
    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;
    }

    /* <EFBFBD>tnTi<EFBFBD>m<EFBFBD>o<EFBFBD>Y */
    if (now_ms - s_nav.stage_start_ms > s_nav.cfg.turn_timeout_ms) {
        transition_to(GNAV_ERROR, board);
    }
}

/* =========================================================
 * Õ5<EFBFBD><EFBFBD><EFBFBD>^g<EFBFBD>)
 * ========================================================= */
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;

    /* <EFBFBD><EFBFBD>1lde: <EFBFBD>tg<EFBFBD>}ig<EFBFBD>m<EFBFBD>SÓX<EFBFBD>h\<EFBFBD><EFBFBD>\wV<EFBFBD>~?*/
    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 <EFBFBD><EFBFBD><EFBFBD>]<EFBFBD>u: “Hr<EFBFBD>w(<EFBFBD>?e_y Y<EFBFBD><EFBFBD>P<EFBFBD> <EFBFBD>Q	{Z<EFBFBD><EFBFBD>\}G<EFBFBD>tT0<EFBFBD><EFBFBD><EFBFBD>] */
        CorridorFilter_Reset();
        s_nav.heading_ref_deg = imu_yaw;
        break;

    case GNAV_ALIGN:
        /* K<EFBFBD><EFBFBD>n<EFBFBD>n5pĉ<EFBFBD><EFBFBD>-l<EFBFBD>zƕ<EFBFBD>`x<EFBFBD><EFBFBD><EFBFBD>g<EFBFBD>j<EFBFBD>M[KF <EFBFBD>Y<EFBFBD>0i<EFBFBD>[<EFBFBD>SAnR<EFBFBD><EFBFBD>o<EFBFBD>|m<EFBFBD>bx<EFBFBD> */
        s_nav.align_ok_count = 0;
        break;

    case GNAV_CORRIDOR_TRACK:
        s_nav.corridor_entry_s = 0.0f;  /* EKF <EFBFBD>[?reset, s `m?0 <EFBFBD>[<EFBFBD> <EFBFBD>o?*/
        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;  /* <EFBFBD>h(h<EFBFBD>ZP<EFBFBD><EFBFBD>]<EFBFBD><EFBFBD>0<EFBFBD>?*/
        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;
}

/* =========================================================
 * O<EFBFBD><EFBFBD>} 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;
    /* <EFBFBD>m<EFBFBD>]jnigkwV transition_to<EFBFBD>}\m<EFBFBD>m?<EFBFBD>U~Z!An board <EFBFBD><EFBFBD>HrA]<EFBFBD><EFBFBD>?       <EFBFBD>m)[<EFBFBD>z<EFBFBD>m?Update [<EFBFBD>&1an<EFBFBD><EFBFBD>}\B_<EFBFBD>?running && IDLE Ó<EFBFBD><EFBFBD>@U transition<EFBFBD><EFBFBD>?*/
    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";
}

/* =========================================================
 * ͓?z>~ 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;
    }

    /* <EFBFBD><EFBFBD><EFBFBD>\<EFBFBD>%<EFBFBD>t<EFBFBD>R<EFBFBD>? <EFBFBD><EFBFBD>d = vx * dt
     * now_ms "<EFBFBD><EFBFBD><EFBFBD><EFBFBD>v"<EFBFBD>&1<EFBFBD>g|m<EFBFBD>r<EFBFBD>S (HAL_GetTick)<EFBFBD>|\{`m<EFBFBD><EFBFBD><EFBFBD>}|m<EFBFBD>r<EFBFBD>Rc<EFBFBD>&1i•X[<EFBFBD>WÓ<EFBFBD>r<EFBFBD>S<EFBFBD>?     * <EFBFBD><EFBFBD><EFBFBD>W<EFBFBD>S IMU ÓX<EFBFBD>h<EFBFBD><EFBFBD><EFBFBD>Q<EFBFBD>NǓX[i<EFBFBD><EFBFBD><EFBFBD>\<EFBFBD>%\<EFBFBD><EFBFBD>\<EFBFBD>yÓ<EFBFBD><EFBFBD>;_Y<EFBFBD>}\<EFBFBD>U
?(X<EFBFBD>?TODO-1 <EFBFBD>m<EFBFBD><EFBFBD>2<EFBFBD>) */
    {
        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;
    }

    /* [<EFBFBD><EFBFBD>ExX~G] IMU yaw <EFBFBD><EFBFBD>,aG_: <EFBFBD>o<EFBFBD><EFBFBD>efÓ5gZ"<EFBFBD>%1,_p<5F><70>Q<EFBFBD> wx}<7D>m<EFBFBD>o<EFBFBD>[x<>D<EFBFBD><44>`<60>m<EFBFBD>]I_p<5F>|\	{ē<1D><>pR<70>?0 */
    float imu_yaw_deg = board->imu_yaw_continuous.is_valid
                        ? board->imu_yaw_continuous.value : s_nav.heading_ref_deg;

    /* <EFBFBD>i?j{<EFBFBD>Hg<EFBFBD>d<EFBFBD>V */
    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 +<EFBFBD>?w<EFBFBD>D<EFBFBD><EFBFBD>Yig<EFBFBD>m<EFBFBD>S 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) {

    /* ============================================================
     * O<EFBFBD><EFBFBD>0<EFBFBD>n)<EFBFBD><EFBFBD>[<EFBFBD>V (`m<EFBFBD>^<EFBFBD>`T<EFBFBD>%1/\Z<EFBFBD>WON<EFBFBD>~<EFBFBD><EFBFBD>eZ<EFBFBD>&b<EFBFBD> 6l<EFBFBD>NV<EFBFBD>Di<EFBFBD>)
     *
     * |m<EFBFBD>r<EFBFBD>Rc<EFBFBD>&1QP<EFBFBD><EFBFBD>x0}
     *   - <EFBFBD>[?<EFBFBD>vf VL53 <EFBFBD>tQg?m͓<EFBFBD>_}<EFBFBD>o,[<EFBFBD>|ȓYef +<EFBFBD>?"<EFBFBD>$1,|<7C>YD<59><44>`<60>tQg~库C<04>W
     *   - Y<EFBFBD>EQvf VL53 <EFBFBD>m<EFBFBD> Q<EFBFBD><EFBFBD>T<EFBFBD>`T<EFBFBD>%1/\Op^S<EFBFBD><EFBFBD>+<EFBFBD><EFBFBD> C1<EFBFBD>[<EFBFBD> Y<EFBFBD>?260cm)<EFBFBD>~\ty<EFBFBD>m<EFBFBD>]<EFBFBD>W
     *   - O<EFBFBD><EFBFBD>0<EFBFBD>n<EFBFBD>t<EFBFBD>o<EFBFBD><EFBFBD>˓zOmq<EFBFBD>X<EFBFBD>`T<EFBFBD>%1/\O<EFBFBD><EFBFBD>0[_R<EFBFBD>Hv1O<EFBFBD><EFBFBD>0[_`moT<EFBFBD>[ 10~30cm<EFBFBD>?     *   - <EFBFBD>mf<EFBFBD><EFBFBD>ȕ<EFBFBD>rwV<EFBFBD>~-[xẓ(1X~<EFBFBD>t<EFBFBD>QD<EFBFBD><EFBFBD>t<EFBFBD>o<EFBFBD><EFBFBD>Z<EFBFBD><EFBFBD>^F]Y<EFBFBD><EFBFBD>FmZ<EFBFBD>?     * ============================================================ */
    case GNAV_ENTRY_STRAIGHT: {
        out->override_v  = s_nav.cfg.entry_v;
        out->safety_mode = SAFETY_MODE_STRAIGHT;

        /* O<EFBFBD><EFBFBD>0<EFBFBD>nZ[<EFBFBD>jn<EFBFBD>[><EFBFBD>l૕6l<EFBFBD>NV<EFBFBD>Di<EFBFBD><EFBFBD>,lONnFRe<EFBFBD>X[.r<EFBFBD>|\{ LINK_STRAIGHT )<EFBFBD>?z<EFBFBD>`<EFBFBD><EFBFBD>f<EFBFBD>}
         * ENTRY “pt<EFBFBD>`<EFBFBD>~Yef\m?TRAVEL_DIR_WEST<EFBFBD>Xjn<EFBFBD>[><EFBFBD>l૕6l<EFBFBD>N<EFBFBD>}\?m͓<EFBFBD>_jn<EFBFBD>[?<EFBFBD>vf<EFBFBD>?/
        {
            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);
        }

        /* <EFBFBD><EFBFBD><EFBFBD>\<EFBFBD>%<EFBFBD>t?+ <EFBFBD>tnTiR<EFBFBD><EFBFBD>0~u */
        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;
    }

    /* ============================================================
     * <EFBFBD>mYR<EFBFBD>^g<EFBFBD><EFBFBD>`<EFBFBD>5<EFBFBD><EFBFBD> zO<EFBFBD>|<EFBFBD>m<EFBFBD> <EFBFBD>o<EFBFBD>R<EFBFBD>`
     * ============================================================ */
    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;

    /* ============================================================
     * <EFBFBD><EFBFBD><EFBFBD>]]~<EFBFBD><EFBFBD>uˆ<EFBFBD>[?     *
     * •<EFBFBD><EFBFBD>}<EFBFBD>s<EFBFBD>~\<EFBFBD>j<EFBFBD>?     *   ^g<EFBFBD>ƖR<EFBFBD>,llu<EFBFBD><EFBFBD>-ai<EFBFBD><EFBFBD>\<EFBFBD>n엢<EFBFBD>r_s<EFBFBD>PU~f<EFBFBD>&1<EFBFBD>wY<EFBFBD>E<EFBFBD>;<EFBFBD><EFBFBD>"Xlக)1<> 6l<36>NP<4E><50>T}<1B>?     *   cm<63>U<1D>Ó5g<35><67>n<1C>L53Z<>~\qrs<EFBFBD><EFBFBD><EFBFBD>tyR<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>nC/ps<70><73>\l<EFBFBD>ȕ?.<2E>?0cm)<1B>?     *   9pɅ<70>[\<EFBFBD>?= 10+10+20 = 40cm<1B>|\{*<2A><>q<1C>f<EFBFBD>&1<>wP<77>mTluO<75>$1<>zw<7A><77>m<EFBFBD> ?     *   igk<><6B>w<EFBFBD>Qg<51>vȓD<C893><44>SZ<>q(lJZ<4A><00>Q<EFBFBD><51><EFBFBD>]]~<7E><0B>oo`m<><6D>}R<><52>V<EFBFBD>WCORRIDOR_TRACKZ<>?     *   EKF(<28>%R_yY<><1D>XQē<1A>G0K<30><4B>_
j<EFBFBD><EFBFBD>[orridor_ctrlHg<EFBFBD>d<EFBFBD>V<EFBFBD>o=<EFBFBD><EFBFBD><EFBFBD>\<EFBFBD>nW<EFBFBD>!4<EFBFBD>f<EFBFBD>%1<EFBFBD>SY<EFBFBD>C<EFBFBD><EFBFBD> ?     *
     * <EFBFBD>m<EFBFBD><EFBFBD>2<EFBFBD>“ĉ
<EFBFBD><EFBFBD>?     *   <EFBFBD>[<EFBFBD>f<EFBFBD>S reacquire_min_depth •&1`<EFBFBD><EFBFBD>?     *   Y<EFBFBD>E<EFBFBD>An<EFBFBD><EFBFBD><EFBFBD>\<EFBFBD>%<EFBFBD>t!	j<EFBFBD>~?<EFBFBD><EFBFBD>n엢<EFBFBD><EFBFBD>Q#Z^S<EFBFBD>SZ<EFBFBD>q4U<EFBFBD>t<EFBFBD>QD<EFBFBD><EFBFBD>t<EFBFBD>o<EFBFBD><EFBFBD>Z<EFBFBD><EFBFBD>^}<EFBFBD><EFBFBD><EFBFBD>]<EFBFBD>S<EFBFBD>t?z}<EFBFBD>o-[x<EFBFBD><EFBFBD><EFBFBD>,<EFBFBD><EFBFBD> ?     *   igk <EFBFBD>m<EFBFBD>om<EFBFBD>h<EFBFBD>hL53O<EFBFBD>)14Qt<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Z<EFBFBD>q[_Hg-W-}ig<EFBFBD>m<EFBFBD>S<EFBFBD>~<EFBFBD>Q~unC<EFBFBD><EFBFBD>V<EFBFBD><EFBFBD>TYq<EFBFBD><EFBFBD>?     * ============================================================ */
    case GNAV_REACQUIRE:
        out->override_v  = s_nav.cfg.reacquire_v;
        out->override_w  = 0.0f;  /* <EFBFBD>m<EFBFBD>]<EFBFBD>Nx<EFBFBD>D<EFBFBD><EFBFBD>`<EFBFBD><EFBFBD>C<EFBFBD>W<EFBFBD><EFBFBD>\<EFBFBD><EFBFBD>^gA<EFBFBD><EFBFBD>V<EFBFBD><EFBFBD>`X~Z<EFBFBD>q}ALIGNÕ5<EFBFBD><EFBFBD><EFBFBD>P<EFBFBD><EFBFBD>]<EFBFBD>a<EFBFBD>Y?*/
        out->safety_mode = SAFETY_MODE_STRAIGHT;

        {
            /* O<EFBFBD><EFBFBD>0<EFBFBD>w#Z^S<EFBFBD>[9pX<]<EFBFBD>-lmnHg<EFBFBD>`<EFBFBD>Wȓ<EFBFBD> Op<EFBFBD>_A~4d@<EFBFBD><EFBFBD>X<EFBFBD><EFBFBD>\<EFBFBD><EFBFBD><EFBFBD>Y 2}<EFBFBD>o,[ <EFBFBD>t<EFBFBD>0x<EFBFBD><EFBFBD><EFBFBD>?*/
            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 {
                /* #Z^S<EFBFBD>[<EFBFBD>m<EFBFBD>]<EFBFBD>Q <EFBFBD><EFBFBD>?ɓ2 "kȓE<EFBFBD>'_<>t?+<2B>?<00>t!<21>f<EFBFBD>m<EFBFBD>0xrZtTBm
                 * 	Z&1pR<EFBFBD>-lA~4d?<EFBFBD>	{<EFBFBD>tk<EFBFBD>i<EFBFBD>m<EFBFBD>q<EFBFBD>zƕ<EFBFBD><EFBFBD>}Õ<EFBFBD>S<EFBFBD>f<EFBFBD>&1<EFBFBD>wY<EFBFBD>E<EFBFBD>)<EFBFBD><EFBFBD>~<EFBFBD><EFBFBD>u\m<EFBFBD>U4QR<EFBFBD><EFBFBD>Ux<EFBFBD><EFBFBD><EFBFBD>?                 * <EFBFBD><EFBFBD><EFBFBD><EFBFBD>`ig<EFBFBD>m<EFBFBD>wZ<EFBFBD><EFBFBD>^g_Ǖ<EFBFBD> <EFBFBD>[X<EFBFBD>vQ<EFBFBD><EFBFBD>r<EFBFBD>ZOp<EFBFBD><EFBFBD>Y<EFBFBD>?*/
                s_nav.reacquire_ok_count = 0;
            }
        }

        if (s_nav.reacquire_ok_count >= s_nav.cfg.reacquire_confirm_ticks) {
            /* <EFBFBD><EFBFBD><EFBFBD>]]~<EFBFBD>Θ <EFBFBD>t?+<EFBFBD>?ig<EFBFBD>m<EFBFBD>SX<EFBFBD><EFBFBD>qtn5pĉ<EFBFBD>Õ5<EFBFBD><EFBFBD><EFBFBD><EFBFBD>}\<EFBFBD>N^g><EFBFBD>deVL53<EFBFBD><EFBFBD><EFBFBD>U<EFBFBD>Z<EFBFBD><EFBFBD>^@US<EFBFBD><EFBFBD>]X~ */
            transition_to(GNAV_ALIGN, board);
        }
        if (elapsed_ms > s_nav.cfg.reacquire_timeout_ms) {
            transition_to(GNAV_ERROR, board);
        }
        break;

    /* ============================================================
     * X<EFBFBD><EFBFBD>qtn5pĉ<EFBFBD> (ALIGN)
     *
     * Yt@<EFBFBD>B_Ó5<EFBFBD><EFBFBD>n<EFBFBD>JkEACQUIRE <EFBFBD>~<EFBFBD><EFBFBD>{ᴓ,a<EFBFBD>YZ<EFBFBD><EFBFBD>^}K<EFBFBD><EFBFBD>n<EFBFBD>nX<EFBFBD><EFBFBD>qtn"<EFBFBD>?EKF <00><><EFBFBD>U<1C>^gA<67><41><EFBFBD>P<EFBFBD><50>]<5D>Xig<69>m<EFBFBD> ?     *
     * <EFBFBD><EFBFBD>C<EFBFBD>W<EFBFBD>[6[0}v = 0<EFBFBD><EFBFBD>[ = -(kp_th<EFBFBD><EFBFBD>e_th + kp_y<EFBFBD><EFBFBD>e_y)
     *
     * <EFBFBD>mp<EFBFBD><EFBFBD>|<EFBFBD>m"Xde EKF (<28>?e_th/e_y p<>|\	{ē<1A>+^<5E>o?VL53 <00>[<5B><><EFBFBD>W<1B>?     *   X<><58>qP<71> VL53 S<><53>]<5D>`<60>[<5B><><EFBFBD>SYt<59>c0}dS2cm / 12cm i<>y<EFBFBD><79>V .<2E>?dS18<00>c c<>D<EFBFBD><10><1B>}\luO<EFBFBD>$1	{Y<><1C>de<64><65>?     *   EKF (<28>?e_th ē?IMU Bi<j?|4d><3E>R<>?+ nC~<7E><>YD<59><44>`q<><71>e<EFBFBD>`<60>~=<3D>ko(<28><>R<EFBFBD><52><17>K%}Y<><19><>O<EFBFBD><4F>?     *
     * 9p~\<EFBFBD>Wɓ2 "k<1B><>ke_th| < th_tol AND |e_y| < y_tol<1B><>\[~
?N <00><><EFBFBD>]<5D> ?     * <00>tnTi<1B>1l?m<><6D><EFBFBD>0X~Z<>q}<7D>m<EFBFBD>]j<>o<EFBFBD>pfy<66>~6[}<7D>tnTi<>tX[<5B>iVL53<00>t)1zV<7A>[<5B><>}cm<63>U<EFBFBD>{UtzO7b
h<7F>}<7D><>?     * ============================================================ */
    case GNAV_ALIGN: {
        out->override_v  = 0.0f;   /* K<EFBFBD><EFBFBD>n<EFBFBD>n */
        out->use_override = true;
        out->request_corridor = false;
        out->safety_mode = SAFETY_MODE_TURN;  /* TURN<EFBFBD>Y3 !}<EFBFBD>,l<EFBFBD>S<EFBFBD>t?z+^f<EFBFBD>Hr<EFBFBD>h^g
<EFBFBD>}<EFBFBD>t<EFBFBD>[C~S<EFBFBD><EFBFBD>]<EFBFBD>`<EFBFBD>~Hrc<EFBFBD>Y<EFBFBD> ̓?*/

        /* 5pĉẓC<EFBFBD>W<EFBFBD>[6[0}`mpTW竕<EFBFBD>q<EFBFBD>[<EFBFBD>~\<EFBFBD>h<EFBFBD>~WW<EFBFBD> <EFBFBD>q<EFBFBD>[ */
        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);

        /* 9p~\<EFBFBD>WR<EFBFBD><EFBFBD>0~u<EFBFBD>,l-<EFBFBD><EFBFBD>xO<EFBFBD>bcm<EFBFBD>]<EFBFBD>u<EFBFBD><EFBFBD>Ʌjn9ppt
jP<EFBFBD>?*/
        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) {
            /* 5pĉ<EFBFBD>9p~\<EFBFBD>W<EFBFBD><EFBFBD>\0n<EFBFBD><EFBFBD>de<EFBFBD>Y<EFBFBD>0i(<EFBFBD>?e_th ǓX[<EFBFBD>g IMU Y<EFBFBD><EFBFBD>P<EFBFBD> <EFBFBD>QW<EFBFBD> */
            s_nav.heading_ref_deg += PARAM_RAD2DEG(state->e_th);
            transition_to(GNAV_CORRIDOR_TRACK, board);
        }

        /* <EFBFBD>tnTi<EFBFBD>m<EFBFBD>o<EFBFBD>Y<EFBFBD>+l	{<EFBFBD>Yd<EFBFBD><EFBFBD>t<EFBFBD><EFBFBD>\?m<EFBFBD><EFBFBD><EFBFBD>0X~Z?*/
        if (elapsed_ms > s_nav.cfg.align_timeout_ms) {
            transition_to(GNAV_CORRIDOR_TRACK, board);
        }
        break;
    }

    /* ============================================================
     * Z<EFBFBD>q4U•`<EFBFBD>F^<EFBFBD>t<EFBFBD>qܑ (\m<EFBFBD>0<EFBFBD>| corridor_ctrl)
     * ============================================================ */
    case GNAV_CORRIDOR_TRACK:
        out->use_override     = false;
        out->request_corridor = true;
        out->safety_mode      = SAFETY_MODE_CORRIDOR;

        /* [Phase-3] Z<EFBFBD>q4U IMU Y<EFBFBD><EFBFBD>P<EFBFBD> <EFBFBD>Q<EFBFBD>gZ<EFBFBD>b<EFBFBD>S͓!<EFBFBD>
         *
         * X<EFBFBD><EFBFBD>q<EFBFBD>`<EFBFBD>5kKF (<EFBFBD>?e_th Y<EFBFBD><EFBFBD>]<EFBFBD>i\m<EFBFBD>U<EFBFBD>}S<EFBFBD><EFBFBD>]<EFBFBD>WZ<EFBFBD> bIm5p<EFBFBD>,| IMU Y<EFBFBD><EFBFBD>P<EFBFBD> <EFBFBD>QW<EFBFBD>(<EFBFBD><EFBFBD>R<EFBFBD>N<EFBFBD>[<EFBFBD><EFBFBD><EFBFBD> ?         * <EFBFBD>o<EFBFBD>PIp e_th <EFBFBD><EFBFBD>can<EFBFBD>m<EFBFBD>]<EFBFBD>ƕ<EFBFBD><EFBFBD>}n*[<EFBFBD>渓zO;u +2<EFBFBD>c<EFBFBD>Y}<EFBFBD>tX[<EFBFBD>i IMU Y<EFBFBD><EFBFBD>P<EFBFBD> <EFBFBD>QW<EFBFBD>ȓY<EFBFBD><EFBFBD>
<EFBFBD>q<EFBFBD> C<EFBFBD>N<EFBFBD>[<EFBFBD><EFBFBD><EFBFBD> ?         * <EFBFBD><EFBFBD>C<EFBFBD>Wc<EFBFBD>(1<EFBFBD>j<EFBFBD><EFBFBD>`XQ<EFBFBD><EFBFBD>3lC~ kp_theta <EFBFBD><EFBFBD>?e_th X<EFBFBD>)[{X<EFBFBD>O}cm<EFBFBD>UV~<EFBFBD><EFBFBD><EFBFBD>_<EFBFBD>a+<EFBFBD><EFBFBD> <EFBFBD><EFBFBD>C<EFBFBD>Wc<EFBFBD>$1<EFBFBD>z)<EFBFBD>Qgjn
         * "<00><>@i<>N<EFBFBD>[?<1B><>\<EFBFBD>n<EFBFBD>oX[<5B>[
^<5E><>NZ<4E>b<>znM}<7D>YD<59><44>`|m1l&}<7D><>!2}<7D>~Gl<47> ?         *
         * YtE<EFBFBD><EFBFBD>U“ĉvx<EFBFBD>-l!v<EFBFBD><EFBFBD><EFBFBD>]de˓xO<EFBFBD>v(<EFBFBD>?alpha Op?e_th Y<EFBFBD><EFBFBD>]<EFBFBD><EFBFBD>R<EFBFBD>?heading_ref_deg <EFBFBD>mOZ}
         * <EFBFBD>tA%,_p<EFBFBD><EFBFBD>QW<EFBFBD><EFBFBD>d<EFBFBD>S5pR_<EFBFBD>}*<EFBFBD><EFBFBD>q<EFBFBD>uZ<EFBFBD>qq<EFBFBD><EFBFBD><EFBFBD>?         *
         * <EFBFBD>mp<EFBFBD><EFBFBD>|<EFBFBD>mX	{"<EFBFBD>$1vf<76>ok<>XZ<58><5A>^
j<EFBFBD>4WL53 <EFBFBD>[<EFBFBD><EFBFBD><EFBFBD>W4Z-[W<EFBFBD><EFBFBD>Y0}
         *   VL53 ȓ?dS2cm <EFBFBD>mC<EFBFBD><EFBFBD>}<EFBFBD>t<EFBFBD>
j<EFBFBD>}\<EFBFBD>q<EFBFBD>~?12cm
         *   ȓ<EFBFBD> g<EFBFBD><EFBFBD>_<EFBFBD>j<EFBFBD>o?= atan2(4cm, 12cm) .<EFBFBD>?18<EFBFBD>c
         *   W<EFBFBD>EQZY<EFBFBD>'h<EFBFBD><EFBFBD>g<EFBFBD><EFBFBD>V<EFBFBD>{ȓ?~9<EFBFBD>c c<EFBFBD>D<EFBFBD><EFBFBD><EFBFBD>|\	{<EFBFBD><EFBFBD><EFBFBD>P<EFBFBD>`K<EFBFBD>3lW<EFBFBD>4d?<EFBFBD><EFBFBD>S<EFBFBD>t?         *
         * <EFBFBD>mp<EFBFBD><EFBFBD>|<EFBFBD>m"Xde EKF e_th<1B>?         *   e_th ē?IMU Bi<j?|4d><3E>R<>?+ nC~<7E><>YD<59><44>`Yt<59>Ptyq<79><71>e<EFBFBD>`<60>~=<3D>ko(<28><>R<EFBFBD><52><17>?         *   <00>o<EFBFBD>PIpY<70>|\vfYt<EFBFBD>Pty<EFBFBD>~<7E>Q~u<1B>HWonf BiHj}<1B><>[_th (<28><>R<EFBFBD>yT<79>c<>iY<69><19><>O(<28>?         *
         * •&16^ɓ2 "k<1B>?         *   - EKF conf >= corridor_ref_correct_conf<1B>%X<>|<7C>t?0.75<1B>?         *   - alpha ˓xO<78>v<1B>?.001<1B>Y}ÓX<>h/uJT<4A>f ~1s<1B>|\	{|m3lf<6C>,<2C>
<01>ic<69>D<EFBFBD><10>/uA<75><41>z
         */
        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;
        }

        /* R<EFBFBD>nbl<EFBFBD><EFBFBD>Y<EFBFBD> 4Z?*/
        {
            bool front_valid = (obs->valid_mask & CORRIDOR_OBS_MASK_FRONT) != 0U;
            if (front_valid && obs->d_front <= s_nav.cfg.corridor_end_detect_dist) {
                /* <EFBFBD><EFBFBD><EFBFBD>\<EFBFBD>%<EFBFBD>m.[<EFBFBD>j<EFBFBD>m<EFBFBD>o<EFBFBD>Y: w<EFBFBD><EFBFBD>Q<EFBFBD>v<EFBFBD>t<EFBFBD><EFBFBD>!| 1.0m <EFBFBD><EFBFBD><EFBFBD>]<EFBFBD>S<EFBFBD>tLk{<EFBFBD>9p,l<EFBFBD>W<EFBFBD>~$<EFBFBD>}<EFBFBD><EFBFBD><EFBFBD>W<EFBFBD>SK<EFBFBD><EFBFBD>V<EFBFBD>i<EFBFBD><EFBFBD>?*/
                float corridor_odom = odom_since_entry();
                if (corridor_odom > 1.0f) {
                    s_nav.corridors_completed++;
                    transition_to(GNAV_TURN_OUT_OF_CORRIDOR, board);
                }
            }
        }
        /* <EFBFBD><EFBFBD><EFBFBD>\<EFBFBD>%<EFBFBD>ttT<EFBFBD>f<EFBFBD>m<EFBFBD>o<EFBFBD>Y */
        if (odom_since_entry() > s_nav.cfg.corridor_length_max) {
            s_nav.corridors_completed++;
            transition_to(GNAV_TURN_OUT_OF_CORRIDOR, board);
        }
        break;

    /* ============================================================
     * ig<EFBFBD>p4^Z5u?mt?(<EFBFBD>~[<EFBFBD><EFBFBD>`<EFBFBD>~<EFBFBD>4Q<EFBFBD><EFBFBD>6l<EFBFBD>NV<EFBFBD>Di<EFBFBD>, “ĉ
<EFBFBD>2: <EFBFBD>m	Y<EFBFBD>OY<EFBFBD><EFBFBD>u<EFBFBD>NZ<EFBFBD>X<EFBFBD>W9p?
     *
     * |m<EFBFBD>r<EFBFBD>Rc<EFBFBD>&1QP<EFBFBD><EFBFBD>x}^g<EFBFBD><EFBFBD>`9p~\<EFBFBD>WZ<EFBFBD><EFBFBD>^poȓ<EFBFBD>o<EFBFBD>[<EFBFBD>Y0}
     *   - S<EFBFBD><EFBFBD>]:~O<EFBFBD>Y}ȓ<EFBFBD>o<EFBFBD>[<EFBFBD>YKU <EFBFBD>mBZ?m͓<EFBFBD>_}d_front ŕ<EFBFBD>_<EFBFBD>[t<EFBFBD>\<EFBFBD> <EFBFBD>czV      [<EFBFBD>~<EFBFBD>`<EFBFBD>[Bi<EFBFBD>i
     *   - Z<EFBFBD><EFBFBD>^:~O<EFBFBD>Y}ȓ<EFBFBD>o!]<EFBFBD>YKU <EFBFBD>m*[?m͓<EFBFBD>_}d_back ŕ<EFBFBD>_<EFBFBD>[t<EFBFBD>\<EFBFBD> <EFBFBD>c<EFBFBD><EFBFBD>       [<EFBFBD>~<EFBFBD>`<EFBFBD>[Bi<EFBFBD>i
     *   - e<EFBFBD>X[.rn?VL53 +<EFBFBD>?<EFBFBD>o,[<EFBFBD>|ȓYef (~10cm)                [<EFBFBD>m<EFBFBD>]de\m<EFBFBD>^<EFBFBD>W9pVk
     *   - ȕ<EFBFBD>p?m͓<EFBFBD>_vf VL53 +<EFBFBD>?<EFBFBD>tQg/ps<EFBFBD><EFBFBD>\l<EFBFBD>ȕ!2iȓYef<EFBFBD>}\<EFBFBD>Wh<EFBFBD><EFBFBD>R<EFBFBD>w<EFBFBD>[<EFBFBD> Y<EFBFBD>F<EFBFBD>i<EFBFBD>m 2Q0  [Z<EFBFBD>q[_͓<EFBFBD>VT~]
     *
     * <EFBFBD>m3 _9p+l<EFBFBD>{<EFBFBD>?     *   A: <EFBFBD><EFBFBD><EFBFBD>\<EFBFBD>%<EFBFBD>t?    odom >= link_distance * 0.85  (<EFBFBD><EFBFBD><EFBFBD>d<EFBFBD>|tgzV, ɓ<EFBFBD>QxVcm?
     *   B: S<EFBFBD><EFBFBD>]:~O<EFBFBD>
YI_V<EFBFBD>? d_frontA%<EFBFBD>v >= link_distance * 0.85 (ɓ<EFBFBD>QxVBi?
     *   C: ȕ<EFBFBD>p?m͓<EFBFBD>_vfVL53 <EFBFBD>m 2Q0/<EFBFBD>t<EFBFBD>Q<EFBFBD>W>50cm<EFBFBD><EFBFBD>\[~
?<EFBFBD><EFBFBD><EFBFBD>] <EFBFBD>t? ()<EFBFBD>X[4^<EFBFBD><EFBFBD>!2tyZ<EFBFBD>q[_, ɓ<EFBFBD>QxV<EFBFBD>m?
     *
     * Yt@<EFBFBD>B_<EFBFBD><EFBFBD>f<EFBFBD>}: B || (A && C)
     *   - S<EFBFBD><EFBFBD>]:~O<EFBFBD>
YI_V<EFBFBD>-hzV<EFBFBD>t<EFBFBD>QD<EFBFBD> +<EFBFBD>?)<EFBFBD>X[4^Yt@<EFBFBD>B_<EFBFBD>X6nY<EFBFBD><EFBFBD>lo(<EFBFBD><EFBFBD>R]<EFBFBD>m<EFBFBD> <EFBFBD>m3 _<EFBFBD>?     *   - <EFBFBD><EFBFBD><EFBFBD>\<EFBFBD>%<EFBFBD>t3 <EFBFBD>Wcm?+ VL53<EFBFBD><EFBFBD> 2<EFBFBD>WZ<EFBFBD>q[_ +<EFBFBD>?q<EFBFBD><EFBFBD>e<EFBFBD>`Yt@<EFBFBD>B_<EFBFBD>X0|)<EFBFBD>JTNr`i<EFBFBD>\}
     * ============================================================ */
    case GNAV_LINK_STRAIGHT:
        out->override_v  = s_nav.cfg.link_v;
        out->safety_mode = SAFETY_MODE_STRAIGHT;

        /* [Phase-1] e<EFBFBD>X[.rnC]HgExR<EFBFBD>o?+ IMU #Z<EFBFBD>\<EFBFBD>`<EFBFBD><EFBFBD>C<EFBFBD>W
         *
         * X<EFBFBD><EFBFBD>quo<EFBFBD>1l<EFBFBD>Q IMU heading_hold_pd()
         * <EFBFBD>gjn<EFBFBD>,l<EFBFBD><EFBFBD>˓<EFBFBD>n?m͓<EFBFBD>_vf VL53 ȓYef<EFBFBD><EFBFBD>\dee<EFBFBD>X[.r<EFBFBD>t<EFBFBD>o<EFBFBD><EFBFBD>K<EFBFBD>-l<EFBFBD>Z<EFBFBD> b<EFBFBD>[ɓ<EFBFBD>q}<EFBFBD>m?IMU #Z<EFBFBD>\<EFBFBD>`
         *       <EFBFBD>o<EFBFBD>PIpe<EFBFBD>X[.rn?VL53 O<EFBFBD>)14QÓ<EFBFBD>ref<EFBFBD><EFBFBD>\<EFBFBD> <EFBFBD> V<EFBFBD>&h<EFBFBD>X<EFBFBD><EFBFBD>quo(<EFBFBD><EFBFBD>R<EFBFBD>Q IMU
         *
         * <EFBFBD>mp<EFBFBD><EFBFBD>|<EFBFBD>mX	{"<EFBFBD>%1?m͓<6D>_<EFBFBD>XZ<58><5A>^
jK<EFBFBD>3l<EFBFBD>WZ<EFBFBD> b<EFBFBD>[ɓ<EFBFBD>q0}
         *   VL53 ȓ?dS2cm <EFBFBD>mC<EFBFBD><EFBFBD>}<EFBFBD>t<EFBFBD>
j<EFBFBD>}\<EFBFBD>q<EFBFBD>~8Y<EFBFBD>| 12cm +<EFBFBD>?atan2(dS4cm, 12cm) = dS18<EFBFBD>c
         *   c<EFBFBD>D<EFBFBD><EFBFBD><EFBFBD>oD<EFBFBD>G0<EFBFBD>|\	{<EFBFBD><EFBFBD><EFBFBD>P<EFBFBD>`K<EFBFBD>3l<EFBFBD>WZ<EFBFBD>b6^R<EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD>Pg_"<EFBFBD>%1<><31>g<EFBFBD><67>V<EFBFBD>z<EFBFBD>~<7E><><EFBFBD>N<EFBFBD>YD<59><44>`<60>~=<3D>ko<6B><6F>?         */
        {
            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) {
                /* <EFBFBD>YD<EFBFBD><EFBFBD>`K<EFBFBD><EFBFBD>_
j = 9p<EFBFBD>p<EFBFBD>j<EFBFBD>t<EFBFBD>o<EFBFBD><EFBFBD> - )<EFBFBD><EFBFBD><EFBFBD>#r<EFBFBD>t<EFBFBD>o<EFBFBD><EFBFBD>
                 * <EFBFBD>YE<EFBFBD><EFBFBD> ?= <EFBFBD>~<EFBFBD><EFBFBD>?m͓<EFBFBD>_J0ig?+<EFBFBD>?fence_sign P<EFBFBD><EFBFBD>Q~u͓!<EFBFBD>“pt<EFBFBD>` */
                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);

                /* #Z<EFBFBD>\<EFBFBD>`<EFBFBD>,l?m͓<EFBFBD>_<EFBFBD>[ɓ<EFBFBD>q<EFBFBD><EFBFBD>m?+ IMU <EFBFBD><EFBFBD>k<EFBFBD><EFBFBD>|<EFBFBD>m?<EFBFBD><EFBFBD>} */
                float blend = s_nav.cfg.link_wall_blend;
                out->override_w = blend * w_wall + (1.0f - blend) * w_imu;
            } else {
                /* e<EFBFBD>X[.rn?VL53 O<EFBFBD>)14QÓ<EFBFBD>ref +<EFBFBD>?<EFBFBD><EFBFBD><EFBFBD> V<EFBFBD>&h<EFBFBD><EFBFBD>~?IMU<EFBFBD>X{X<EFBFBD><EFBFBD>q<EFBFBD><EFBFBD>mp<EFBFBD><EFBFBD>zw<EFBFBD><EFBFBD><EFBFBD>} */
                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;

            /* <EFBFBD>h(h<EFBFBD>Z<EFBFBD>tg<EFBFBD>}S<EFBFBD><EFBFBD>]:~O<EFBFBD>
Y<EFBFBD>qQ<EFBFBD><EFBFBD>U<EFBFBD> ?*/
            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;
            }

            /* ---- <EFBFBD>m3 _ A: <EFBFBD><EFBFBD><EFBFBD>\<EFBFBD>%<EFBFBD>t?(p<EFBFBD><EFBFBD>Q<EFBFBD>j<EFBFBD><EFBFBD><EFBFBD>d<EFBFBD>|, "<EFBFBD>?85% 9ppt
j) ---- */
            bool odom_ok = odom_since_entry() >= s_nav.cfg.link_distance * 0.85f;

            /* ---- <EFBFBD>m3 _ B: S<EFBFBD><EFBFBD>]:~O<EFBFBD>
YI_V<EFBFBD>-hzV (Bi<j?|4d? ---- */
            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;
                /* ^gA<EFBFBD>闓m^<EFBFBD>>~R<EFBFBD>g<EFBFBD>XuZ<EFBFBD> O<EFBFBD>YAnK<EFBFBD><EFBFBD>_<EFBFBD>u(FRONT_LASER_OFFSET)<EFBFBD>|\~}igkwV"<EFBFBD>'1<>kē<1A>
jJ<EFBFBD>wx}K<EFBFBD><EFBFBD>_<EFBFBD>u<EFBFBD><EFBFBD><EFBFBD>W<EFBFBD>y */
                laser_ok = (d_front_delta >= s_nav.cfg.link_distance * 0.85f);
            }

            /* ---- <EFBFBD>m3 _ C: ȕ<EFBFBD>p?m͓<EFBFBD>_vf VL53 Z<EFBFBD>q[_<EFBFBD>Y<EFBFBD> 4Z?(Ǖ<EFBFBD> ig<EFBFBD>p;u2<EFBFBD><EFBFBD><EFBFBD>] <EFBFBD>t? ----
             *
             * R<EFBFBD><EFBFBD>0~uÕX<EFBFBD> ?0.5m (<EFBFBD><EFBFBD>RuogZ?
             *   <EFBFBD>YE<EFBFBD>6r<EFBFBD>tQg/ps<EFBFBD><EFBFBD>\l<EFBFBD>ȕ!2i VL53 <EFBFBD>t<EFBFBD>~<EFBFBD>f .<EFBFBD>?(<EFBFBD><EFBFBD>6l<EFBFBD>N9p?2 - ^g@<EFBFBD><EFBFBD><EFBFBD>/2 - VL53P<EFBFBD>oT	)
             *                               = (0.40/2 - 0.20/2 - 0.0)
             *                               = 0.10m
             *   R<EFBFBD>g/pZ<EFBFBD>q}Y<EFBFBD>F<EFBFBD>i VL53 <EFBFBD>t<EFBFBD>~<EFBFBD>f > 1.2m (<EFBFBD>tmT<EFBFBD>VȓYef<EFBFBD>t<EFBFBD>o<EFBFBD><EFBFBD>) <EFBFBD><EFBFBD>(h<EFBFBD>h<EFBFBD><EFBFBD>?             *   ÕX<EFBFBD> ?0.5m f<EFBFBD>$1<EFBFBD><EFBFBD>p<EFBFBD>lT<EFBFBD>{•<EFBFBD><EFBFBD>}<EFBFBD>t<EFBFBD>QD<EFBFBD>V<EFBFBD><EFBFBD>T<EFBFBD>W
             */
            /* cd <EFBFBD>[<EFBFBD>cjn<EFBFBD>mCZ<EFBFBD>ge<EFBFBD>X[.rnG<EFBFBD> f<EFBFBD>}<EFBFBD>ma<EFBFBD>O^Y<EFBFBD>5h}<EFBFBD>o<EFBFBD>]de */
            /* f<EFBFBD>?LINK_STRAIGHT Õ5<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>[urrent_corridor_id `m<EFBFBD>]<EFBFBD>iR<EFBFBD>3lˆ9p<EFBFBD>\<EFBFBD>kZ?(Op-lmnǓX[<EFBFBD>g)
               <EFBFBD><EFBFBD><EFBFBD> `m?cd->travel_dir Op<EFBFBD><EFBFBD><EFBFBD>iR<EFBFBD>3lˆ9p<EFBFBD>\EPɓ!<EFBFBD>w(<EFBFBD><EFBFBD>R<EFBFBD>gZ<EFBFBD>/b})<EFBFBD>X[4^"<EFBFBD>&1uoR<6F><52>0<EFBFBD>g0<67><30>d<EFBFBD>Xf<58>%1bd<62>mF<6D>lக)1<> 6l<36>N */
            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);  /* ig<EFBFBD>p;u2<EFBFBD><EFBFBD>?(40ms @ 20ms[<EFBFBD>&1an) */

            /* ---- q<EFBFBD><EFBFBD>e<EFBFBD>`R<EFBFBD><EFBFBD>0~u: 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;

    /* ============================================================
     * Q<EFBFBD><EFBFBD>T<EFBFBD>n)<EFBFBD><EFBFBD>x<EFBFBD>
     * ============================================================ */
    case GNAV_EXIT_STRAIGHT:
        out->override_v  = s_nav.cfg.exit_v;
        out->safety_mode = SAFETY_MODE_STRAIGHT;

        /* [Phase-1] Q<EFBFBD><EFBFBD>T<EFBFBD>nZxQ<EFBFBD>{cm<EFBFBD><EFBFBD>dee<EFBFBD>X[.rnFR<EFBFBD>ok6^R<EFBFBD>?         * C6 Q<EFBFBD><EFBFBD>p<EFBFBD>wZ<EFBFBD><EFBFBD>^jn<EFBFBD>[><EFBFBD>l૕6l<EFBFBD>NW<EFBFBD>Di<EFBFBD><EFBFBD>}\?m͓<EFBFBD>_jn<EFBFBD>[?<EFBFBD>vf<EFBFBD>X{ LINK_STRAIGHT O<EFBFBD><EFBFBD><EFBFBD>de<EFBFBD><EFBFBD>f<EFBFBD>}<EFBFBD>?         * <EFBFBD>t<EFBFBD>o<EFBFBD><EFBFBD> ~3.9m<EFBFBD>~\.vig<EFBFBD>p4^Z8u<EFBFBD>f<EFBFBD>[@i?<EFBFBD><EFBFBD>}\?m͓<EFBFBD>_<EFBFBD>[ɓ<EFBFBD>q?mȓ	Y<EFBFBD>sJ<EFBFBD>?         */
        {
            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);
        }

        /* <EFBFBD>Y<EFBFBD> 4Z)[vfZ<EFBFBD>b<EFBFBD>S<EFBFBD>m?*/
        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);
            }
        }
        /* <EFBFBD><EFBFBD><EFBFBD>\<EFBFBD>%<EFBFBD>mGZ<EFBFBD>j<EFBFBD>m<EFBFBD>o<EFBFBD>Y */
        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;

    /* ============================================================
     * e<EFBFBD><EFBFBD>p<EFBFBD>NZ<EFBFBD><EFBFBD><EFBFBD>YV<EFBFBD>?     * ============================================================ */
    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;

    /* ============================================================
     * 
X<EFBFBD> ?     * ============================================================ */
    case GNAV_FINISHED:
        out->override_v = 0.0f;
        out->override_w = 0.0f;
        out->safety_mode = SAFETY_MODE_IDLE;
        out->active = false;
        break;

    /* ============================================================
     * <EFBFBD>[<EFBFBD>P6r<EFBFBD><EFBFBD>?     * ============================================================ */
    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;
    }

    /* ǓX[<EFBFBD>gHg<EFBFBD>d<EFBFBD>VÕ5<EFBFBD><EFBFBD><EFBFBD> (Y<EFBFBD><EFBFBD>XQf<EFBFBD>?switch P<EFBFBD>mT<EFBFBD>Q
?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;
}