from __future__ import annotations import logging, time, asyncio from typing import Optional from . import config, state from .geom import ang_diff_signed, normalize360, sector_contains, is_near from .ptz_io import move_ptz_bounded, stop_ptz, goto_preset_token, read_ptz_azimuth_deg from .notify import notify_detected logger = logging.getLogger("PTZTracker") # ===== DEBUG helpers ===== def _dbg_state(cam_idx: int, msg: str) -> None: """ Унифицированный вывод текущего состояния патруля по камере. Важно: пишет на уровне DEBUG, чтобы не засрать лог при INFO. """ st = state.ptz_states.get(cam_idx, {}) cfg = config.CAMERA_CONFIG.get(cam_idx, {}) logger.debug( "[PATROL][DBG] cam=%s %s | " "mode=%s act=%s paused=%s track=%s sync_wait=%s " "dir=%s endpoint_latch=%s " "leg=[%s..%s] " "sector=[%s..%s] " "az_deg=%s", cam_idx, msg, st.get("mode"), st.get("patrol_active"), st.get("patrol_paused"), st.get("tracking_active"), st.get("sync_wait"), st.get("patrol_dir"), st.get("endpoint_latch"), st.get("leg_start"), st.get("leg_end"), cfg.get("sector_min_deg"), cfg.get("sector_max_deg"), st.get("az_deg"), ) def _dbg_skip(cam_idx: int, reason: str) -> None: """ Логируем «почему супервизор не трогает камеру», но с троттлингом, чтобы сообщение не сыпалось на каждый тик. """ now = time.time() key = f"_patrol_dbg_skip_{cam_idx}" last = getattr(state, key, 0.0) # не чаще раза в 3 секунды if now - last >= 3.0: setattr(state, key, now) _dbg_state(cam_idx, f"skip: {reason}") # ===== helpers ===== def _nearest_valid_preset(cam_idx: int) -> Optional[str]: cfg = config.CAMERA_CONFIG[cam_idx] st = state.ptz_states[cam_idx] az = st.get("az_deg") p1, p2 = cfg.get("preset1"), cfg.get("preset2") if not (p1 and p2): return p1 or p2 if az is None: return p1 a1, a2 = cfg.get("preset1_deg"), cfg.get("preset2_deg") if isinstance(a1, (int, float)) and isinstance(a2, (int, float)): d1 = abs(ang_diff_signed(az, a1)) d2 = abs(ang_diff_signed(az, a2)) return p1 if d1 <= d2 else p2 return p1 def _in_sector(pan: float | None, cfg: dict) -> bool | None: """ Обёртка над sector_contains(angle, dmin, dmax), безопасная к None. Возвращает: - True/False, если сектор задан и угол валиден; - None, если сектор не задан или pan некорректен. """ smin = cfg.get("sector_min_deg") smax = cfg.get("sector_max_deg") if not ( isinstance(smin, (int, float)) and isinstance(smax, (int, float)) and isinstance(pan, (int, float)) ): return None return sector_contains(float(pan), float(smin), float(smax)) def _near_endpoint(pan: float | None, cfg: dict, tol: float = 1.0) -> bool: """ Проверка, близко ли мы к одной из preset1_deg / preset2_deg с учётом возможных None и нечисловых значений. """ if not isinstance(pan, (int, float)): return False a1 = cfg.get("preset1_deg") a2 = cfg.get("preset2_deg") if isinstance(a1, (int, float)) and is_near(float(pan), float(a1), tol): return True if isinstance(a2, (int, float)) and is_near(float(pan), float(a2), tol): return True return False def return_to_preset(cam_idx: int, reason: str = "") -> None: st = state.ptz_states[cam_idx] now = time.time() logger.info("[RETURN] cam %s -> preset (reason=%s)", cam_idx, reason) # Анти-спам: если только что уже возвращали — не дёргать снова if (now - float(st.get("last_return_at", 0.0))) < 2.5: _dbg_state(cam_idx, "return_to_preset: suppressed by cooldown") return # Гасим запись/детект и переводим в IDLE notify_detected(cam_idx, False, force=True) st["mode"] = "IDLE" st["tracking_active"] = False st["patrol_paused"] = False # важно: не блокируем future-resume # Сброс регуляторов и вспомогательных состояний st["last_dx"] = 0.0 st["last_dy"] = 0.0 st["ix"] = 0.0 st["iy"] = 0.0 st["zoom_int"] = 0.0 st["zoom_prev_cmd"] = 0.0 st["zoom_ramp_start"] = 0.0 st["stable_frames"] = 0 st["alarm_sent"] = False st["zoom_state"] = 0 st["was_zoomed_in"] = False # Сброс логики зума/потерь st["zoom_lock"] = False st["lock_candidate_since"] = None st["loss_since"] = None # Убираем «эхо» от последней цели и защёлку у пресета st["last_bbox"] = None st["endpoint_latch"] = 0 # 0=вне пресетов, 1=P1, 2=P2 # Останавливаем текущие команды PTZ stop_ptz(cam_idx) # Выбор ближайшего валидного пресета и установка направления следующей «ноги» cfg = config.CAMERA_CONFIG[cam_idx] p1, p2 = cfg.get("preset1"), cfg.get("preset2") tok = _nearest_valid_preset(cam_idx) if tok is not None and p1 and p2: st["patrol_dir"] = +1 if tok == p1 else -1 # Важно: пока камера едет на goto, не слать continuous команды settle = float(getattr(config, "GOTO_SETTLE_SEC", 2.5)) dwell = float(getattr(config, "END_DWELL_SEC", 1.0) or 1.0) st["goto_in_progress_until"] = now + settle if tok: goto_preset_token(cam_idx, tok) # Сброс режима патруля на время возврата st["patrol_active"] = False st["sweep_active"] = False st["sweep_pan"] = 0.0 st["sweep_start_at"] = 0.0 st["leg_start"] = now st["leg_end"] = now + max(1.0, dwell) # Анти-спам метка st["last_return_at"] = now #ВАЖНО: всегда планируем возобновление патруля (и при заданном секторе тоже) try: _dbg_state(cam_idx, f"return_to_preset: schedule patrol resume after settle+dwell={settle+dwell:.2f}s") ensure_patrol_resume(cam_idx, delay=max(0.5, settle + dwell)) except Exception: logger.exception("[PATROL] ensure_patrol_resume failed in return_to_preset") _dbg_state(cam_idx, f"return_to_preset done reason={reason}") def ensure_patrol_resume(cam_idx: int, delay: float = 1.0) -> None: st = state.ptz_states[cam_idx] cfg = config.CAMERA_CONFIG[cam_idx] now = time.time() # если идёт трек или стоит пауза — не возобновляем патруль if st.get("tracking_active") or st.get("patrol_paused"): _dbg_state(cam_idx, "ensure_patrol_resume: skip (tracking_active or patrol_paused)") return # анти-спам резюмом (важно, иначе будешь вечно в goto) RESUME_COOLDOWN_SEC = float(getattr(config, "PATROL_RESUME_COOLDOWN_SEC", 2.0)) last = float(st.get("patrol_resume_last_at", 0.0) or 0.0) if (now - last) < RESUME_COOLDOWN_SEC: _dbg_state(cam_idx, "ensure_patrol_resume: suppressed by resume cooldown") return st["patrol_resume_last_at"] = now # если только что был goto — ждём пока доедет gip = float(st.get("goto_in_progress_until", 0.0) or 0.0) if gip > now: delay = max(delay, gip - now) # --- Выбираем ближайший валидный пресет --- try: tok = _nearest_valid_preset(cam_idx) # "1"/"2"/... except Exception: tok = None p1 = cfg.get("preset1") p2 = cfg.get("preset2") # --- ВАЖНО: выставляем направление патруля исходя из того, в каком пресете окажемся --- # Логика: если стоим на P1 -> едем к P2 (dir=+1), если стоим на P2 -> едем к P1 (dir=-1) if tok and p1 and p2: if tok == p1: st["patrol_dir"] = +1 st["endpoint_latch"] = 1 elif tok == p2: st["patrol_dir"] = -1 st["endpoint_latch"] = 2 # --- Парковка на пресет перед патрулём: ТОЛЬКО если реально не уже на нём --- do_goto = False if tok: az = st.get("az_deg") # если умеешь — используй твой _near_endpoint(az,cfg,tol) try: near = _near_endpoint(az, cfg, tol=float(getattr(config, "PATROL_PARK_TOL_DEG", 1.0))) except Exception: near = False # не дёргаем goto, если уже рядом с пресетом (иначе вечный goto и патруль “умирает”) if not near: do_goto = True if do_goto: stop_ptz(cam_idx) settle = float(getattr(config, "GOTO_SETTLE_SEC", 2.5)) st["goto_in_progress_until"] = max(float(st.get("goto_in_progress_until", 0.0) or 0.0), now + settle) goto_preset_token(cam_idx, tok) delay = max(delay, settle + 0.5) # --- Планируем патруль --- st["patrol_active"] = True # чтобы bounded-тактик не скипал из-за mode if st.get("mode") not in ("IDLE", "SEARCH"): st["mode"] = "IDLE" st["leg_extend_total"] = 0.0 # важно для твоего bounded-продления st["leg_start"] = now + delay st["leg_end"] = st["leg_start"] + config.PATROL_LEG_SEC # sweep режим только если нет сектора if cfg.get("sector_min_deg") is None or cfg.get("sector_max_deg") is None: base = config.SWEEP_PAN_SPEED if st.get("patrol_dir", +1) > 0 else -config.SWEEP_PAN_SPEED st["sweep_active"] = True st["sweep_pan"] = float(cfg.get("pan_sign", 1) or 1) * base st["sweep_start_at"] = st["leg_start"] + config.SWEEP_SETTLE_SEC else: st["sweep_active"] = False st["sweep_pan"] = 0.0 st["sweep_start_at"] = 0.0 _dbg_state(cam_idx, f"ensure_patrol_resume: scheduled leg after delay={delay} tok={tok} dir={st.get('patrol_dir')}") def patrol_start_leg(cam_idx: int, dir_sign: int) -> None: st = state.ptz_states[cam_idx] # не стартуем «ногу», если трек или пауза патруля if st.get("tracking_active") or st.get("patrol_paused"): _dbg_state(cam_idx, "patrol_start_leg: blocked (tracking_active or patrol_paused)") return cfg = config.CAMERA_CONFIG[cam_idx] st["patrol_dir"] = 1 if dir_sign >= 0 else -1 st["patrol_active"] = True now = time.time() st["leg_start"] = now st["leg_end"] = now + config.PATROL_LEG_SEC # НИЧЕГО НЕ ДЁРГАЕМ GOTO — патруль дальше ведёт supervisor continuous-ом # Просто запускаем sweep (если включен unbounded режим) if config.SMOOTH_SWEEP_ENABLE: base = config.SWEEP_PAN_SPEED if (st["patrol_dir"] > 0) else -config.SWEEP_PAN_SPEED st["sweep_active"] = True st["sweep_pan"] = float(cfg.get("pan_sign", 1) or 1) * base st["sweep_start_at"] = now + config.SWEEP_SETTLE_SEC else: st["sweep_active"] = False st["sweep_pan"] = 0.0 st["sweep_start_at"] = 0.0 st["shot_pause_until"] = 0.0 st["shot_next_at"] = now + config.SWEEP_SETTLE_SEC + 0.35 logger.info("[PATROL] cam %s leg %s: started (no GOTO)", cam_idx, "+1" if st["patrol_dir"] > 0 else "-1") _dbg_state(cam_idx, "patrol_start_leg: started") def patrol_finish_and_reverse(cam_idx: int) -> None: cfg = config.CAMERA_CONFIG[cam_idx] st = state.ptz_states[cam_idx] now = time.time() _dbg_state(cam_idx, "patrol_finish_and_reverse: begin") # Останавливаем sweep-состояния (bounded/unbounded обновится ниже) st["sweep_active"] = False st["sweep_pan"] = 0.0 # ВАЖНО: никакого GOTO на конец. Просто меняем направление ноги и едем continuous-ом. st["patrol_dir"] = -st["patrol_dir"] st["leg_start"] = now st["leg_end"] = now + config.PATROL_LEG_SEC # для unbounded — включаем sweep обратно if cfg.get("sector_min_deg") is None or cfg.get("sector_max_deg") is None: base = config.SWEEP_PAN_SPEED if (st["patrol_dir"] > 0) else -config.SWEEP_PAN_SPEED st["sweep_pan"] = float(cfg.get("pan_sign", 1) or 1) * base st["sweep_active"] = True st["sweep_start_at"] = now + float(max(0.0, config.END_DWELL_SEC)) else: # bounded — sweep не нужен, bounded supervisor сам поведёт st["sweep_active"] = False st["sweep_pan"] = 0.0 st["sweep_start_at"] = 0.0 st["mode"] = "IDLE" notify_detected(cam_idx, False, force=True) _dbg_state(cam_idx, "patrol_finish_and_reverse: reversed leg (no GOTO)") def patrol_start_leg_bounded(cam_idx: int, dir_sign: int) -> None: st = state.ptz_states[cam_idx] if st.get("tracking_active") or st.get("patrol_paused"): _dbg_state(cam_idx, "patrol_start_leg_bounded: blocked (tracking_active or patrol_paused)") return cfg = config.CAMERA_CONFIG[cam_idx] if cfg.get("sector_min_deg") is None or cfg.get("sector_max_deg") is None: _dbg_state(cam_idx, "patrol_start_leg_bounded: no sector -> fallback to unbounded") patrol_start_leg(cam_idx, dir_sign) return st["patrol_dir"] = 1 if dir_sign >= 0 else -1 st["patrol_active"] = True now = time.time() st["leg_start"] = now st["leg_end"] = now + config.PATROL_LEG_SEC # bounded режим: движение делает patrol_supervisor_tick_bounded() (continuous), # поэтому тут GOTO не используем st["sweep_active"] = False st["sweep_pan"] = 0.0 st["sweep_start_at"] = 0.0 st["shot_pause_until"] = 0.0 st["shot_next_at"] = now + 1.0 logger.info("[PATROL/BOUNDED] cam %s leg %s: started (no GOTO)", cam_idx, "+1" if st["patrol_dir"] > 0 else "-1") _dbg_state(cam_idx, "patrol_start_leg_bounded: started") def patrol_supervisor_tick_bounded() -> None: now = time.time() dead_band_deg = 0.25 min_step_cmd = 0.02 az_none_fallback_sec = 2.0 # New (configurable) end_tol_deg = float(getattr(config, "PATROL_END_TOL_DEG", 1.2)) extend_step_sec = float(getattr(config, "PATROL_LEG_EXTEND_SEC", 2.0)) extend_max_sec = float(getattr(config, "PATROL_LEG_EXTEND_MAX_SEC", 20.0)) max_speed = float(getattr(config, "PATROL_MAX_SPEED", 0.25)) gain_deg = float(getattr(config, "PATROL_PAN_GAIN_DEG", 45.0)) scale = float(getattr(config, "PATROL_SPEED_SCALE", 0.70)) for cam in config.PTZ_CAM_IDS: st = state.ptz_states[cam] cfg = config.CAMERA_CONFIG[cam] # While camera is doing goto — don't send continuous commands if now < float(st.get("goto_in_progress_until", 0.0) or 0.0): _dbg_skip(cam, "bounded: goto_in_progress") continue if st.get("tracking_active") or st.get("patrol_paused"): _dbg_skip(cam, "bounded: tracking_active or patrol_paused") continue if cfg.get("sector_min_deg") is None or cfg.get("sector_max_deg") is None: _dbg_skip(cam, "bounded: no sector_min/max_deg") continue if not st.get("patrol_active"): _dbg_skip(cam, "bounded: patrol_active=False") continue if st.get("mode") not in ("IDLE", "SEARCH"): _dbg_skip(cam, f"bounded: mode={st.get('mode')} not IDLE/SEARCH") continue az1 = cfg.get("preset1_deg") az2 = cfg.get("preset2_deg") if not isinstance(az1, (int, float)) or not isinstance(az2, (int, float)): logger.warning( "[PATROL] cam %s: no preset azimuths -> fallback to smooth sweep", cam, ) cfg["sector_min_deg"] = None cfg["sector_max_deg"] = None continue span = abs(ang_diff_signed(az2, az1)) if span < 1.0: logger.warning( "[PATROL] cam %s: too small span (%.2f°) -> fallback to smooth sweep", cam, span, ) cfg["sector_min_deg"] = None cfg["sector_max_deg"] = None continue current_az = st.get("az_deg") if current_az is None: if st.get("az_none_since") is None: st["az_none_since"] = now if now - st["az_none_since"] >= az_none_fallback_sec: logger.warning( "[PATROL] cam %s: azimuth is None > %.1fs -> temporary unbounded sweep", cam, az_none_fallback_sec, ) cfg["sector_min_deg"] = None cfg["sector_max_deg"] = None continue st["az_none_since"] = None # Determine the "end" azimuth for this leg patrol_dir = st.get("patrol_dir", +1) end_az = normalize360(az2 if patrol_dir > 0 else az1) # If leg time is over — don't reverse until we actually reached the end if now >= float(st.get("leg_end", 0.0) or 0.0): err_to_end = abs(ang_diff_signed(end_az, float(current_az))) if err_to_end <= end_tol_deg: _dbg_state( cam, f"bounded: leg_end reached AND near end (err={err_to_end:.2f}°) -> reverse", ) patrol_finish_and_reverse(cam) continue # Extend the leg, but with a cap ext_total = float(st.get("leg_extend_total", 0.0) or 0.0) if ext_total < extend_max_sec: st["leg_end"] = now + extend_step_sec st["leg_extend_total"] = ext_total + extend_step_sec _dbg_state( cam, ( "bounded: leg_end but NOT at end " f"(err={err_to_end:.2f}°) -> extend +{extend_step_sec}s " f"(total={st['leg_extend_total']:.1f}s)" ), ) else: _dbg_state( cam, ( "bounded: extend limit reached " f"(err={err_to_end:.2f}°) -> reverse anyway" ), ) patrol_finish_and_reverse(cam) continue # Normal trajectory: target_az by smoothstep/ease elapsed = max(0.0, now - st["leg_start"]) progress = max( 0.0, min(1.0, elapsed / max(1e-6, config.PATROL_LEG_SEC)), ) ease = (3.0 * progress * progress) - (2.0 * progress * progress * progress) if patrol_dir > 0: target_az = az1 + ang_diff_signed(az2, az1) * ease else: target_az = az2 + ang_diff_signed(az1, az2) * ease target_az = normalize360(target_az) err_deg = ang_diff_signed(target_az, float(current_az)) if abs(err_deg) <= dead_band_deg: pan_speed = 0.0 else: pan_speed = scale * (err_deg / max(1e-6, gain_deg)) pan_speed = max(-max_speed, min(max_speed, pan_speed)) if abs(pan_speed) < min_step_cmd: pan_speed = 0.0 if pan_speed != 0.0: move_ptz_bounded(cam, pan_speed, 0.0, 0.0) _dbg_state( cam, ( f"bounded: move pan_speed={pan_speed:.3f} " f"err_deg={err_deg:.3f} target_az={target_az:.2f} end_az={end_az:.2f}" ), ) def patrol_supervisor_tick() -> None: now = time.time() for cam in config.PTZ_CAM_IDS: st = state.ptz_states[cam] cfg = config.CAMERA_CONFIG[cam] # While camera is doing goto — don't send continuous commands if now < float(st.get("goto_in_progress_until", 0.0) or 0.0): _dbg_skip(cam, "unbounded: goto_in_progress") continue if st.get("tracking_active") or st.get("patrol_paused"): _dbg_skip(cam, "unbounded: tracking_active or patrol_paused") continue # If bounded mode is active (sector is defined) — unbounded tick does nothing if cfg.get("sector_min_deg") is not None and cfg.get("sector_max_deg") is not None: continue if not ( config.SMOOTH_SWEEP_ENABLE and st.get("patrol_active") and st.get("sweep_active") ): _dbg_skip( cam, ( "sweep not active | " f"smooth={config.SMOOTH_SWEEP_ENABLE} " f"patrol={st.get('patrol_active')} " f"sweep={st.get('sweep_active')}" ), ) continue if st.get("patrol_active") and now >= st.get("leg_end", 0.0): _dbg_state(cam, "unbounded: leg_end reached -> reverse") patrol_finish_and_reverse(cam) continue if now < st.get("sweep_start_at", 0.0): continue if st.get("mode") not in ("IDLE", "SEARCH"): _dbg_skip(cam, f"unbounded: mode={st.get('mode')} not IDLE/SEARCH") continue elapsed = max(0.0, now - st["leg_start"]) remain = max(0.0, st["leg_end"] - now) r_up = min(1.0, elapsed / max(1e-3, config.SWEEP_RAMP_SEC)) r_dn = min(1.0, remain / max(1e-3, config.SWEEP_RAMP_SEC)) ramp = min(r_up, r_dn) pan = max(-1.0, min(1.0, st.get("sweep_pan", 0.0) * ramp)) if abs(pan) > 1e-3: move_ptz_bounded(cam, pan, 0.0, 0.0) _dbg_state(cam, f"unbounded: move pan={pan:.3f} ramp={ramp:.3f}") def patrol_init_on_startup() -> None: for cam in config.PTZ_ORDER: dir_sign = config.PATROL_START_DIR.get(cam, +1) cfg = config.CAMERA_CONFIG[cam] if cfg.get("sector_min_deg") is not None and cfg.get("sector_max_deg") is not None: patrol_start_leg_bounded(cam, dir_sign) else: patrol_start_leg(cam, dir_sign) start_token = cfg.get("preset1") if dir_sign >= 0 else cfg.get("preset2") dir_txt = "+1" if dir_sign >= 0 else "-1" logger.info( "[PATROL] cam %s leg %s: goto START %s", cam, dir_txt, start_token ) _dbg_state(cam, f"patrol_init_on_startup: dir_sign={dir_sign}") def patrol_resync_on_endpoint(cam: int, az: float) -> None: st = state.ptz_states[cam]; cfg = config.CAMERA_CONFIG[cam] # не трогаем ресинк, если трек или пауза патруля if st.get("tracking_active") or st.get("patrol_paused"): _dbg_state(cam, "patrol_resync_on_endpoint: skip (tracking_active or patrol_paused)") return # Инициализация защёлки по месту if "endpoint_latch" not in st: st["endpoint_latch"] = 0 # 0=вне пресетов, 1=P1, 2=P2 if not st.get("patrol_active"): st["endpoint_latch"] = 0 _dbg_state(cam, "patrol_resync_on_endpoint: patrol_active=False -> reset latch") return p1a, p2a = cfg.get("preset1_deg"), cfg.get("preset2_deg") if p1a is None or p2a is None: st["endpoint_latch"] = 0 _dbg_state(cam, "patrol_resync_on_endpoint: no preset azimuths -> reset latch") return now = time.time() pan_sign = float(cfg.get("pan_sign", 1) or 1) def _dir_sign(a_from: float, a_to: float) -> float: d = ang_diff_signed(a_to, a_from) return 1.0 if d >= 0.0 else -1.0 # Находимся у P1 if is_near(az, p1a): if st.get("endpoint_latch") == 1: return # уже защёлкнулись на P1 — не перезапускать ногу st["endpoint_latch"] = 1 st["patrol_dir"] = +1 st["leg_start"] = now st["leg_end"] = now + config.PATROL_LEG_SEC st["mode"] = "IDLE" if config.SMOOTH_SWEEP_ENABLE: base_dir = _dir_sign(p1a, p2a) st["sweep_active"] = True st["sweep_pan"] = pan_sign * config.SWEEP_PAN_SPEED * base_dir st["sweep_start_at"] = now + config.SWEEP_SETTLE_SEC else: st["sweep_active"] = False; st["sweep_pan"] = 0.0; st["sweep_start_at"] = 0.0 logger.info("[PATROL][RESYNC] cam %s at P1 → leg to P2 (dir=+1)", cam) _dbg_state(cam, "patrol_resync_on_endpoint: latch=P1, leg to P2") return # Находимся у P2 if is_near(az, p2a): if st.get("endpoint_latch") == 2: return # уже защёлкнулись на P2 st["endpoint_latch"] = 2 st["patrol_dir"] = -1 st["leg_start"] = now st["leg_end"] = now + config.PATROL_LEG_SEC st["mode"] = "IDLE" if config.SMOOTH_SWEEP_ENABLE: base_dir = _dir_sign(p2a, p1a) st["sweep_active"] = True st["sweep_pan"] = pan_sign * config.SWEEP_PAN_SPEED * base_dir st["sweep_start_at"] = now + config.SWEEP_SETTLE_SEC else: st["sweep_active"] = False; st["sweep_pan"] = 0.0; st["sweep_start_at"] = 0.0 logger.info("[PATROL][RESYNC] cam %s at P2 → leg to P1 (dir=-1)", cam) _dbg_state(cam, "patrol_resync_on_endpoint: latch=P2, leg to P1") return # Ушли с пресета — снимаем защёлку st["endpoint_latch"] = 0 _dbg_state(cam, "patrol_resync_on_endpoint: left presets -> latch=0") # ===== SYNC patrol ===== def is_in_sync_group(cam: int) -> Optional[str]: return config.sync_group_by_cam.get(cam) def cohort_dir_for_cam(cam: int, group_dir: int) -> int: sign = config.cohort_sign_by_cam.get(cam, +1) return +1 if sign * int(group_dir) >= 0 else -1 def _start_token_for_dir(cfg: dict, dir_sign: int) -> Optional[str]: p1, p2 = cfg.get("preset1"), cfg.get("preset2") if not (p1 and p2): return None return p1 if dir_sign >= 0 else p2 def sync_mark_wait(cam: int) -> None: st = state.ptz_states[cam] st["patrol_active"] = False st["sweep_active"] = False st["sweep_pan"] = 0.0 st["mode"] = "IDLE" st["sync_wait"] = True notify_detected(cam, False, force=True) _dbg_state(cam, "sync_mark_wait: waiting for cohort leg") def sync_park_to_start(cam: int, next_dir: int) -> None: st = state.ptz_states[cam] # в синхро-режиме тоже не дёргать PTZ, если трек/пауза if st.get("tracking_active") or st.get("patrol_paused"): _dbg_state(cam, "sync_park_to_start: blocked (tracking_active or patrol_paused)") return cfg = config.CAMERA_CONFIG[cam] tok = _start_token_for_dir(cfg, next_dir) stop_ptz(cam) if tok: goto_preset_token(cam, tok) sync_mark_wait(cam) _dbg_state(cam, f"sync_park_to_start: parked to start preset={tok}, next_dir={next_dir}") def sync_activate_leg(cam: int, dir_sign: int, start_at: float, end_at: float) -> None: st = state.ptz_states[cam] # не активировать «ногу», если трек/пауза if st.get("tracking_active") or st.get("patrol_paused"): _dbg_state(cam, "sync_activate_leg: blocked (tracking_active or patrol_paused)") return st["patrol_dir"] = 1 if dir_sign >= 0 else -1 st["patrol_active"] = True st["leg_start"] = start_at st["leg_end"] = end_at st["sweep_active"] = False st["sweep_pan"] = 0.0 st["sweep_start_at"] = 0.0 st["sync_wait"] = False _dbg_state(cam, f"sync_activate_leg: dir_sign={dir_sign}, leg=[{start_at}..{end_at}]") async def sync_patrol_coordinator_loop(): while True: t_now = time.time() try: for gname, coh in (config.SYNC_COHORTS or {}).items(): stg = state.sync_state[gname] if t_now < stg.get("next_start_at", 0.0): continue group_dir = stg["dir"] next_start = t_now + config.SYNC_BARRIER_SEC for cam in (coh.get("plus", []) + coh.get("minus", [])): st = state.ptz_states[cam] if st.get("mode") == "TRACK": _dbg_skip(cam, f"sync: skip park, mode=TRACK (group={gname})") continue # не парковать, если трек/пауза if st.get("tracking_active") or st.get("patrol_paused"): _dbg_skip(cam, f"sync: skip park (tracking_active or patrol_paused), group={gname}") continue want_dir = cohort_dir_for_cam(cam, group_dir) sync_park_to_start(cam, want_dir) await asyncio.sleep(max(0.0, next_start - time.time())) leg_start = time.time() leg_end = leg_start + config.PATROL_LEG_SEC for cam in (coh.get("plus", []) + coh.get("minus", [])): st = state.ptz_states[cam] if st.get("mode") == "TRACK": _dbg_skip(cam, f"sync: skip leg activate, mode=TRACK (group={gname})") continue # не активировать, если трек/пауза if st.get("tracking_active") or st.get("patrol_paused"): _dbg_skip(cam, f"sync: skip leg activate (tracking_active or patrol_paused), group={gname}") continue want_dir = cohort_dir_for_cam(cam, group_dir) sync_activate_leg(cam, want_dir, leg_start, leg_end) dwell = max(0.0, float(config.SYNC_DWELL_SEC)) stg["leg_idx"] += 1 stg["leg_start"] = leg_start stg["leg_end"] = leg_end stg["next_start_at"] = leg_end + dwell stg["dir"] = -group_dir await asyncio.sleep(0.05) except Exception as e: logger.error("[SYNC] coordinator error: %s", e) await asyncio.sleep(0.2) async def track_return_watchdog(cam_idx: int) -> None: """ Watchdog: - TRACK: следит за выходом за сектор и лимитом времени трекинга - IDLE/SEARCH: * после окончания TRACK: один раз паркует в пресет (post-track park once) и возобновляет патруль * если патруль активен: следит за "вылетом" из сектора и плохим tilt/zoom → паркует в пресет и возобновляет * если патруль НЕ активен: возвращает к пресетам, если камера долго вне пресетов """ import time from . import state, config from .patrol import ensure_patrol_resume from .sector import sector_contains # если у тебя это в другом модуле — поправь импорт # _near_endpoint и return_to_preset должны быть доступны в текущем модуле/области # (как у тебя в версии выше) st = state.ptz_states[cam_idx] cfg = config.CAMERA_CONFIG.get(cam_idx, {}) # ====== TRACK (во время трекинга) ====== TRACK_OFFSECTOR_GRACE_SEC = float(getattr(config, "TRACK_OFFSECTOR_GRACE_SEC", 3.0)) TRACK_MAX_UNINTERRUPTED_SEC = float(getattr(config, "TRACK_MAX_UNINTERRUPTED_SEC", 30.0)) # ====== POST-TRACK (после потери цели) ====== POST_TRACK_RETURN_SEC = float(getattr(config, "POST_TRACK_RETURN_SEC", 1.4)) # ждать после потери цели перед парковкой POST_TRACK_GUARD_SEC = float(getattr(config, "POST_TRACK_GUARD_SEC", 4.0)) # сколько времени блокируем повторные "парковки" после post-track park # ====== IDLE/SEARCH (без трекинга) ====== IDLE_OFFPRESET_GRACE_SEC = float(getattr(config, "IDLE_OFFPRESET_GRACE_SEC", 7.0)) # если патруль активен и камера "вылетела" из сектора — сколько ждать до парковки PATROL_OFFSECTOR_RETURN_SEC = float(getattr(config, "PATROL_OFFSECTOR_RETURN_SEC", 6.0)) # tilt-контроль в патруле PATROL_TILT_TOL_DEG = float(getattr(config, "PATROL_TILT_TOL_DEG", 4.0)) PATROL_BAD_TILT_GRACE_SEC = float(getattr(config, "PATROL_BAD_TILT_GRACE_SEC", 2.5)) # zoom-сброс в патруле (если нет детекций) PATROL_ZOOM_RESET_SEC = float(getattr(config, "PATROL_ZOOM_RESET_SEC", 9.0)) PATROL_RECOVER_AFTER_PARK_SEC = float(getattr(config, "PATROL_RECOVER_AFTER_PARK_SEC", 1.0)) # защита от спама парковкой PARK_COOLDOWN_SEC = float(getattr(config, "WATCHDOG_PARK_COOLDOWN_SEC", 6.0)) # короткая пауза после парковки, чтобы статус успел обновиться (иначе можно триггерить повторно на старых данных) PARK_IGNORE_SEC = float(getattr(config, "WATCHDOG_PARK_IGNORE_SEC", 1.6)) # ===== helpers ===== def _norm_tilt_cfg(v: float) -> float: """ Нормализация tilt из cameras.toml/конфига. Часто tilt хранится в десятых/сотых градуса, а status отдаёт градусы. """ av = abs(v) if 900 < av <= 90000: return v / 100.0 if 90 < av <= 9000: return v / 10.0 return v def _get_tilt_deg() -> float | None: v = st.get("tilt_deg") if isinstance(v, (int, float)): return float(v) v = st.get("tilt") if isinstance(v, (int, float)): return float(v) return None def _expected_patrol_tilt_deg() -> float | None: """ Возвращает ожидаемый tilt патруля в градусах или None если данных нет/мусор. Жёстко режем мусор вроде -138, -150 и т.п., чтобы не ломать патруль. """ # 1) явный patrol_tilt_deg v = cfg.get("patrol_tilt_deg") if isinstance(v, (int, float)): vv = _norm_tilt_cfg(float(v)) if abs(vv) <= 90.0: return vv return None # 2) fallback: tilt пресетов for key in ("preset1_tilt_deg", "preset2_tilt_deg"): v = cfg.get(key) if isinstance(v, (int, float)): vv = _norm_tilt_cfg(float(v)) if abs(vv) <= 90.0: return vv return None def _in_sector_safe(az: float | None) -> bool | None: smin = cfg.get("sector_min_deg") smax = cfg.get("sector_max_deg") if not (isinstance(az, (int, float)) and isinstance(smin, (int, float)) and isinstance(smax, (int, float))): return None return sector_contains(float(az), float(smin), float(smax)) def _reset_accumulators(): st.pop("offsector_since", None) st.pop("offpreset_since", None) st.pop("patrol_offsector_since", None) st.pop("patrol_bad_tilt_since", None) st.pop("patrol_zoom_bad_since", None) def _throttled_park(reason: str) -> bool: """ Единая точка парковки: уважает goto_in_progress + cooldown + ignore window. Возвращает True если реально начали парковку. """ now = time.time() # не мешаем выполнению goto if now < float(st.get("goto_in_progress_until", 0.0) or 0.0): return False # "тихий период" после предыдущей парковки — даём статусу обновиться if now < float(st.get("watchdog_ignore_until", 0.0) or 0.0): return False last = float(st.get("watchdog_last_park_at", 0.0) or 0.0) if (now - last) < PARK_COOLDOWN_SEC: return False st["watchdog_last_park_at"] = now st["watchdog_ignore_until"] = now + PARK_IGNORE_SEC # сбросить накопители, чтобы сразу после goto не сработало снова _reset_accumulators() # важное: подготовить патруль к нормальному старту после парковки st["mode"] = "IDLE" st["endpoint_latch"] = 0 st["leg_extend_total"] = 0.0 return_to_preset(cam_idx, reason=reason) ensure_patrol_resume(cam_idx, delay=PATROL_RECOVER_AFTER_PARK_SEC) return True # ===== loop ===== while True: await asyncio.sleep(0.2) now = time.time() # пока едем на goto — ничего не трогаем if now < float(st.get("goto_in_progress_until", 0.0) or 0.0): _reset_accumulators() # post-track таймер не трогаем, но "парковать" не будем пока goto идёт continue # если в ignore window — тоже не дёргаем (статус ещё может быть старый) if now < float(st.get("watchdog_ignore_until", 0.0) or 0.0): _reset_accumulators() continue az = st.get("az_deg") mode = st.get("mode") # ---- фикс: детектируем переход TRACK -> не TRACK ---- last_mode = st.get("last_mode") if last_mode == "TRACK" and mode != "TRACK": st["post_track_since"] = now st["post_track_park_done"] = False st["last_mode"] = mode # ====== TRACK ====== if mode == "TRACK": if "track_start_at" not in st: st["track_start_at"] = now # во время трека не запускаем post-track паркинг st.pop("post_track_since", None) st["post_track_park_done"] = False st.pop("post_track_guard_until", None) # контроль выхода из сектора во время TRACK if isinstance(az, (int, float)): in_sector = _in_sector_safe(float(az)) near_endpoint = _near_endpoint(az, cfg, tol=1.0) if in_sector is False and not near_endpoint: if "offsector_since" not in st: st["offsector_since"] = now elif now - st["offsector_since"] > TRACK_OFFSECTOR_GRACE_SEC: st.pop("offsector_since", None) _throttled_park("track_offsector_timeout") continue else: st.pop("offsector_since", None) # лимит непрерывного трекинга if now - float(st.get("track_start_at", now)) > TRACK_MAX_UNINTERRUPTED_SEC: st.pop("track_start_at", None) _throttled_park("track_max_duration") continue else: st.pop("track_start_at", None) st.pop("offsector_since", None) # ====== IDLE / SEARCH ====== if mode in ("IDLE", "SEARCH"): patrol_active = bool(st.get("patrol_active")) # ---- (0) Post-track park ONCE ---- pts = st.get("post_track_since") guard_until = float(st.get("post_track_guard_until", 0.0) or 0.0) if isinstance(pts, (int, float)) and not st.get("post_track_park_done", False): # не даём другим правилам сразу после трека дёргать парковку много раз if now < guard_until: _reset_accumulators() continue if (now - float(pts)) >= POST_TRACK_RETURN_SEC: started = _throttled_park("track_end_park") if started: st["post_track_park_done"] = True st["post_track_guard_until"] = now + POST_TRACK_GUARD_SEC continue # ---- 1) если патруль активен: самовосстановление по сектору/tilt/zoom ---- if patrol_active: # (A) если камера реально вылетела из сектора — вернуть in_sector = _in_sector_safe(float(az)) if isinstance(az, (int, float)) else None near_endpoint = _near_endpoint(az, cfg, tol=1.0) if in_sector is False and not near_endpoint: if "patrol_offsector_since" not in st: st["patrol_offsector_since"] = now elif now - st["patrol_offsector_since"] >= PATROL_OFFSECTOR_RETURN_SEC: st.pop("patrol_offsector_since", None) _throttled_park("patrol_offsector") continue else: st.pop("patrol_offsector_since", None) # (B) tilt “застрял” (после трека/ручного) exp_tilt = _expected_patrol_tilt_deg() cur_tilt = _get_tilt_deg() # если exp_tilt мусор — просто не проверяем tilt if (exp_tilt is not None) and (cur_tilt is not None): bad_tilt = abs(float(cur_tilt) - float(exp_tilt)) > PATROL_TILT_TOL_DEG if bad_tilt: if "patrol_bad_tilt_since" not in st: st["patrol_bad_tilt_since"] = now elif now - st["patrol_bad_tilt_since"] >= PATROL_BAD_TILT_GRACE_SEC: st.pop("patrol_bad_tilt_since", None) _throttled_park("patrol_bad_tilt") continue else: st.pop("patrol_bad_tilt_since", None) else: st.pop("patrol_bad_tilt_since", None) # (C) zoom “застрял”: если давно нет цели, а zoom_state/lock не нулевые — парк в пресет last_seen = float(st.get("last_seen", 0.0) or 0.0) no_target_for = (now - last_seen) if last_seen > 0 else 1e9 zoom_stuck = ( bool(st.get("zoom_lock", False)) or (abs(float(st.get("zoom_prev_cmd", 0.0) or 0.0)) > 1e-3) or (int(st.get("zoom_state", 0) or 0) != 0) or (abs(float(st.get("zoom_int", 0.0) or 0.0)) > 0.02) ) if (no_target_for >= PATROL_ZOOM_RESET_SEC) and zoom_stuck: if "patrol_zoom_bad_since" not in st: st["patrol_zoom_bad_since"] = now elif now - st["patrol_zoom_bad_since"] >= 0.8: st.pop("patrol_zoom_bad_since", None) _throttled_park("patrol_zoom_reset") continue else: st.pop("patrol_zoom_bad_since", None) # при активном патруле offpreset-таймер не нужен st.pop("offpreset_since", None) continue # ---- 2) если патруля нет: классическое “idle away from presets too long” ---- near_preset = _near_endpoint(az, cfg, tol=1.0) if near_preset: st.pop("offpreset_since", None) else: if "offpreset_since" not in st: st["offpreset_since"] = now elif now - st["offpreset_since"] > IDLE_OFFPRESET_GRACE_SEC: st.pop("offpreset_since", None) _throttled_park("idle_offpreset_timeout") continue else: st.pop("offpreset_since", None) st.pop("patrol_offsector_since", None) st.pop("patrol_bad_tilt_since", None) st.pop("patrol_zoom_bad_since", None)