You cannot select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
Chameleon/integration/ptz_field_sampling/status.patch

130 lines
6.3 KiB
Diff

--- a/status.py
+++ b/status.py
@@ -14,7 +14,7 @@
# ---------------- Публичные утилиты статуса ----------------
-from .zoom_controller import observe_calibration
+from .zoom_controller import _episode_telemetry, _owner, observe_calibration
def get_cached_angles(cam_id: int) -> Tuple[Optional[float], Optional[float], Optional[float]]:
@@ -97,6 +97,13 @@
rate_alpha = clipf(float(getattr(config, "PTZ_RATE_EMA_ALPHA", 0.40)), 0.02, 1.0)
+ pan_alpha = tilt_alpha = rate_alpha
+ if _track_status_period(cfg, st) is not None:
+ # Preserve the previous time constant at the faster TRACK cadence.
+ baseline_hz = max(0.1, float(getattr(config, "PTZ_STATUS_HZ", 4.0)))
+ pan_alpha = 1 - (1 - rate_alpha) ** (max(0.0, pan_sample_dt) * baseline_hz)
+ tilt_alpha = 1 - (1 - rate_alpha) ** (max(0.0, tilt_sample_dt) * baseline_hz)
+
if pan is not None:
pan_n = float(pan) % 360.0
if isinstance(previous_pan, (int, float)) and 0.03 <= pan_sample_dt <= 2.0:
@@ -104,7 +111,7 @@
pan_limit = float(cfg.get("ptz_pan_max_speed_deg_s", getattr(config, "AIM_PAN_SPEED_DEG_S", 160.0)))
if abs(raw_rate) <= max(1.0, pan_limit * 1.35):
old_rate = float(st.get("pan_rate_deg_s", 0.0) or 0.0)
- st["pan_rate_deg_s"] = (1.0 - rate_alpha) * old_rate + rate_alpha * raw_rate
+ st["pan_rate_deg_s"] = (1.0 - pan_alpha) * old_rate + pan_alpha * raw_rate
pan_rate_updated = True
st["az_deg"] = pan_n # <-- это читает ptz_io._ptz_worker при bounded-патруле
st["pan_deg"] = pan_n # удобный синоним
@@ -126,7 +133,7 @@
tilt_limit = float(cfg.get("ptz_tilt_max_speed_deg_s", getattr(config, "AIM_TILT_SPEED_DEG_S", 120.0)))
if abs(raw_rate) <= max(1.0, tilt_limit * 1.35):
old_rate = float(st.get("tilt_rate_deg_s", 0.0) or 0.0)
- st["tilt_rate_deg_s"] = (1.0 - rate_alpha) * old_rate + rate_alpha * raw_rate
+ st["tilt_rate_deg_s"] = (1.0 - tilt_alpha) * old_rate + tilt_alpha * raw_rate
tilt_rate_updated = True
st["tilt_deg"] = tilt_n
@@ -196,6 +203,14 @@
st["last_status_attempt_at"] = now
+def _track_status_period(camera_config, st):
+ rate = camera_config.get("ptz_status_track_hz")
+ if (type(rate) in (float, int) and 10 <= rate <= 15
+ and st.get("mode") == "TRACK" and st.get("tracking_active")):
+ return 1.0 / rate
+ return None
+
+
# -------------------- Основной поллер статуса ---------------------
async def _poll_camera_status_once(
cam_id: int, camera_config: dict, log_period: float,
@@ -204,6 +219,8 @@
"""Publish one result only while its camera configuration is still current."""
started_mono = time.monotonic()
config_snapshot = camera_config.copy()
+ st = state.ptz_states.get(cam_id, {})
+ sample_owner = _owner(st) if _track_status_period(camera_config, st) is not None else None
try:
full = await asyncio.to_thread(get_ptz_status_full, cam_id)
pan, tilt = full.pan, full.tilt
@@ -248,6 +265,17 @@
zoom_ratio=zoom_ratio, hfov_deg=hfov_deg, zoom_raw=zoom_raw,
zoom_tag=zoom_tag, zoom_age_sec=zoom_age_sec,
)
+ st = state.ptz_states.get(cam_id, {})
+ if (sample_owner is not None and sample_owner[1] > 0 and _owner(st) == sample_owner
+ and _track_status_period(camera_config, st) is not None):
+ _episode_telemetry(
+ config, cam_id, sample_owner, "status_sample", observed_at,
+ request_ms=request_ms, status_ok=(pan is not None or tilt is not None or hfov_deg is not None),
+ FOV_actual=hfov_deg,
+ FOV_status_utc=observed_at - zoom_age_sec
+ if hfov_deg is not None and zoom_age_sec is not None else None,
+ optics_fresh=hfov_deg is not None and zoom_age_sec == 0.0,
+ )
if log_period > 0 and _should_log_telemetry(cam_id, observed_at, log_period):
st = state.ptz_states.get(cam_id, {})
logger.info(
@@ -280,7 +308,7 @@
)
loop = asyncio.get_running_loop()
in_flight: dict[int, asyncio.Task] = {}
- next_due: dict[int, float] = {}
+ last_start: dict[int, float] = {}
session = {"active": True}
cancelling = False
try:
@@ -297,22 +325,29 @@
logger.exception("[STATUS] cam %d publication failed", cam_id)
# Do not cancel removed cameras' to_thread work and immediately
# launch another request for a reused ID. Retire it when done.
- for cam_id in list(next_due):
+ for cam_id in list(last_start):
if cam_id not in cam_ids and cam_id not in in_flight:
- del next_due[cam_id]
+ del last_start[cam_id]
now_mono = loop.time()
+ delay = min(0.05, interval)
for cam_id in sorted(cam_ids):
cfg = config.CAMERA_CONFIG.get(cam_id)
- if cfg is None or cam_id in in_flight or now_mono < next_due.get(cam_id, 0.0):
+ if cfg is None:
continue
- next_due[cam_id] = now_mono + interval
+ track_period = _track_status_period(cfg, state.ptz_states.get(cam_id, {}))
+ period = track_period if track_period is not None else interval
+ if track_period is not None:
+ delay = min(delay, period / 8)
+ if cam_id in in_flight or now_mono < last_start.get(cam_id, -float("inf")) + period:
+ continue
+ last_start[cam_id] = now_mono
in_flight[cam_id] = asyncio.create_task(
_poll_camera_status_once(cam_id, cfg, log_period, session),
name=f"ptz-status-{cam_id}",
)
# Bounded supervisor tick also notices camera add/delete and shutdown.
# Individual tasks publish as soon as ready, not at this tick.
- await asyncio.sleep(min(0.05, interval))
+ await asyncio.sleep(delay)
except asyncio.CancelledError:
logger.info("[STATUS] PTZ status poller cancelled")
cancelling = True