diff --git a/armor/src/armor_finder/anti_top/anti_top.cpp b/armor/src/armor_finder/anti_top/anti_top.cpp index 80115b1..83533ac 100644 --- a/armor/src/armor_finder/anti_top/anti_top.cpp +++ b/armor/src/armor_finder/anti_top/anti_top.cpp @@ -106,11 +106,26 @@ void ArmorFinder::antiTop(double dist_m, double pitch_imu_deg) { // 5. Check if it's time to fire double fly_time_s = BallisticSolver::get_flight_time(dist_m, pitch_imu_deg, MUZZLE_VELOCITY, BALLISTIC_K); - double total_delay_s = fly_time_s + SYSTEM_DELAY / 1000.0; + // 核心优化:O(1) 绝对相位耦合算法 + // 原理:子弹必须在装甲板精确到达 Yaw 中心的那一刻撞击 + double T_face = abs((PI / 2.0) / auto_omega); // 陀螺转过一个面的理论耗时 - double wait_time_s = min_t_hit - total_delay_s; - if (wait_time_s < 0) { - wait_time_s += abs((PI / 2.0) / auto_omega); // Look at the next plate + // 如果我们现在立刻开火,子弹到达的时间点超越了下一块板的到来时间, + // 意味着我们必然“脱靶”(子弹飞在半空时,最近的板子已经溜过去了)。 + // 在这个差值 time_shortfall 内,我们究竟错过了几块板子? + double time_shortfall = total_delay_s - min_t_hit; + + double wait_time_s = 0.0; + if (time_shortfall <= 0) { + // 第一块板子的时间还没到,完全赶得及!我们只需要原地等差值补齐就行。 + wait_time_s = -time_shortfall; + } else { + // 延迟太高,必定完美错过前面的板子。 + // 通过 ceil(向上取整)推导出“子弹飞到时,我们至少要迎击第 n 块板”: + int n_plates_missed = ceil(time_shortfall / T_face); + + // 我们真实需要等待的时机 = 命中第 n 块板的绝对时间 - 子弹与系统的总延迟 + wait_time_s = (min_t_hit + n_plates_missed * T_face) - total_delay_s; } bool fire = false;