# ============================================================ # INTEGRATION_GUIDE.py # Гайд по интеграции модулей перехвата в main.py # # НЕ ЗАПУСКАТЬ — это документация с фрагментами кода. # Каждый блок показывает, куда именно вставить код в main.py. # ============================================================ # ───────────────────────────────────────────────────────────── # ШАГ 1: ИМПОРТЫ (добавить в начало main.py, после существующих) # ───────────────────────────────────────────────────────────── """ import config_intercept # новый конфиг from config_intercept import * from range_estimation import RangeEstimator from proportional_navigation import ProportionalNavigator from intercept_fsm import InterceptFSM, InterceptPhase from autopilot_bridge import AutopilotBridge from imm_filter import IMMFilter """ # ───────────────────────────────────────────────────────────── # ШАГ 2: ИНИЦИАЛИЗАЦИЯ (в main(), после создания guidance_ctrl) # ───────────────────────────────────────────────────────────── """ # --- Intercept modules --- range_est = RangeEstimator() print(range_est.status_line()) pn_nav = ProportionalNavigator() print(pn_nav.status_line()) intercept_fsm = InterceptFSM() print(intercept_fsm.status_line()) autopilot = AutopilotBridge() autopilot.start() print(autopilot.status_line()) imm = IMMFilter() print(imm.status_line()) # Состояния для нового пайплайна range_state = None pn_state = None intercept_params = None """ # ───────────────────────────────────────────────────────────── # ШАГ 3: KALMAN → IMM (опционально, если IMM_REPLACE_KALMAN=True) # Заменяет строку: kf = Kalman8D() # ───────────────────────────────────────────────────────────── """ if IMM_REPLACE_KALMAN and IMM_ENABLE: kf = imm # IMMFilter имеет тот же API что Kalman8D else: kf = Kalman8D() """ # ───────────────────────────────────────────────────────────── # ШАГ 4: RANGE ESTIMATION (внутри главного цикла while True, # ПОСЛЕ того как locked_box_eff обновлён и guidance_state # вычислен, т.е. перед отрисовкой) # ───────────────────────────────────────────────────────────── """ # --- Range estimation --- if confirmed and locked_box_eff is not None: range_state = range_est.update( locked_box_eff, frame_ts, dt, confirmed=True ) elif pred_box_eff is not None: range_state = range_est.update( pred_box_eff, frame_ts, dt, confirmed=False ) else: range_state = range_est.update(None, frame_ts, dt) # При потере цели — сброс if miss_streak >= MAX_MISSES: range_est.reset() range_state = None """ # ───────────────────────────────────────────────────────────── # ШАГ 5: INTERCEPT FSM (после range estimation) # ───────────────────────────────────────────────────────────── """ # --- Intercept FSM --- intercept_params = intercept_fsm.update( confirmed=confirmed, hit_streak=hit_streak, miss_streak=miss_streak, range_state=range_state, guidance_state=guidance_state, frame_id=frame_id, ) # FSM может переопределить det_every if intercept_params.get("det_every_override") is not None: det_every = min(int(det_every), int(intercept_params["det_every_override"])) # FSM может заставить fullscan if intercept_params.get("force_fullscan"): need_fullscan = True """ # ───────────────────────────────────────────────────────────── # ШАГ 6: PROPORTIONAL NAVIGATION (после FSM, перед autopilot) # ───────────────────────────────────────────────────────────── """ # --- Proportional Navigation --- pn_state = None if intercept_params.get("use_pn") and confirmed: # Если есть 3D-оценка дальности if (range_state is not None and range_state.get("range_m") is not None and range_state["confidence"] > 0.3): # Позиция цели в метрах (упрощённо: проекция из пикселей) target_center = box_center(locked_box_eff if locked_box_eff is not None else pred_box_eff) my_center = np.array([ew * 0.5, eh * 0.5], dtype=np.float32) # Пиксели → метры (через range и FOV) range_m = float(range_state["range_m"]) px_to_m = range_m / max(1.0, range_est.focal_px) target_pos_m = (target_center - my_center) * px_to_m my_pos_m = np.zeros(2, dtype=np.float32) # Скорости vx_px = float(kf.x[4, 0]) if kf.initialized else 0.0 vy_px = float(kf.x[5, 0]) if kf.initialized else 0.0 target_vel_m = np.array([vx_px, vy_px], dtype=np.float32) * px_to_m my_vel_m = np.zeros(2, dtype=np.float32) # неизвестна без IMU pn_state = pn_nav.update_3d( my_pos_m, my_vel_m, target_pos_m, target_vel_m, closing_vel=range_state.get("closing_vel", 0.0), timestamp=frame_ts, ) else: # Fallback: screen-space PN target_center = None if locked_box_eff is not None: target_center = box_center(locked_box_eff) elif pred_box_eff is not None: target_center = box_center(pred_box_eff) target_vel_px = None if kf.initialized: target_vel_px = np.array( [float(kf.x[4, 0]), float(kf.x[5, 0])], dtype=np.float32, ) if target_center is not None: pn_state = pn_nav.update_screen( target_center, ew, eh, target_vel_px, frame_ts, ) else: pn_nav.reset() """ # ───────────────────────────────────────────────────────────── # ШАГ 7: AUTOPILOT BRIDGE (после PN) # ───────────────────────────────────────────────────────────── """ # --- Autopilot bridge --- autopilot_cmd = autopilot.update( frame_id=frame_id, timestamp=frame_ts, guidance_state=guidance_state, intercept_params=intercept_params, range_state=range_state, pn_state=pn_state, confirmed=confirmed, miss_streak=miss_streak, ) """ # ───────────────────────────────────────────────────────────── # ШАГ 8: ОТРИСОВКА (после существующей отрисовки, перед writer) # ───────────────────────────────────────────────────────────── """ # --- Intercept overlays --- if range_state is not None: range_est.draw_overlay(frame_orig, range_state, sx, sy) if pn_state is not None: pn_nav.draw_overlay(frame_orig, pn_state, sx, sy) if intercept_params is not None: intercept_fsm.draw_overlay(frame_orig, intercept_params) if IMM_ENABLE and IMM_REPLACE_KALMAN: imm.draw_overlay(frame_orig) """ # ───────────────────────────────────────────────────────────── # ШАГ 9: ЛОГИРОВАНИЕ (расширить track_logger.log_frame) # ───────────────────────────────────────────────────────────── """ # Добавить к существующему вызову track_logger.log_frame(): # (нужно расширить FIELDNAMES в decision_logger.py) # Или создать отдельный лог: if intercept_params and range_state: # Дополнительные метрики для анализа pass # см. decision_logger.py → добавить поля """ # ───────────────────────────────────────────────────────────── # ШАГ 10: CLEANUP (после главного цикла, перед cap.release()) # ───────────────────────────────────────────────────────────── """ autopilot.stop() # Остальной cleanup как прежде: autogaze_worker.stop() yolo_worker.stop() ... """ # ───────────────────────────────────────────────────────────── # ШАГ 11: СБРОС ПРИ ПОТЕРЕ ЦЕЛИ # В блоках где происходит полный reset (miss_streak >= MAX_MISSES # и другие блоки сброса в main.py), добавить: # ───────────────────────────────────────────────────────────── """ range_est.reset() pn_nav.reset() intercept_fsm.reset() range_state = None pn_state = None intercept_params = None """ # ============================================================= # ПОРЯДОК ВЫЗОВОВ В КАЖДОМ КАДРЕ (итоговый пайплайн): # ============================================================= # # 1. Чтение кадра, get_effective_frame # 2. Camera motion compensation # 3. Kalman/IMM predict # 4. KLT update # 5. YOLO submit / get # 6. ByteTrack update # 7. Target selection (существующая логика) # 8. Kalman/IMM update (chosen box) # 9. Guidance update (screen guidance) ← существующий # 10. Range estimation update ← НОВЫЙ # 11. Intercept FSM update ← НОВЫЙ # 12. Proportional Navigation update ← НОВЫЙ # 13. Autopilot bridge update ← НОВЫЙ # 14. Decision logger # 15. Draw overlays # 16. Video writer / display # ============================================================= # ============================================================= # КАЛИБРОВКА КАМЕРЫ # ============================================================= # # Для точной оценки дальности нужно измерить focal_length_px. # # Метод 1 (рекомендуемый): # 1. Поставить объект известного размера (например, 30см линейку) # на известном расстоянии (например, 5 метров) # 2. Сфотографировать, измерить размер в пикселях # 3. focal_px = (pixel_size * distance) / real_size # Пример: линейка 30см = 42px на расстоянии 5м: # focal_px = 42 * 5.0 / 0.3 = 700 # # Метод 2 (из спецификации камеры): # focal_px = focal_mm * image_width_px / sensor_width_mm # Типичные FPV камеры: # - Caddx Ratel 2: 2.1mm lens, 1/1.8" sensor → ~350px # - RunCam Phoenix: 2.1mm lens, 1/2" sensor → ~315px # - DJI O3 Air: 2.1mm lens → ~420px # # Метод 3 (автокалибровка, грубый): # Если знаете размер цели (0.27м для FPV дрона) и примерно # расстояние при первом захвате, можно подобрать focal_px # итеративно. # =============================================================