opti : 327.6 Hz [~5s] | t265 : 219.6 | dyna : 60.2 | sbus : 46.5 | main : 404.4 opti : 335.4 Hz [~5s] | t265 : 219.7 | dyna : 59.9 | sbus : 46.1 | main : 404.4 opti : 333.4 Hz [~5s] | t265 : 219.6 | dyna : 60.1 | sbus : 46.8 | main : 404.4 opti : 333.5 Hz [~5s] | t265 : 219.7 | dyna : 59.7 | sbus : 46.7 | main : 404.4 opti : 331.8 Hz [~5s] | t265 : 219.6 | dyna : 60.0 | sbus : 46.2 | main : 404.4 opti : 332.9 Hz [~5s] | t265 : 219.7 | dyna : 59.7 | sbus : 47.1 | main : 404.4 opti : 334.8 Hz [~5s] | t265 : 219.5 | dyna : 60.1 | sbus : 48.0 | main : 404.4 opti : 333.3 Hz [~5s] | t265 : 221.5 | dyna : 59.6 | sbus : 47.1 | main : 405.4 opti : 333.9 Hz [~5s] | t265 : 221.4 | dyna : 59.8 | sbus : 46.3 | main : 405.4 opti : 334.2 Hz [~5s] | t265 : 221.5 | dyna : 59.7 | sbus : 47.9 | main : 405.4 opti : 334.5 Hz [~5s] | t265 : 221.3 | dyna : 59.8 | sbus : 48.0 | main : 406.4 opti : 334.0 Hz [~5s] | t265 : 223.1 | dyna : 59.7 | sbus : 45.9 | main : 407.3 opti : 335.7 Hz [~5s] | t265 : 223.1 | dyna : 59.8 | sbus : 47.5 | main : 406.3 opti : 333.8 Hz [~5s] | t265 : 221.3 | dyna : 60.1 | sbus : 48.2 | main : 406.3 opti : 334.7 Hz [~5s] | t265 : 219.5 | dyna : 60.0 | sbus : 46.2 | main : 406.3 opti : 335.7 Hz [~5s] | t265 : 219.5 | dyna : 60.1 | sbus : 47.5 | main : 405.3 opti : 335.4 Hz [~5s] | t265 : 219.5 | dyna : 60.3 | sbus : 46.2 | main : 404.4 opti : 335.7 Hz [~5s] | t265 : 219.5 | dyna : 60.4 | sbus : 46.9 | main : 404.3 opti : 335.4 Hz [~5s] | t265 : 219.5 | dyna : 60.2 | sbus : 46.4 | main : 404.3 opti : 335.1 Hz [~5s] | t265 : 219.5 | dyna : 60.2 | sbus : 48.7 | main : 404.4 opti : 334.4 Hz [~5s] | t265 : 219.5 | dyna : 60.3 | sbus : 47.0 | main : 404.4 opti : 335.4 Hz [~5s] | t265 : 230.8 | dyna : 59.9 | sbus : 46.5 | main : 410.1 opti : 336.6 Hz [~5s] | t265 : 239.5 | dyna : 60.6 | sbus : 44.9 | main : 414.5 opti : 337.3 Hz [~5s] | t265 : 247.5 | dyna : 60.4 | sbus : 47.3 | main : 421.1 opti : 343.0 Hz [~5s] | t265 : 246.8 | dyna : 60.1 | sbus : 46.1 | main : 426.1 opti : 333.7 Hz [~5s] | t265 : 246.0 | dyna : 59.9 | sbus : 45.9 | main : 427.7 opti : 340.3 Hz [~5s] | t265 : 240.4 | dyna : 61.1 | sbus : 45.5 | main : 426.7 opti : 341.3 Hz [~5s] | t265 : 249.7 | dyna : 59.8 | sbus : 46.3 | main : 428.9 opti : 338.6 Hz [~5s] | t265 : 246.4 | dyna : 59.9 | sbus : 46.1 | main : 426.3

// ===== HZ MONITOR (5-second average) =====
constexpr double HZ_AVG_SEC = 5.0;

// Use expected rates to set window sizes (~5 seconds worth of updates)
constexpr int W_OPTI = 600;   // ~120 Hz * 5 s (adjust if needed)
constexpr int W_T265 = 1000;  // ~200 Hz * 5 s
constexpr int W_DXL  = 2000;  // ~400 Hz * 5 s
constexpr int W_SBUS = 700;   // ~140 Hz * 5 s
constexpr int W_MAIN = 1850;  // ~370 Hz * 5 s (CTRL_DT=2.7ms)

template<int W>
struct HzMon {
  uint64_t last_cnt = 0;
  uint64_t last_t_ns = 0;
  double hist[W] = {0.0};
  int idx = 0;
  int filled = 0;

  double update(uint64_t cnt, uint64_t now_ns) {
    if (last_t_ns == 0) { last_cnt = cnt; last_t_ns = now_ns; return 0.0; }

    const uint64_t dc = cnt - last_cnt;
    const double dt = (now_ns - last_t_ns) * 1e-9;

    last_cnt = cnt;
    last_t_ns = now_ns;

    if (dt <= 1e-6) return 0.0;

    const double hz = static_cast<double>(dc) / dt;

    hist[idx] = hz;
    idx = (idx + 1) % W;
    if (filled < W) filled++;

    double sum = 0.0;
    for (int i = 0; i < filled; ++i) sum += hist[i];
    return sum / filled;
  }
};

HzMon<W_OPTI> hz_opti;
HzMon<W_T265> hz_t265;
HzMon<W_DXL>  hz_dxl;
HzMon<W_SBUS> hz_sbus;
HzMon<W_MAIN> hz_main;

uint64_t last_print_ns = 0;
uint64_t main_loop_cnt = 0;

맨위에 와일 밖에다가

다이나믹셀 리드

    // ===== HZ MONITOR UPDATE/PRINT (5s average) =====
    const uint64_t now_ns = now_steady_ns();
    main_loop_cnt++;

    const double opti_rate = hz_opti.update(opti.get_frame_count(), now_ns);
    const double t265_rate = hz_t265.update(t265.get_frame_count(), now_ns);
    const double dxl_rate  = hz_dxl.update(dxl.get_frame_count(),  now_ns);
    const double sbus_rate = hz_sbus.update(sbus.get_frame_count(), now_ns);
    const double main_rate = hz_main.update(main_loop_cnt, now_ns);

    // print every 1s (values are ~5s moving average)
    if (now_ns - last_print_ns > 1'000'000'000ULL) {
      std::fprintf(stdout,
        "opti : %6.1f Hz [~%.0fs] | t265 : %6.1f | dyna : %6.1f | sbus : %6.1f | main : %6.1f\n",
        opti_rate, HZ_AVG_SEC, t265_rate, dxl_rate, sbus_rate, main_rate);
      std::fflush(stdout);
      last_print_ns = now_ns;
    }
    
    포지션 컨트롤