|
|
"""Predictive TRACK zoom policy. All time inputs are seconds on the same clock."""
|
|
|
|
|
|
from collections import deque
|
|
|
from dataclasses import dataclass, replace
|
|
|
import json
|
|
|
import math
|
|
|
from pathlib import Path
|
|
|
from queue import Full, Queue
|
|
|
from threading import Lock, Thread
|
|
|
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
|
class ZoomConfig:
|
|
|
# Fit per camera and native command. Gains multiply ln(252/6)/full_travel_s.
|
|
|
nominal_gain: float
|
|
|
upper_gain: float
|
|
|
wide_fov: float = 59.0
|
|
|
tele_fov: float = 2.0
|
|
|
full_travel_s: float = 5.5
|
|
|
native: int = 10
|
|
|
entry_fov: float = 35.0
|
|
|
initial_gate_relax_factor: float = 1.45
|
|
|
tele_edge_margin_ratio: float = 0.32
|
|
|
min_allowed_hfov_deg: float = 10.0
|
|
|
latency_horizon_ms: float = 245.0
|
|
|
tele_pan_native10_deg_s: float = 0.65
|
|
|
tele_tilt_native10_deg_s: float = 0.85
|
|
|
tele_position_gain: float = 2.7
|
|
|
tele_rate_gain: float = 0.85
|
|
|
tele_native_max: float = 35.0
|
|
|
tele_steering_start_fov: float = 20.0
|
|
|
tele_full_steering_fov: float = 16.0
|
|
|
step_back_cmd: float = 0.10
|
|
|
tele_calibration_enabled: bool = True
|
|
|
calibration_settle_s: float = .35
|
|
|
calibration_timeout_s: float = 1.2
|
|
|
calibration_stable_deg: float = .10
|
|
|
entry_center: float = 0.15 # fraction of full frame, 30% of half-width
|
|
|
repeat_center: float = 0.09
|
|
|
edge_margin: float = 0.16
|
|
|
tracker_score: float = 0.55
|
|
|
tracker_frames: int = 3
|
|
|
frame_age_s: float = 0.25
|
|
|
detector_age_s: float = 0.65
|
|
|
status_age_s: float = 0.65
|
|
|
evidence_gap_s: float = 0.20
|
|
|
max_outward_deg_s: float = 0.15
|
|
|
trajectory_slack_deg: float = 0.6
|
|
|
edge_accel: float = 0.55 # full frame / sВІ, outward
|
|
|
edge_watch_margin: float = 0.24
|
|
|
steering_delay_s: float = 0.18
|
|
|
max_residual_deg_s: float = 12.0
|
|
|
wide_pulse_fov: float = 25.0
|
|
|
tele_pulse_fov: float = 15.0
|
|
|
wide_pulse_s: float = 0.34
|
|
|
tele_pulse_s: float = 0.12
|
|
|
stop_slack_s: float = 0.06
|
|
|
stop_ack_slack_s: float = 0.08
|
|
|
target_px: float = 48.0
|
|
|
status_gain: float = 0.7
|
|
|
status_error_log: float = 0.08
|
|
|
ff_gain: float = 0.7
|
|
|
ff_max_deg_s: float = 3.0
|
|
|
|
|
|
def __post_init__(self):
|
|
|
for name, value in vars(self).items():
|
|
|
if name == "tele_calibration_enabled" and isinstance(value, bool):
|
|
|
continue
|
|
|
if not math.isfinite(value) or value <= 0:
|
|
|
raise ValueError(f"invalid zoom setting {name}")
|
|
|
if not (
|
|
|
self.native == 10 and 0 < self.nominal_gain <= self.upper_gain
|
|
|
and 0 < self.tele_fov < self.tele_pulse_fov < self.wide_pulse_fov
|
|
|
< self.entry_fov < self.wide_fov < 90
|
|
|
and 0 < self.repeat_center <= self.entry_center <= 0.15
|
|
|
and 0 < self.edge_margin <= self.edge_watch_margin < 0.5
|
|
|
and isinstance(self.tracker_frames, int) and 2 <= self.tracker_frames <= 12
|
|
|
and self.tracker_score <= 1 and self.status_gain <= 1
|
|
|
and self.frame_age_s <= 0.25 and self.detector_age_s <= 0.65
|
|
|
and self.status_age_s <= 0.65
|
|
|
and 1 <= self.initial_gate_relax_factor <= 1.8
|
|
|
and self.edge_margin <= self.tele_edge_margin_ratio <= .38
|
|
|
and self.tele_fov <= self.min_allowed_hfov_deg <= self.tele_pulse_fov
|
|
|
and self.wide_pulse_fov > 12
|
|
|
and 40 <= self.latency_horizon_ms <= 400
|
|
|
and 0 < self.tele_full_steering_fov < self.tele_steering_start_fov <= self.wide_pulse_fov
|
|
|
and self.tele_native_max <= 45 and self.tele_rate_gain <= 1
|
|
|
and self.step_back_cmd <= .12
|
|
|
and isinstance(self.tele_calibration_enabled, bool)
|
|
|
and 0.25 <= self.calibration_settle_s < self.calibration_timeout_s <= 2
|
|
|
and self.calibration_stable_deg <= .25
|
|
|
and 0.10 <= self.tele_pulse_s <= self.wide_pulse_s <= 0.34
|
|
|
and self.wide_pulse_s + self.stop_slack_s <= 0.40 + 1e-9
|
|
|
and self.stop_ack_slack_s <= 0.12
|
|
|
):
|
|
|
raise ValueError("zoom settings exceed the native/geometry/watchdog limits")
|
|
|
|
|
|
@property
|
|
|
def log_rate(self):
|
|
|
return self.nominal_gain * math.log(252 / 6) / self.full_travel_s
|
|
|
|
|
|
@property
|
|
|
def upper_log_rate(self):
|
|
|
return self.upper_gain * math.log(252 / 6) / self.full_travel_s
|
|
|
|
|
|
def margin_for(self, fov):
|
|
|
weight = min(1.0, max(0.0, (self.wide_pulse_fov - fov) / (self.wide_pulse_fov - 12.0)))
|
|
|
return self.edge_margin + weight * (self.tele_edge_margin_ratio - self.edge_margin)
|
|
|
|
|
|
def limits_window(self, fov, seconds):
|
|
|
next_focal = _log_focal(fov) + self.upper_log_rate * seconds
|
|
|
return next_focal <= _log_focal(self.min_allowed_hfov_deg)
|
|
|
|
|
|
def window(self, fov):
|
|
|
scale = min(1.0, max(0.0, (fov - self.tele_pulse_fov)
|
|
|
/ (self.wide_pulse_fov - self.tele_pulse_fov)))
|
|
|
nominal = self.tele_pulse_s + scale * (self.wide_pulse_s - self.tele_pulse_s)
|
|
|
return nominal, min(0.40, nominal + self.stop_slack_s)
|
|
|
|
|
|
|
|
|
def _log_focal(fov):
|
|
|
return -math.log(math.tan(math.radians(fov / 2)))
|
|
|
|
|
|
|
|
|
def _fov(log_focal):
|
|
|
return math.degrees(2 * math.atan(math.exp(-log_focal)))
|
|
|
|
|
|
|
|
|
class OpticalStateEstimator:
|
|
|
"""Integrate accepted native-10 windows, then correct with timestamped status."""
|
|
|
|
|
|
def __init__(self, config):
|
|
|
self.cfg = config
|
|
|
self.actual = None
|
|
|
self.status_at = 0.0
|
|
|
self.nominal = self.upper = 0.0
|
|
|
self.spans = deque()
|
|
|
self.samples = deque()
|
|
|
self.active = None
|
|
|
self.valid = False
|
|
|
|
|
|
def _seconds_since_status(self, at, status_at=None):
|
|
|
status_at = self.status_at if status_at is None else status_at
|
|
|
spans = self.spans
|
|
|
if self.active:
|
|
|
start, hard_end = self.active
|
|
|
spans = (*spans, (start, hard_end))
|
|
|
return sum(max(0.0, min(end, at) - max(start, status_at))
|
|
|
for start, end in spans)
|
|
|
|
|
|
def estimate(self, at):
|
|
|
for status_at, nominal, upper, _ in reversed(self.samples):
|
|
|
if status_at <= at:
|
|
|
elapsed = self._seconds_since_status(at, status_at)
|
|
|
tele = _log_focal(self.cfg.tele_fov)
|
|
|
return (_fov(min(tele, nominal + self.cfg.log_rate * elapsed)),
|
|
|
_fov(min(tele, upper + self.cfg.upper_log_rate * elapsed)))
|
|
|
return None, None
|
|
|
|
|
|
def observe(self, at, hfov, now):
|
|
|
if not (self.status_at < at <= now and math.isfinite(hfov)
|
|
|
and self.cfg.tele_fov - 0.1 <= hfov <= self.cfg.wide_fov + 1.0):
|
|
|
return
|
|
|
measured = _log_focal(hfov)
|
|
|
estimated, upper = self.estimate(at)
|
|
|
had_motion = self._seconds_since_status(at) > 0.0
|
|
|
discrepancy = (had_motion and estimated is not None
|
|
|
and (measured > _log_focal(upper) + self.cfg.status_error_log
|
|
|
or measured < _log_focal(self.actual) - self.cfg.status_error_log))
|
|
|
if discrepancy:
|
|
|
self.valid = False
|
|
|
elif not self.active:
|
|
|
self.valid = True
|
|
|
if not had_motion or estimated is None or discrepancy:
|
|
|
self.nominal = measured
|
|
|
else:
|
|
|
prediction = _log_focal(estimated)
|
|
|
self.nominal = prediction + self.cfg.status_gain * (measured - prediction)
|
|
|
# The filtered estimate never reduces the conservative edge envelope.
|
|
|
self.upper = max(self.nominal, measured + self.cfg.status_error_log)
|
|
|
self.actual, self.status_at = hfov, at
|
|
|
self.samples.append((at, self.nominal, self.upper, measured))
|
|
|
cutoff = at - self.cfg.status_age_s - self.cfg.frame_age_s
|
|
|
while len(self.samples) > 1 and self.samples[1][0] <= cutoff:
|
|
|
self.samples.popleft()
|
|
|
while self.spans and self.spans[0][1] <= self.samples[0][0]:
|
|
|
self.spans.popleft()
|
|
|
|
|
|
def start(self, put_at, ack_at, hard_end):
|
|
|
if self.active or not self.fresh(ack_at) or not put_at <= ack_at < hard_end:
|
|
|
self.valid = False
|
|
|
self.active = (put_at, hard_end)
|
|
|
|
|
|
def stop(self, at, *, clean):
|
|
|
if self.active:
|
|
|
start, hard_end = self.active
|
|
|
if at > hard_end + self.cfg.stop_ack_slack_s or at < start or not clean:
|
|
|
self.valid = False
|
|
|
if at > start:
|
|
|
self.spans.append((start, min(at, hard_end + self.cfg.stop_ack_slack_s)))
|
|
|
self.active = None
|
|
|
|
|
|
def fresh(self, now):
|
|
|
return self.valid and 0 <= now - self.status_at <= self.cfg.status_age_s
|
|
|
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
|
class ZoomFrame:
|
|
|
at: float
|
|
|
seq: int
|
|
|
offset: tuple[float, float] # positive right/down; fractions of full frame
|
|
|
bbox_frac: tuple[float, float]
|
|
|
bbox_px: tuple[float, float]
|
|
|
angles: tuple[float, float] # camera-signed LOS error in degrees
|
|
|
rates: tuple[float, float] # camera-signed LOS residual, deg/s
|
|
|
aspect: float # image height / width
|
|
|
tracker_score: float | None
|
|
|
detector_at: float
|
|
|
detector_conf: float | None
|
|
|
geometry_ok: bool
|
|
|
los_valid: bool
|
|
|
rearm_ready: bool = True
|
|
|
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
|
class ZoomDecision:
|
|
|
reason: str
|
|
|
window: tuple[float, float] | None = None
|
|
|
stop: bool = False
|
|
|
ff_deg_s: tuple[float, float] = (0.0, 0.0)
|
|
|
margin: float | None = None
|
|
|
step_back: bool = False
|
|
|
gate: str = "tracking_zoom"
|
|
|
edge_margin_required: float | None = None
|
|
|
|
|
|
|
|
|
class ZoomController:
|
|
|
"""Per TRACK epoch; the existing worker retains ownership and all STOPs."""
|
|
|
|
|
|
def __init__(self, config, owner):
|
|
|
self.cfg = config
|
|
|
self.owner = owner
|
|
|
self.optics = OpticalStateEstimator(config)
|
|
|
self.history = deque(maxlen=config.tracker_frames)
|
|
|
self.request = None
|
|
|
self.first_ack_at = None
|
|
|
self.confirmed_stop = None
|
|
|
self.completed_request = None
|
|
|
self.stop_cooldowns = None
|
|
|
self.latest_pose = None
|
|
|
self.calibration = None
|
|
|
|
|
|
def accepted(self, request, put_at, ack_at, hard_end):
|
|
|
if self.request == request:
|
|
|
return
|
|
|
self.request = request
|
|
|
self.confirmed_stop = None
|
|
|
self.completed_request = None
|
|
|
self.stop_cooldowns = None
|
|
|
if self.first_ack_at is None:
|
|
|
self.first_ack_at = ack_at
|
|
|
self.optics.start(put_at, ack_at, hard_end)
|
|
|
|
|
|
def stopped(self, request, at, *, confirmed, clean):
|
|
|
if self.request != request:
|
|
|
return
|
|
|
self.optics.stop(at, clean=clean)
|
|
|
self.request = None
|
|
|
if clean and self.optics.fresh(at):
|
|
|
self.completed_request = request
|
|
|
if confirmed:
|
|
|
self.confirmed_stop = (at, self.optics.status_at)
|
|
|
|
|
|
def optical_confirmation(self, request, at):
|
|
|
if self.completed_request == request and self.request is None and self.optics.fresh(at):
|
|
|
self.confirmed_stop = (at, self.optics.status_at)
|
|
|
|
|
|
def can_reuse_feedback(self, now, feedback_after):
|
|
|
return (self.confirmed_stop is not None and self.request is None
|
|
|
and self.confirmed_stop[0] == feedback_after and self.optics.fresh(now)
|
|
|
and self.optics.status_at >= self.confirmed_stop[1])
|
|
|
|
|
|
def owns(self, st):
|
|
|
return (st.get("predictive_zoom_controller") is self and _owner(st) == self.owner
|
|
|
and st.get("mode") == "TRACK" and st.get("tracking_active"))
|
|
|
|
|
|
def repeat_after_stop(self, st, now):
|
|
|
return bool(
|
|
|
self.owns(st) and self.can_reuse_feedback(
|
|
|
now, float(st.get("zoom_feedback_required_after", 0.0) or 0.0)
|
|
|
)
|
|
|
and st.get("zoom_step_last_delivery_result") == "confirmed"
|
|
|
and not any(st.get(key) for key in (
|
|
|
"zoom_step_feedback_pending", "zoom_step_pending",
|
|
|
"zoom_stop_obligation_active", "zoom_no_effect_blocked",
|
|
|
"zoom_step_cancel_requested", "zoom_rearm_required", "lens_move_stop_required",
|
|
|
"lens_move_put_inflight",
|
|
|
))
|
|
|
and self.stop_cooldowns == _cooldowns(st)
|
|
|
and tuple(st.get("last_ptz_physical_cmd", (None, None, None)))[2] == 0
|
|
|
)
|
|
|
|
|
|
def _record(self, frame, fresh):
|
|
|
history = self.history
|
|
|
if (not fresh or frame.tracker_score is None
|
|
|
or frame.tracker_score < self.cfg.tracker_score):
|
|
|
history.clear()
|
|
|
return
|
|
|
if history:
|
|
|
prev = history[-1]
|
|
|
if frame.seq <= prev.seq or frame.at <= prev.at:
|
|
|
history.clear()
|
|
|
return
|
|
|
dt = frame.at - prev.at
|
|
|
if (dt > self.cfg.evidence_gap_s
|
|
|
or any(abs(b - a - rate * dt) > self.cfg.trajectory_slack_deg
|
|
|
for a, b, rate in zip(prev.angles, frame.angles, prev.rates))):
|
|
|
history.clear()
|
|
|
history.append(frame)
|
|
|
|
|
|
def calibration_hold(self, now):
|
|
|
item = self.calibration
|
|
|
return bool(self.cfg.tele_calibration_enabled and item and item.get("stop_ack_at") is not None
|
|
|
and item.get("HFOV_before") and item["HFOV_before"] < 15.0
|
|
|
and 0 <= now - item["stop_ack_at"] <= self.cfg.calibration_timeout_s)
|
|
|
|
|
|
def tele_aim(self, frame, now, camera_rates, optical_rates=(0.0, 0.0)):
|
|
|
# Angular position predicts the image at actuation. The rate term
|
|
|
# reconstructs target motion from residual plus measured gimbal motion.
|
|
|
sample = self.latest_pose
|
|
|
cfg = self.cfg
|
|
|
if (sample is None or sample[0] != frame.seq or not self.optics.fresh(now)
|
|
|
or self.optics.actual >= cfg.tele_steering_start_fov
|
|
|
or not all(math.isfinite(rate) for rate in camera_rates)):
|
|
|
return None
|
|
|
_, angles, rates, reliable = sample
|
|
|
if not reliable:
|
|
|
return None
|
|
|
horizon = max(cfg.latency_horizon_ms / 1000, now - frame.at)
|
|
|
blend = min(1.0, (cfg.tele_steering_start_fov - self.optics.actual) /
|
|
|
(cfg.tele_steering_start_fov - cfg.tele_full_steering_fov))
|
|
|
command = []
|
|
|
for angle, residual, gimbal, optical, reference in zip(
|
|
|
angles, rates, camera_rates, optical_rates,
|
|
|
(cfg.tele_pan_native10_deg_s, cfg.tele_tilt_native10_deg_s)):
|
|
|
predicted = angle + residual * horizon
|
|
|
target_rate = gimbal + residual
|
|
|
demand = cfg.tele_position_gain * predicted + cfg.tele_rate_gain * target_rate + optical
|
|
|
native = min(cfg.tele_native_max, 10 * math.sqrt(abs(demand) / reference))
|
|
|
command.append(math.copysign(native / 100, demand))
|
|
|
return tuple(command), blend
|
|
|
|
|
|
def decide(self, frame, now, *, first, cooldown, transport_clear):
|
|
|
cfg = self.cfg
|
|
|
gate = "initial" if first else "tracking_zoom"
|
|
|
self.latest_pose = None
|
|
|
values = (*frame.offset, *frame.bbox_frac, *frame.bbox_px,
|
|
|
*frame.angles, *frame.rates, frame.at, frame.aspect)
|
|
|
fresh = (frame.geometry_ok and frame.los_valid and frame.seq > 0
|
|
|
and all(math.isfinite(x) for x in values)
|
|
|
and 0 < frame.aspect <= 4 and min(frame.bbox_frac + frame.bbox_px) > 0
|
|
|
and 0 <= now - frame.at <= cfg.frame_age_s)
|
|
|
prev = self.history[-1] if self.history else None
|
|
|
moving = self.request is not None
|
|
|
if frame.tracker_score is not None and not (
|
|
|
math.isfinite(frame.tracker_score) and frame.tracker_score >= cfg.tracker_score
|
|
|
):
|
|
|
self.history.clear()
|
|
|
return ZoomDecision("low_tracker_score", stop=moving, gate=gate)
|
|
|
if not fresh or (prev is not None and (frame.at <= prev.at or frame.seq <= prev.seq)):
|
|
|
self.history.clear()
|
|
|
return ZoomDecision("stale_or_geometry", stop=moving, gate=gate)
|
|
|
if not self.optics.fresh(now):
|
|
|
return ZoomDecision("fov_cooldown", stop=moving, gate=gate)
|
|
|
|
|
|
fov, min_fov = self.optics.estimate(now)
|
|
|
captured_fov, captured_min = self.optics.estimate(frame.at)
|
|
|
if captured_fov is None:
|
|
|
return ZoomDecision("fov_cooldown", stop=moving, gate=gate)
|
|
|
captured_base = next(measured for at, _, _, measured in reversed(self.optics.samples)
|
|
|
if at <= frame.at)
|
|
|
def projection(hfov):
|
|
|
horizontal = math.tan(math.radians(hfov / 2))
|
|
|
return horizontal, horizontal * frame.aspect
|
|
|
|
|
|
axes_at_capture = projection(captured_fov)
|
|
|
angles = tuple(math.degrees(math.atan(2 * offset * axis))
|
|
|
for offset, axis in zip(frame.offset, axes_at_capture))
|
|
|
if prev and 0 < frame.at - prev.at <= cfg.evidence_gap_s:
|
|
|
dt = frame.at - prev.at
|
|
|
rates = tuple((value - old) / dt for value, old in zip(angles, prev.angles))
|
|
|
frame = replace(frame, rates=rates)
|
|
|
frame = replace(frame, angles=angles)
|
|
|
self._record(frame, fresh)
|
|
|
residual_ok = max(map(abs, frame.rates)) <= cfg.max_residual_deg_s
|
|
|
self.latest_pose = (frame.seq, frame.angles, frame.rates,
|
|
|
residual_ok and len(self.history) >= 2)
|
|
|
if not residual_ok:
|
|
|
return ZoomDecision("edge_margin_low", stop=moving, gate=gate)
|
|
|
window = cfg.window(min_fov)
|
|
|
if moving:
|
|
|
remaining = max(0.0, self.optics.active[1] - now)
|
|
|
else:
|
|
|
remaining = window[1]
|
|
|
remaining += cfg.stop_ack_slack_s
|
|
|
horizon = max(cfg.latency_horizon_ms / 1000, now - frame.at) + remaining
|
|
|
broad_initial = first and fov > cfg.entry_fov and len(self.history) >= cfg.tracker_frames
|
|
|
# Residual is signed in camera coordinates. Use its magnitude for the
|
|
|
# edge projection, so a sign flip or overshoot cannot shrink the bound.
|
|
|
narrowed = min(_log_focal(cfg.tele_fov),
|
|
|
_log_focal(min_fov) + cfg.upper_log_rate * remaining)
|
|
|
# Use the widest supported captured view as the reference so a
|
|
|
# narrower prediction never understates the pixel magnification.
|
|
|
growth = math.exp(max(0.0, narrowed - captured_base + cfg.status_error_log))
|
|
|
half_tan = projection(captured_min)
|
|
|
motion = [math.tan(math.radians(min(80, abs(rate) * horizon))) / (2 * axis)
|
|
|
for rate, axis in zip(frame.rates, half_tan)]
|
|
|
bounds = []
|
|
|
for off, size, move, rate in zip(frame.offset, frame.bbox_frac, motion, frame.rates):
|
|
|
# A converging trajectory may reduce the projected excursion at wide
|
|
|
# entry. Preserve the current offset as a bound against early braking.
|
|
|
extent = (max(abs(off), abs(off + math.copysign(move, rate)))
|
|
|
if broad_initial and off * rate < 0 else abs(off) + move)
|
|
|
bounds.append(extent + size / 2)
|
|
|
margin = min(0.5 - extent * growth for extent in bounds)
|
|
|
required = cfg.margin_for(min_fov)
|
|
|
observed_margin = min(.5 - abs(off) - size / 2 for off,size in zip(frame.offset,frame.bbox_frac))
|
|
|
detector_fresh = (0 <= frame.at - frame.detector_at <= cfg.detector_age_s
|
|
|
and 0 <= now - frame.detector_at <= cfg.detector_age_s)
|
|
|
steering_horizon = max(cfg.latency_horizon_ms / 1000, now - frame.at)
|
|
|
forecast_margin = min(
|
|
|
.5 - abs(math.tan(math.radians(max(-80, min(80, angle + rate * steering_horizon))))
|
|
|
/ (2 * axis)) - size/2
|
|
|
for angle, rate, axis, size in zip(frame.angles, frame.rates, half_tan, frame.bbox_frac)
|
|
|
)
|
|
|
step_back = (min_fov <= 12.0 and not moving and detector_fresh
|
|
|
and min(observed_margin, forecast_margin) < required + .025)
|
|
|
if min_fov <= cfg.min_allowed_hfov_deg or not cfg.limits_window(min_fov, remaining):
|
|
|
return ZoomDecision("min_hfov_limit", stop=moving, margin=margin,
|
|
|
step_back=step_back, gate=gate, edge_margin_required=required)
|
|
|
if margin < required:
|
|
|
return ZoomDecision("edge_margin_low", stop=moving, margin=margin,
|
|
|
step_back=step_back, gate=gate, edge_margin_required=required)
|
|
|
if moving and prev and 0 < frame.at - prev.at <= cfg.evidence_gap_s:
|
|
|
dt = frame.at - prev.at
|
|
|
for off, rate, old_rate, axis in zip(frame.angles, frame.rates, prev.rates, half_tan):
|
|
|
outward_accel = math.copysign(1, off) * (rate - old_rate) / (dt * 2 * math.degrees(axis))
|
|
|
outward_rate = math.copysign(1, off) * rate
|
|
|
if margin < max(cfg.edge_watch_margin, required + .04) and outward_rate > 0 and outward_accel > cfg.edge_accel:
|
|
|
return ZoomDecision("edge_acceleration", stop=True, margin=margin, gate=gate)
|
|
|
|
|
|
norm = math.hypot(*frame.angles)
|
|
|
outward = sum(a * r for a, r in zip(frame.angles, frame.rates)) / norm if norm else 0.0
|
|
|
wide_fraction = max(0.0, (fov - cfg.entry_fov) / (cfg.wide_fov - cfg.entry_fov))
|
|
|
center_limit = cfg.repeat_center
|
|
|
if broad_initial:
|
|
|
center_limit = cfg.entry_center
|
|
|
if outward < 0.0:
|
|
|
center_limit *= 1 + min(1.0, wide_fraction) * (cfg.initial_gate_relax_factor - 1)
|
|
|
outward_limit = cfg.max_outward_deg_s * (cfg.initial_gate_relax_factor if broad_initial else 1)
|
|
|
if moving:
|
|
|
if not detector_fresh:
|
|
|
return ZoomDecision("waiting_yolo", stop=True, margin=margin, gate=gate)
|
|
|
if min(frame.bbox_px) >= cfg.target_px:
|
|
|
return ZoomDecision("size_reached", stop=True, margin=margin, gate=gate)
|
|
|
if now >= self.optics.active[1]:
|
|
|
return ZoomDecision("fov_cooldown", stop=True, margin=margin, gate=gate)
|
|
|
# d(theta)/dt needed to cancel (df/f)*tan(theta) on each axis.
|
|
|
optical_gain = cfg.ff_gain * min(1.0, fov / cfg.tele_steering_start_fov)
|
|
|
ff = tuple(max(-cfg.ff_max_deg_s, min(cfg.ff_max_deg_s,
|
|
|
math.degrees(optical_gain * cfg.log_rate * math.sin(math.radians(a))
|
|
|
* math.cos(math.radians(a))))) for a in frame.angles)
|
|
|
return ZoomDecision("pulse_active", ff_deg_s=ff, margin=margin, gate=gate)
|
|
|
if min(frame.bbox_px) >= cfg.target_px:
|
|
|
return ZoomDecision("size_reached", margin=margin, gate=gate)
|
|
|
if not frame.rearm_ready:
|
|
|
return ZoomDecision("geometry_rearm", margin=margin, gate=gate)
|
|
|
if self.calibration_hold(now):
|
|
|
return ZoomDecision("tele_calibration_wait", margin=margin, gate=gate,
|
|
|
edge_margin_required=required)
|
|
|
if not transport_clear or cooldown or min_fov <= cfg.tele_fov + 0.05:
|
|
|
return ZoomDecision("fov_cooldown", margin=margin, gate=gate,
|
|
|
edge_margin_required=required)
|
|
|
if not detector_fresh:
|
|
|
return ZoomDecision("waiting_yolo", margin=margin, gate=gate)
|
|
|
if frame.tracker_score is None or len(self.history) < cfg.tracker_frames:
|
|
|
return ZoomDecision("low_tracker_score", margin=margin, gate=gate)
|
|
|
if max(map(abs, frame.offset)) > center_limit or outward > outward_limit:
|
|
|
return ZoomDecision("not_centered", margin=margin, gate=gate, edge_margin_required=required)
|
|
|
return ZoomDecision("fast_start" if first else "burst", window=window, margin=margin,
|
|
|
gate=gate, edge_margin_required=required)
|
|
|
|
|
|
|
|
|
class EpisodeTelemetryLogger:
|
|
|
"""Write JSONL off the control/STOP threads, with a bounded queue."""
|
|
|
|
|
|
def __init__(self, directory, capacity=4096):
|
|
|
self.directory = Path(directory)
|
|
|
self.queue = Queue(maxsize=capacity)
|
|
|
self.dropped = 0
|
|
|
Thread(target=self._write, name="ptz-zoom-telemetry", daemon=True).start()
|
|
|
|
|
|
def submit(self, camera, owner, event, at, **fields):
|
|
|
generation, started = owner
|
|
|
try:
|
|
|
self.queue.put_nowait((camera, generation, started, {
|
|
|
"cam": camera, "generation": generation, "track_start_utc": started,
|
|
|
"utc": at, "elapsed_ms": round((at - started) * 1000, 1),
|
|
|
"event": event, **fields,
|
|
|
}))
|
|
|
except Full:
|
|
|
self.dropped += 1
|
|
|
|
|
|
def _write(self):
|
|
|
import logging
|
|
|
log = logging.getLogger("PTZTracker")
|
|
|
while True:
|
|
|
camera, generation, started, row = self.queue.get()
|
|
|
try:
|
|
|
self.directory.mkdir(parents=True, exist_ok=True)
|
|
|
path = self.directory / f"ptz-{camera}-{generation}-{started:.6f}.jsonl"
|
|
|
with path.open("a", encoding="utf-8") as out:
|
|
|
out.write(json.dumps(row, allow_nan=False) + "\n")
|
|
|
except (OSError, ValueError):
|
|
|
log.exception("Unable to write PTZ zoom telemetry")
|
|
|
finally:
|
|
|
self.queue.task_done()
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def _owner(st):
|
|
|
return int(st.get("ptz_generation", 0) or 0), float(st.get("track_started_at", 0.0) or 0.0)
|
|
|
|
|
|
|
|
|
def controller_for_track(st, camera_config):
|
|
|
"""Called only from TRACK, under the same lock used by status and the worker."""
|
|
|
if not st.get("tracking_active") or st.get("mode") != "TRACK" or _owner(st)[1] <= 0:
|
|
|
return None
|
|
|
current = st.get("predictive_zoom_controller")
|
|
|
if current is None or not current.owns(st):
|
|
|
try:
|
|
|
settings = ZoomConfig(**camera_config["predictive_zoom"])
|
|
|
except (TypeError, ValueError):
|
|
|
# A malformed per-camera calibration must leave legacy control available.
|
|
|
return None
|
|
|
current = ZoomController(settings, _owner(st))
|
|
|
st["predictive_zoom_controller"] = current
|
|
|
sample = st.get("predictive_zoom_hfov")
|
|
|
if sample is not None:
|
|
|
current.optics.observe(*sample, now=sample[0])
|
|
|
return current
|
|
|
|
|
|
|
|
|
def transport_accepted(config, camera, st, request, native, put_at, ack_at, hard_end,
|
|
|
put_rtt_s=None):
|
|
|
controller = st.get("predictive_zoom_controller")
|
|
|
if controller is None or not controller.owns(st):
|
|
|
return
|
|
|
if controller.calibration and controller.calibration["request"] != request:
|
|
|
_close_calibration(config, camera, controller, ack_at, "next_pulse")
|
|
|
if controller.calibration is None:
|
|
|
before_at = float(st.get("zoom_step_start_status_at", 0.0) or 0.0)
|
|
|
before = st.get("zoom_step_start_hfov_deg")
|
|
|
controller.calibration = dict(
|
|
|
request=request, native=native, put_at=put_at, ack_at=ack_at,
|
|
|
put_rtt_ms=None if put_rtt_s is None else round(1000 * put_rtt_s, 2),
|
|
|
HFOV_before=before, before_at=before_at, stop_issued_at=None,
|
|
|
)
|
|
|
if controller.first_ack_at is None:
|
|
|
controller.first_ack_at = ack_at
|
|
|
_telemetry(config, camera, controller, "first_zoom", ack_at,
|
|
|
t_to_first_zoom_ms=round((ack_at - controller.owner[1]) * 1000, 1),
|
|
|
t_to_first_zoom_put_ms=round((put_at - controller.owner[1]) * 1000, 1),
|
|
|
request=request, native=native)
|
|
|
if (st.get("predictive_zoom_request") == request
|
|
|
and native == controller.cfg.native):
|
|
|
controller.accepted(request, put_at, ack_at, hard_end)
|
|
|
|
|
|
|
|
|
def _cooldowns(st):
|
|
|
return (float(st.get("zoom_step_cooldown_until", 0.0) or 0.0),
|
|
|
float(st.get("zoom_recenter_until", 0.0) or 0.0))
|
|
|
|
|
|
|
|
|
def _reusable_optics(controller, st, at):
|
|
|
before = st.get("zoom_step_start_hfov_deg")
|
|
|
actual = controller.optics.actual
|
|
|
return bool(before and actual and actual < before
|
|
|
and controller.optics.status_at == st.get("zoom_status_at")
|
|
|
and float(st.get("zoom_step_started_at", 0.0)) < controller.optics.status_at <= at)
|
|
|
|
|
|
|
|
|
def _normal_stop_cooldowns(config, st, at):
|
|
|
expected = (getattr(config, "ZOOM_STEP_COOLDOWN_SEC", 0.14),
|
|
|
getattr(config, "ZOOM_STEP_RECENTER_SEC", 0.14))
|
|
|
return all(observed <= at + delay for observed, delay in zip(_cooldowns(st), expected))
|
|
|
|
|
|
|
|
|
def transport_confirmed(config, st, request, at):
|
|
|
controller = st.get("predictive_zoom_controller")
|
|
|
if (controller is not None and controller.owns(st)
|
|
|
and _reusable_optics(controller, st, at) and _normal_stop_cooldowns(config, st, at)):
|
|
|
controller.optical_confirmation(request, at)
|
|
|
controller.stop_cooldowns = _cooldowns(st)
|
|
|
|
|
|
|
|
|
def transport_stopped(config, camera, st, request, at, *, reason, confirmed, clean,
|
|
|
duration_s=None, stop_issued_at=None, stop_rtt_s=None):
|
|
|
controller = st.get("predictive_zoom_controller")
|
|
|
if controller is None or not controller.owns(st):
|
|
|
return
|
|
|
calibration = controller.calibration
|
|
|
if calibration and calibration["request"] == request:
|
|
|
sample = next(((t, _fov(measured)) for t, _, _, measured
|
|
|
in reversed(controller.optics.samples)
|
|
|
if stop_issued_at and t <= stop_issued_at), (None, None))
|
|
|
calibration.update(stop_issued_at=stop_issued_at, stop_ack_at=at,
|
|
|
stop_rtt_ms=None if stop_rtt_s is None else round(1000 * stop_rtt_s, 2),
|
|
|
duration_s=duration_s, stop_reason=reason,
|
|
|
stop_status_at=sample[0], HFOV_stop_sample=sample[1],
|
|
|
clean_stop=bool(clean), first_post_stop=None, latest_post_stop=None)
|
|
|
if stop_issued_at is None:
|
|
|
_close_calibration(config, camera, controller, at, "stop_timing_missing")
|
|
|
if controller.request != request:
|
|
|
return
|
|
|
clean = clean and st.get("predictive_zoom_early_stop") != request
|
|
|
reusable = confirmed and _reusable_optics(controller, st, at)
|
|
|
controller.stopped(request, at, clean=clean, confirmed=reusable and _normal_stop_cooldowns(config, st, at))
|
|
|
controller.stop_cooldowns = _cooldowns(st)
|
|
|
_telemetry(config, camera, controller, "stop", at,
|
|
|
request=request, reason=reason, confirmed=confirmed, clean=clean)
|
|
|
|
|
|
|
|
|
_telemetry_loggers = {}
|
|
|
_telemetry_lock = Lock()
|
|
|
|
|
|
|
|
|
def _telemetry(config, camera, controller, event, at, **fields):
|
|
|
_episode_telemetry(config, camera, controller.owner, event, at, **fields)
|
|
|
|
|
|
|
|
|
def _episode_telemetry(config, camera, owner, event, at, **fields):
|
|
|
directory = getattr(config, "PREDICTIVE_ZOOM_TELEMETRY_DIR", "logs/ptz_zoom")
|
|
|
if not directory:
|
|
|
return
|
|
|
with _telemetry_lock:
|
|
|
log = _telemetry_loggers.get(directory)
|
|
|
if log is None:
|
|
|
log = EpisodeTelemetryLogger(directory)
|
|
|
_telemetry_loggers[directory] = log
|
|
|
log.submit(camera, owner, event, at, **fields, dropped_before=log.dropped)
|
|
|
|
|
|
|
|
|
def telemetry_tick(config, camera, controller, frame, decision, zoom, legacy_reason, at):
|
|
|
estimated, narrowest = controller.optics.estimate(at)
|
|
|
reason = decision.reason
|
|
|
if zoom <= 0.0 and decision.window is not None:
|
|
|
reason = legacy_reason
|
|
|
_telemetry(config, camera, controller, "tick", at, seq=frame.seq,
|
|
|
frame_utc=frame.at, zoom_cmd=zoom, reason=reason,
|
|
|
stop_requested=decision.stop, FOV_actual=controller.optics.actual,
|
|
|
FOV_status_utc=controller.optics.status_at or None,
|
|
|
FOV_estimated=estimated, FOV_min_estimated=narrowest,
|
|
|
bbox_px=frame.bbox_px, tracker_score=frame.tracker_score,
|
|
|
detector_conf=frame.detector_conf, detector_utc=frame.detector_at or None,
|
|
|
margin=decision.margin, ff_deg_s=decision.ff_deg_s, gate=decision.gate,
|
|
|
edge_margin_required=decision.edge_margin_required)
|
|
|
|
|
|
|
|
|
def transport_zoom_block_reason(st, camera_config, now, hard_s):
|
|
|
settings = camera_config.get("predictive_zoom")
|
|
|
if settings is None:
|
|
|
return None
|
|
|
controller = st.get("predictive_zoom_controller")
|
|
|
if controller is not None and controller.owns(st):
|
|
|
cfg = controller.cfg
|
|
|
if controller.calibration_hold(now):
|
|
|
return "tele_calibration_wait"
|
|
|
if not controller.optics.fresh(now):
|
|
|
return "min_hfov_limit"
|
|
|
_, narrowest = controller.optics.estimate(now)
|
|
|
if controller.optics.active:
|
|
|
hard_s = max(0.0, controller.optics.active[1] - now)
|
|
|
else:
|
|
|
try:
|
|
|
cfg = ZoomConfig(**settings)
|
|
|
at, hfov = st.get("predictive_zoom_hfov", (0.0, 0.0))
|
|
|
if not (0 <= now - at <= cfg.status_age_s and cfg.tele_fov <= hfov <= cfg.wide_fov + 1):
|
|
|
return "min_hfov_limit"
|
|
|
narrowest = _fov(_log_focal(hfov) + cfg.status_error_log)
|
|
|
except (TypeError, ValueError):
|
|
|
return "min_hfov_limit"
|
|
|
if narrowest is None or not cfg.limits_window(narrowest, hard_s + cfg.stop_ack_slack_s):
|
|
|
return "min_hfov_limit"
|
|
|
return None
|
|
|
|
|
|
|
|
|
def _close_calibration(config, camera, controller, now, outcome):
|
|
|
item = controller.calibration
|
|
|
if item is None:
|
|
|
return
|
|
|
controller.calibration = None
|
|
|
after = item.pop("latest_post_stop", None)
|
|
|
first = item.pop("first_post_stop", None)
|
|
|
before, stop_sample = item["HFOV_before"], item.get("HFOV_stop_sample")
|
|
|
after_fov = after[1] if after else None
|
|
|
# A request-bound measurement always has independent device timestamps.
|
|
|
# HTTP acknowledgement establishes ordering, not physical lens motion.
|
|
|
duration = item.get("duration_s")
|
|
|
qualified = (outcome == "settled" and item["native"] == 10 and before is not None
|
|
|
and before < 15 and after_fov is not None and after_fov < before
|
|
|
and item.get("clean_stop") and duration and duration > 0
|
|
|
and 0 <= item["put_at"] - item["before_at"] <= controller.cfg.status_age_s)
|
|
|
rate = (_log_focal(after_fov) - _log_focal(before)) / duration if qualified else None
|
|
|
_telemetry(config, camera, controller, "native10_pulse_calibration", now, **item,
|
|
|
outcome=outcome, HFOV_after=after_fov, after_at=after[0] if after else None,
|
|
|
delta_hfov_deg=None if before is None or after_fov is None else after_fov - before,
|
|
|
first_post_stop_at=first[0] if first else None,
|
|
|
HFOV_first_post_stop=first[1] if first else None,
|
|
|
post_stop_tail_deg=first[1] - after_fov if first and after else None,
|
|
|
stop_bracket_narrowing_deg=stop_sample - after_fov if stop_sample and after else None,
|
|
|
stop_sample_age_ms=None if not item.get("stop_status_at") else
|
|
|
1000 * (item["stop_issued_at"] - item["stop_status_at"]),
|
|
|
effective_tele_log_rate_s=rate)
|
|
|
|
|
|
|
|
|
def expire_calibration(config, camera, st, now, *, final=False):
|
|
|
controller = st.get("predictive_zoom_controller")
|
|
|
item = controller.calibration if controller else None
|
|
|
if item is None:
|
|
|
return
|
|
|
if final or (item.get("stop_ack_at") and now-item["stop_ack_at"] > controller.cfg.calibration_timeout_s):
|
|
|
_close_calibration(config, camera, controller, now, "episode_ended" if final else "settle_timeout")
|
|
|
|
|
|
|
|
|
def observe_calibration(config, camera, st, at, hfov, now):
|
|
|
controller = st.get("predictive_zoom_controller")
|
|
|
if controller is None or controller.calibration is None:
|
|
|
return
|
|
|
if not controller.owns(st):
|
|
|
_close_calibration(config, camera, controller, now, "owner_changed")
|
|
|
return
|
|
|
item = controller.calibration
|
|
|
ack = item.get("stop_ack_at")
|
|
|
if ack is None or at < ack:
|
|
|
return
|
|
|
if (st.get("zoom_step_pending") or st.get("lens_move_stop_required")
|
|
|
or st.get("lens_move_put_inflight") and (
|
|
|
st.get("lens_move_put_kind") == "goto"
|
|
|
or float(st.get("lens_move_put_zoom", 0.) or 0.) != 0
|
|
|
)
|
|
|
or abs(float(st.get("last_ptz_physical_cmd", (0., 0., 0.))[2])) > 0):
|
|
|
_close_calibration(config, camera, controller, now, "intervening_lens_motion")
|
|
|
return
|
|
|
if item["first_post_stop"] is None:
|
|
|
item["first_post_stop"] = (at, hfov)
|
|
|
previous = item["latest_post_stop"]
|
|
|
item["latest_post_stop"] = (at, hfov)
|
|
|
if at - ack > controller.cfg.calibration_timeout_s:
|
|
|
_close_calibration(config, camera, controller, now, "unsettled")
|
|
|
elif (previous and at - ack >= controller.cfg.calibration_settle_s
|
|
|
and abs(previous[1] - hfov) <= controller.cfg.calibration_stable_deg):
|
|
|
_close_calibration(config, camera, controller, now, "settled")
|
|
|
|
|
|
|
|
|
def record_zoom_blocks(config, camera, st, at, blocked):
|
|
|
if not getattr(config, "PREDICTIVE_ZOOM_TELEMETRY_DIR", "logs/ptz_zoom"):
|
|
|
return
|
|
|
owner = _owner(st)
|
|
|
record = st.get("episode_zoom_blocks")
|
|
|
if record is None or record["owner"] != owner:
|
|
|
if record is not None:
|
|
|
flush_zoom_blocks(config, camera, st, at, final=True)
|
|
|
record = {"owner": owner, "counts": {}, "last_values": {}, "last_write": at - 1}
|
|
|
st["episode_zoom_blocks"] = record
|
|
|
for reason, values in blocked.items():
|
|
|
record["counts"][reason] = record["counts"].get(reason, 0) + 1
|
|
|
record["last_values"][reason] = values
|
|
|
if at - record["last_write"] >= 1.0:
|
|
|
flush_zoom_blocks(config, camera, st, at)
|
|
|
|
|
|
|
|
|
def flush_zoom_blocks(config, camera, st, at, *, final=False):
|
|
|
record = st.get("episode_zoom_blocks")
|
|
|
if record is None:
|
|
|
return
|
|
|
_episode_telemetry(config, camera, record["owner"], "zoom_block_summary", at,
|
|
|
final=final, zoom_block_reason=dict(record["counts"]),
|
|
|
last_values=dict(record["last_values"]))
|
|
|
record["last_write"] = at
|
|
|
if final:
|
|
|
st.pop("episode_zoom_blocks", None)
|