Add broad-to-tele zoom safety, latency-aware steering and calibration telemetry

feature/ptz-broad-to-tele
Ярослав Карташов 2 weeks ago
parent a7a017e7d1
commit 5cc610a92a

@ -14,14 +14,31 @@
Длительность от 340 мс при HFOV от 25° до 120 мс при HFOV до 15°. У каждого импульса собственный неизменяемый дедлайн с верхним пределом watchdog 400 мс. Ранний STOP использует действующий приоритетный канал. Он сохраняет свежий доворот там, где аппаратная политика разрешает раздельную остановку осей. Требуемый полный STOP сохраняется. Весовые файлы, YOLO, OSTrack и тревоги используют прежние пути.
## Телеметрия и проверки
## Управление широким углом и телеуглом
Профили PR №8 с двумя коэффициентами по-прежнему загружаются. Дополнительные параметры задаются в том же predictive_zoom для камеры.
| Параметр | По умолчанию | Назначение |
|---|---:|---|
| initial_gate_relax_factor | 1,45 | Расширяет допуск начального захвата только на широком угле при сходящейся траектории. |
| tele_edge_margin_ratio | 0,32 | Буфер до края на HFOV 12° и ниже. Плавно растёт от 0,16 при 25°. |
| min_allowed_hfov_deg | 10,0° | Нижний предел прогноза для непрерывного положительного зума. Проверяется перед отправкой PUT. |
| latency_horizon_ms | 245 | Горизонт от захвата кадра до действия доворота. |
| tele_pan_native10_deg_s, tele_tilt_native10_deg_s | 0,65 и 0,85 | Калибровка преобразования угловой скорости в команду. |
| tele_position_gain, tele_rate_gain | 2,7 и 0,85 | Угловая коррекция и упреждение. |
| tele_calibration_enabled | true | На HFOV ниже 15° между положительными импульсами допускает ожидание стабилизации статуса до 1,2 с. |
Укажите PREDICTIVE_ZOOM_TELEMETRY_DIR для записи JSONL отдельно по каждому TRACK. Первый импульс содержит времена до отправки PUT и до успешного ответа. Строки кадров содержат причину решения, ширину и высоту bbox, исходный YOLO confidence, OSTrack score, измеренный и оценочный HFOV, время измерения и нижнюю границу HFOV. Запись идёт в отдельном потоке через ограниченную очередь.
Ускоренный вход разрешается при последовательных достоверных кадрах, допустимой текущей рамке и безопасном прогнозе. Неподтверждённые кадры, конфликт владельца и транспортные запреты продолжают блокировать движение. На телеугле профиль уменьшает вес старого PID по мере сужения поля и подмешивает угловую коррекцию с учётом задержки и синхронизированной скорости приводов. При угрозе краю выполняется отдельный короткий импульс расширения с действующими ограничителями отрицательного зума. Полный STOP остаётся приоритетным.
Предел 10° относится к прогнозу разрешённых импульсов. Настоящая линза и задержки могут отличаться от профиля, поэтому первую полевую серию проверяют по свежему статусу. Абсолютные переходы к пресету обслуживаются прежним кодом.
## Телеметрия и проверки
- Тесты контроллера и логирования: python -m unittest discover -s integration/ptz_predictive_zoom -v
- График эпизода: python integration/ptz_predictive_zoom/plot_episode.py episode.jsonl episode.svg
- Повтор полных путей TRACK и транспорта: run_local_replay.py принимает каталог снимков Codex, каталог этого пакета, корень Electro-pribory и вариант before или after. Для after рядом со снимками нужна пропатченная копия ptz_predictive_zoom_candidate_20261111. Повтор использует локальные fixtures PR №7, файлы телеметрии и имитацию сети и камеры.
По умолчанию сводка zoom_block_summary записывается в JSONL в logs/ptz_zoom через ограниченную очередь. Параметр PREDICTIVE_ZOOM_TELEMETRY_DIR задаёт путь или отключает запись значением None. Сводка включает накопленные centering, bbox_check, angle_tol, edge_margin, min_hfov_limit и put_inflight с последними значениями. Снимок появляется не чаще раза в секунду и при завершении TRACK. Камера, время начала и поколение команд идентифицируют эпизод.
В одном синтетическом пролёте за 3,6 с получено 7 принятых импульсов вместо 4. Пять повторов запущены до нового статуса после STOP. При исходном смещении 26% полуширины кандидат запустил первый импульс, а исходный контроллер за этот пролёт до зума не дошёл. Линза, кадры и валидация в этом сравнении моделировались.
Событие native10_pulse_calibration связывает окно команды, монотонные RTT PUT и STOP, статус до импульса, первый статус после STOP и установившийся HFOV. Поле post_stop_tail_deg показывает наблюдённое сужение между двумя статусами после STOP. Если известен свежий замер до STOP, stop_bracket_narrowing_deg дополнительно показывает изменение между ним и установившимся статусом. Оба измерения ограничены частотой опроса. Отменённые, пересекающиеся и незавершённые окна помечаются. Команда python fit_tele_profile.py episode.jsonl собирает предварительный профиль из допустимых окон для отдельной проверки. Автоматического применения нет.
Профили PTZ19 и PTZ21 для офлайн-повтора приведены в [field_replay_profiles.json](field_replay_profiles.json). Они получены по пяти связанным парам native10 и FOV. Стендовые проверки покрыли задержки видео 180–250 мс и статуса ISAPI 40–60 мс. Измеренного верхнего предела физического отклика на телеугле пока нет. Автоматической загрузки этих значений в конфигурацию камеры нет.
- Unit-тесты: python -m unittest discover -s integration/ptz_predictive_zoom -v
- Полный повтор TRACK и транспорта: run_local_replay.py принимает каталог снимков Codex, каталог пакета, корень Electro-pribory, вариант before или after и необязательный путь к материализованной серверной копии.
- Парный стенд: replay_broad_tele.py запускается с --camera 19 и --camera 21, дополнительно с --edge-stress, --put-rtt и --problem. Каталог с candidate и pins.json задаёт PTZ_BROAD_TELE_REPLAY_DIR. Нужны сохранённые записи AIM и стенд предыдущего PR в соседних каталогах снимков.
- График одного эпизода: python plot_episode.py episode.jsonl episode.svg

@ -0,0 +1,105 @@
{
"paired_cases": 32,
"problem_cases": 3,
"runtime_after_sha": {
"postprocess.py": "dba9b3dc69768526777e166a8ef3270ee54a5d9ed7415a3f905b65e05a8534a5",
"ptz_io.py": "a469c6683c9afc93a1499dd45146e1198bbd752505ba9045704ce5fb8a5d378b",
"status.py": "0f0b91414e04eedc547a5e6931825974f9b5bc09d5795babe5c6ac6179f0882d",
"zoom_controller.py": "6bb81b19e7125daa4ab3ae387ac8ba963bde86f0a51a938d72684ec32d755be3"
},
"minimum_margin_at_6_deg": {
"PR8": 0.1844,
"BT": 0.2508
},
"step_back_cases_at_6_deg": 10,
"joint_below_18_by_camera_starts_at_least_12_deg": {
"19": {
"PR8_pct": 81.54,
"BT_pct": 100.0,
"paired_samples": 661
},
"21": {
"PR8_pct": 81.45,
"BT_pct": 99.92,
"paired_samples": 1256
}
},
"joint_below_18_by_camera_wide_starts": {
"19": {
"PR8_pct": 100.0,
"BT_pct": 100.0,
"paired_samples": 379
},
"21": {
"PR8_pct": 86.45,
"BT_pct": 99.9,
"paired_samples": 974
}
},
"joint_11_to_18_by_camera": {
"19": {
"PR8_pct": 100.0,
"BT_pct": 100.0,
"paired_samples": 407
},
"21": {
"PR8_pct": 86.83,
"BT_pct": 99.9,
"paired_samples": 1002
}
},
"below_18_by_controller_at_own_exposure": {
"19:59.0": {
"PR8_pct": 100.0,
"BT_pct": 100.0,
"PR8_samples": 873,
"BT_samples": 383
},
"19:12.0": {
"PR8_pct": 56.74,
"BT_pct": 100.0,
"PR8_samples": 282,
"BT_samples": 282
},
"19:6.0": {
"PR8_pct": 50.44,
"BT_pct": 93.88,
"PR8_samples": 686,
"BT_samples": 686
},
"21:59.0": {
"PR8_pct": 85.56,
"BT_pct": 99.21,
"PR8_samples": 990,
"BT_samples": 1008
},
"21:12.0": {
"PR8_pct": 64.18,
"BT_pct": 100.0,
"PR8_samples": 282,
"BT_samples": 282
},
"21:6.0": {
"PR8_pct": 41.98,
"BT_pct": 90.23,
"PR8_samples": 686,
"BT_samples": 686
}
},
"problem_first_start_s": [
{
"PR8": 2.18,
"BT": 1.68
},
{
"PR8": 2.22,
"BT": 1.72
},
{
"PR8": 2.25,
"BT": 1.75
}
],
"no_common_tele_exposure_pairs": 5,
"field_calibration_performed": false
}

@ -0,0 +1,68 @@
"""Summarize qualified telephoto pulses from episode JSONL into review candidates."""
import argparse
from collections import defaultdict
import json
import math
from pathlib import Path
from statistics import median
def focal(fov):
return -math.log(math.tan(math.radians(fov / 2)))
def summarize(files):
grouped = defaultdict(dict)
for file in files:
with file.open(encoding="utf-8") as rows:
for line in rows:
item = json.loads(line)
if item.get("event") != "native10_pulse_calibration":
continue
key = (item["track_start_utc"], item["generation"], item["request"])
grouped[item["cam"]][key] = item
result = {}
for camera, records in sorted(grouped.items()):
accepted = []
for item in records.values():
rate = item.get("effective_tele_log_rate_s")
if (item.get("outcome") != "settled" or not isinstance(rate, (float, int))
or not math.isfinite(rate) or rate <= 0):
continue
before, after = item["HFOV_before"], item["HFOV_after"]
if not 0 < after < before < 15:
continue
if item.get("put_rtt_ms") is None or item.get("stop_rtt_ms") is None:
continue
accepted.append(item)
result[str(camera)] = {"qualified": len(accepted), "seen": len(records),
"candidate_for_review": None}
# A single track may have a common framing or status-timing bias.
if len(accepted) < 3 or len({x["track_start_utc"] for x in accepted}) < 2:
continue
rates = [x["effective_tele_log_rate_s"] for x in accepted]
bracket = [
max(0., focal(x["HFOV_after"]) - focal(x["HFOV_stop_sample"]))
for x in accepted if x.get("stop_status_at") is not None
and x["put_at"] <= x["stop_status_at"] <= x["stop_issued_at"]
and 0 <= x["stop_issued_at"] - x["stop_status_at"] <= .18
]
full_sweep_rate = math.log(252 / 6) / 5.5
result[str(camera)]["candidate_for_review"] = {
"observed_hfov_before_deg": [min(x["HFOV_before"] for x in accepted),
max(x["HFOV_before"] for x in accepted)],
"median_effective_tele_log_rate_s": median(rates),
"stress_upper_gain": max(rates) * 1.25 / full_sweep_rate,
"qualified_stop_brackets": len(bracket),
"max_stop_bracket_log_narrowing": max(bracket, default=None),
"max_put_rtt_ms": max(x["put_rtt_ms"] for x in accepted),
"max_stop_rtt_ms": max(x["stop_rtt_ms"] for x in accepted),
}
return {"scope": "observed_telephoto_windows_only", "production_enable": False, "cameras": result}
if __name__ == "__main__":
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("files", nargs="+", type=Path)
args = parser.parse_args()
print(json.dumps(summarize(args.files), indent=2, allow_nan=False))

@ -1,17 +1,17 @@
{
"postprocess.py": {
"before": "776b5b337e5f898e69770fec517c4459560f4cc6adeb951af40ac8f0b2178534",
"after": "82ba632e7e2696e5b48daa99e77741482b98a02103eaf7fb416c58fd686e4327"
"after": "dba9b3dc69768526777e166a8ef3270ee54a5d9ed7415a3f905b65e05a8534a5"
},
"ptz_io.py": {
"before": "13d31539193b1a773a5d1db2187cc2a0c8db67d0aacaad89082face64df07499",
"after": "0b0651b0e0344e4f78aeaf01f76c969d0043148a4ac92e4b7b6fd4a5f0d6a2e7"
"after": "a469c6683c9afc93a1499dd45146e1198bbd752505ba9045704ce5fb8a5d378b"
},
"status.py": {
"before": "2fca9a2000c2cac9af42ad3f637416c66e1bb87f2f8166c7aefbbebbe5d07b06",
"after": "820db8615638c728d1017ea365f3d16b95edfe61dbb684ba0cda8de051ea8bd1"
"after": "0f0b91414e04eedc547a5e6931825974f9b5bc09d5795babe5c6ac6179f0882d"
},
"zoom_controller.py": {
"after": "7197baba29976215b05f6f86ae1563e8b5c28b314a80894600ede517de00f91e"
"after": "6bb81b19e7125daa4ab3ae387ac8ba963bde86f0a51a938d72684ec32d755be3"
}
}

@ -0,0 +1,484 @@
"""Run PR7 and PR8 on identical trajectories reconstructed from logged AIM."""
from bisect import bisect_right
from collections import Counter, deque
from contextlib import ExitStack
from hashlib import sha256
from pathlib import Path
from statistics import median
from types import ModuleType, SimpleNamespace
from unittest.mock import patch
import argparse
import cv2
import json
from inspect import unwrap
import logging
import math
import os
import socket
import sys
OUT = Path(os.environ.get(
"PTZ_BROAD_TELE_REPLAY_DIR",
Path(__file__).resolve().parents[3] / "ptz_broad_tele_20261111",
))
DATA = OUT.parent / "ptz_native10_profiles_20261111"
sys.path.insert(0, str(DATA))
from measure_native10 import FIELD, PULSES, PR7, FULL_SWEEP_LOG_RATE, ROOT, read, utc
LAB = ROOT / "ptz_virtual_lab_20261111"
sys.path.insert(0, str(LAB))
import simulator as sim
CANDIDATE = ROOT / "ptz_predictive_zoom_candidate_20261111"
BUNDLE = ROOT / "chameleon_predictive_zoom_git_20261111/integration/ptz_predictive_zoom"
BASE_STATUS = sim.SOURCE / "status.py"
NEW = OUT / "candidate"
NEW_PINS = OUT / "pins.json"
TRACE_KEYS = ("normal_zoom_quality_ok", "zoom_quality_ok", "zoom_rearm_ready", "zoom_safe",
"zoom_angular_safe", "stable_ok", "near_edge", "edge_recovery_active",
"followup_ready", "raw_zoom_needed", "normal_size_hold", "final_guard_active",
"visual_control_stale", "visual_control_coast", "zoom_zero_reason",
"zoom_cmd", "can_zoom_in", "predictive_decision")
FLIGHTS = {
"19": ("03:11:07", "04:54:08"),
"21": ("04:13:33", "04:37:45"),
}
def angle(error, fov):
return math.degrees(math.atan(2*error*math.tan(math.radians(fov/2))))
def trajectory(camera, episode, source):
start, end = map(utc, (episode["started_at"], episode["ended_at"]))
candidates = [r for r in source["cameras"][camera]["samples"]
if start <= r["t"] <= min(end, start+25)
and r["err"] and r["fov"] and r["gimbal"] and r["optics_age"] is not None
and r["optics_age"] <= .65 and r["source"] in ("visual","detector","acquire")]
groups = []
for r in candidates:
if not groups or r["t"] - groups[-1][-1]["t"] > 1.65:
groups.append([])
groups[-1].append(r)
# Prefer the longest contiguous trace so neither controller bridges a gap.
rows = max(groups, key=lambda g: g[-1]["t"]-g[0]["t"])
origin = rows[0]["t"]
pose = [0.0, 0.0]
samples = []
for i,r in enumerate(rows):
if i:
previous = rows[i-1]
dt = r["t"] - previous["t"]
for axis, sign in enumerate((1,-1)):
pose[axis] += sign * dt * (r["gimbal"][axis]+previous["gimbal"][axis])/2
bearing = tuple(pose[k]+angle(r["err"][k], r["fov"][k]) for k in range(2))
samples.append((r["t"]-origin,*bearing))
duration = math.floor(samples[-1][0]*100)/100
if duration < 3:
raise ValueError(f"Insufficient contiguous AIM trace: camera {camera} {episode['started_at']}")
# Only a scalar filtered size is available. Use a projected square with that
# geometric mean at initial FOV and hold angular extent constant.
first_size = episode["size_first"] or .018
fov_x, fov_y = rows[0]["fov"]
angular = tuple(2*angle(first_size/2, fov) for fov in (fov_x, fov_y))
return dict(camera=int(camera), episode=episode["started_at"], aim_start=origin,
duration=duration, knots=samples, angular_extent=angular,
source_rows=len(samples), observed_hfov=rows[0]["fov"])
class FlightCamera(sim.Camera):
def __init__(self, parameters, pan_sign, tilt_sign, trace, initial_fov):
super().__init__(parameters, pan_sign, tilt_sign)
self.trace = trace
self.times = [x[0] for x in trace["knots"]]
self.magnification = sim.OPTICAL_ZOOM ** (
math.log(math.tan(math.radians(59/2))/math.tan(math.radians(initial_fov/2)))
/ math.log(math.tan(math.radians(59/2))/math.tan(math.radians(2/2)))
)
if initial_fov < 20:
_, az, el = trace["knots"][0]
# Keep this synthetic narrow-angle stress start inside the aperture.
self.pan, self.tilt = az-.10*initial_fov, el-.05*initial_fov
def target(self, t):
i = min(len(self.times)-2, max(0, bisect_right(self.times,t)-1))
a,b = self.trace["knots"][i:i+2]
frac = min(1,max(0,(t-a[0])/(b[0]-a[0])))
az, el = (x+(y-x)*frac for x,y in zip(a[1:],b[1:]))
if self.trace.get("edge_stress"):
# Same finite lateral acceleration for both controllers.
push = min(1.1,max(0.0,t-.90))
az += 2.8 * push*push / 1.21
return az,el
def project(self, t, *, clipped=True):
center = self.target(t)
bounds = []
for bearing, pose, width, fov, pixels in zip(
center,(self.pan,self.tilt),self.trace["angular_extent"],(self.hfov,self.vfov),(1280,720)
):
# stress small angles with a tiny object that still asks for zoom
if self.trace.get("stress"):
width *= .22
deviation = bearing-pose
if abs(deviation)+width/2 >= 85:
return None
focal = (pixels/2)/math.tan(math.radians(fov/2))
lo,hi = [round(pixels/2+focal*math.tan(math.radians(deviation+o*width/2))) for o in (-1,1)]
if clipped:
lo,hi = max(0,lo),min(pixels-1,hi)
if hi <= lo:
return None
bounds.append((lo,hi))
return (bounds[0][0],bounds[1][0],bounds[0][1],bounds[1][1])
def capture_control(pp, function):
row = {}
code = unwrap(pp.postprocess_control).__code__
def trace(frame,event,arg):
if event == "call" and frame.f_code is code:
frame.f_trace_lines = False
return trace
if event == "return" and frame.f_code is code:
row.update({k:frame.f_locals[k] for k in TRACE_KEYS if k in frame.f_locals})
row["return_line"] = frame.f_lineno
return None
original = sys.gettrace()
sys.settrace(trace)
try:
function()
finally:
sys.settrace(original)
return row
def blocker(row, st):
"""Primary gate plus all simultaneous blockers. Locals come from live policy."""
if "zoom_cmd" not in row:
return "track_or_frame_rejected", []
if row["zoom_cmd"] > 0:
return None,[]
reasons = [
(row.get("final_guard_active"), "gimbal_brake"),
(row.get("near_edge") or row.get("edge_recovery_active"), "edge_margin_low"),
(row.get("visual_control_stale") or row.get("visual_control_coast"), "stale_visual"),
(not row.get("zoom_rearm_ready",True), "geometry_rearm"),
(not row.get("raw_zoom_needed",True) or row.get("normal_size_hold"), "size_controller"),
(not row.get("zoom_quality_ok",True), "quality_or_validation"),
(not row.get("followup_ready",True), "fov_cooldown"),
(not row.get("zoom_safe",True), "framing"),
(not row.get("zoom_angular_safe",True), "angular_limit"),
(not row.get("stable_ok",True), "not_centered_or_not_stable"),
]
flags = [why for enabled,why in reasons if enabled]
predictive = row.get("predictive_decision")
if predictive and predictive.stop:
return predictive.reason,flags
return (flags[0] if flags else row.get("zoom_zero_reason") or st.get("zoom_useful_pulse_reason") or "unknown"),flags
def run(trace, profile, steering, revision, video_ms, isapi_ms, initial_fov=59.,
plant_stress=False, put_rtt=False):
import numpy
import torch
cam = trace["camera"]-11
cfg = sim.Parameters(camera=trace["camera"], seconds=trace["duration"],
video_ms=video_ms, command_ms=isapi_ms, optics_report_ms=isapi_ms,
frame_hz=10, status_hz=4, steering_exponent=2,
pan_at_10=steering["pan"]["native10_equivalent_deg_s"],
tilt_at_10=steering["tilt"]["native10_equivalent_deg_s"],
zoom_at_10=profile["upper_gain" if plant_stress else "nominal_gain"] * FULL_SWEEP_LOG_RATE
/ math.log(math.tan(math.radians(59/2))/math.tan(math.radians(2/2))) * math.log(42)/math.log(2))
with ExitStack() as stack:
for owner,name in ((socket.socket,"connect"),(socket.socket,"connect_ex"),
(socket,"create_connection"),(torch,"load"),(torch.cuda,"_lazy_init")):
stack.enter_context(patch.object(owner,name,sim.deny))
stack.enter_context(patch.object(torch.cuda,"is_available",return_value=False))
pkg = ModuleType(sim.PKG)
pkg.__path__ = [str(sim.BASE/"geometry_candidate/ptz_tracker_modular"),str(sim.ROOT)]
stale_modules = {k:m for k,m in sys.modules.items() if k.startswith(sim.PKG+".")}
for k in stale_modules:
sys.modules.pop(k)
stack.enter_context(patch.dict(sys.modules,{sim.PKG:pkg}))
model = ModuleType(sim.PKG+".model")
model.model_shared,model.yolo_forward=object(),sim.deny
sys.modules[model.__name__] = model
sim.load(sim.PKG+".visual_control_probe",sim.SOURCE/"visual_control_probe.py")
control = sim.load(sim.PKG+".visual_control",sim.SOURCE/"visual_control.py")
pins = read(NEW_PINS if revision=="BT" else BUNDLE/"pins.json")
bundle = NEW if revision=="BT" else BUNDLE
candidate = NEW if revision=="BT" else CANDIDATE
for filename, values in pins.items():
path = (bundle if filename == "zoom_controller.py" else candidate) / filename
assert sha256(path.read_bytes()).hexdigest() == values["after"], filename
sim.load(sim.PKG+".zoom_controller",bundle/"zoom_controller.py")
io_path, pp_path, status_path = [candidate/f for f in ("ptz_io.py","postprocess.py","status.py")]
io = sim.load(sim.PKG+".ptz_io",io_path)
status = sim.load(sim.PKG+".status",status_path)
pp = sim.load(sim.PKG+".postprocess",pp_path)
from ptz_tracker_modular import config,state,visual_tracker
values = read(sim.BASE/"zoom_continuation_candidate/runtime_policy_snapshot.json")["values"]
for key,value in values.items():
stack.enter_context(patch.object(config,key,value,create=True))
stack.enter_context(patch.object(config,"PTZ_CAM_IDS",[cam,*[x for x in config.PTZ_CAM_IDS if x!=cam]]))
transport = sim.load("native10_mock_transport",sim.ROOT/"tests/test_zoom_useful_transport.py")
before = sim.load("native10_control_fixture",sim.REPLAY)
before.np,before.torch,before.config,before.state = numpy,torch,config,state
before.ptz_io,before.pp,before.tracker,before.control,before.transport = io,pp,visual_tracker,control,transport
case = before.Replay()
case.setUp()
stack.callback(case.doCleanups)
f,st = case.fixture,case.st
cam_config = config.CAMERA_CONFIG[cam]
settings = dict(ptz_hfov_wide_deg=59.,ptz_hfov_tele_deg=2.)
settings["predictive_zoom"] = dict(profile)
if revision=="BT":
settings["predictive_zoom"].update(
tele_pan_native10_deg_s=cfg.pan_at_10, tele_tilt_native10_deg_s=cfg.tilt_at_10,
latency_horizon_ms=video_ms+isapi_ms/2,
)
settings["predictive_zoom"].update(trace.get("tuning",{}))
stack.enter_context(patch.object(config,"PREDICTIVE_ZOOM_TELEMETRY_DIR",None,create=True))
stack.enter_context(patch.dict(cam_config,settings))
for key in ("ZOOM_STEP_PULSE_SEC","ZOOM_STEP_HARD_STOP_SEC"):
stack.enter_context(patch.object(config,key,values[key]))
plant = FlightCamera(cfg,float(cam_config.get("pan_sign",1)),float(cam_config.get("tilt_sign",-1)),trace,initial_fov)
initial = plant.project(0)
if initial is None:
raise ValueError("Invalid initial viewport")
st.update(last_bbox=initial,prev_cx=(initial[0]+initial[2])/2560,
prev_cy=(initial[1]+initial[3])/1440,stable_frames=0,
track_started_at=100.,last_seen=100.,visual_tracker_last_detector_at=100.,
visual_tracker_last_zoom_detector_at=100.,visual_tracker_last_detector_conf=.8)
status._update_runtime_state(cam,None,None,99.99,zoom_ratio=plant.magnification,hfov_deg=plant.hfov)
st["vfov_deg"]=plant.vfov
# Replace only the asynchronous STOP thread's scheduler so it runs at
# this virtual tick; the actual priority STOP path still runs.
if revision in ("PR8","BT"):
class InlineThread:
def __init__(self, *, target, **_): self.target=target
def start(self): self.target()
original_thread = io.threading.Thread
def choose_thread(*args, **kwargs):
if kwargs.get("name") == "ptz-predictive-stop":
return InlineThread(**kwargs)
return original_thread(*args, **kwargs)
stack.enter_context(patch.object(io.threading,"Thread",choose_thread))
t = 0.
commands, rows, decisions = [], [], []
statuses, frames = deque(),deque()
request_starts, request_ids = [],set()
prior_lens = 0
busy_until = 0.0
def send(_url, *, data=b"", **_kw):
nonlocal prior_lens, busy_until
axes = tuple(transport.xml_axis(str(data),name) for name in ("pan","tilt","zoom"))
# Model physical propagation and GET response separately. PUT
# success and STOP timers use the existing deterministic fixture.
issue = max(t,f.wall-100) if put_rtt else t
response = issue+isapi_ms/1000 if put_rtt else issue
propagation = isapi_ms/(2000 if put_rtt else 1000)
plant.commands.append((issue+propagation,axes))
commands.append((round(issue,4),axes))
if put_rtt:
busy_until = max(busy_until,response)
f.wall,f.mono=100+response,1000+response
request = st.get("zoom_step_pending_id")
if axes[2]>0 and prior_lens<=0 and request and request not in request_ids:
request_starts.append(round(response,4))
request_ids.add(request)
prior_lens=axes[2]
return True,200,""
stack.enter_context(patch.object(io,"_ptz_put",side_effect=send))
stack.enter_context(patch.object(io,"_ptz_put_priority_stop",side_effect=send))
logger=logging.getLogger("PTZTracker")
saved=logger.level
logger.setLevel(logging.ERROR)
stack.callback(logger.setLevel,saved)
interval = video_ms//10
stop_step = round(trace["duration"]*100)
optics_count=0
for step in range(stop_step+1):
t=step/100
now = max(t,busy_until)
f.wall,f.mono=100+now,1000+now
if step:
plant.step(t,.01)
if step%25==0:
statuses.append((step+isapi_ms//10,step,plant.magnification,plant.hfov,plant.vfov,
plant.pan_rate*plant.pan_sign,plant.tilt_rate*plant.tilt_sign))
while statuses and statuses[0][0]<=step and t+1e-8>=busy_until:
_,origin,mag,hfov,vfov,pan,tilt=statuses.popleft()
measured_at=100+origin/100
st.update(status_rate_at=measured_at,pan_rate_at=measured_at,tilt_rate_at=measured_at,
pan_rate_deg_s=pan,tilt_rate_deg_s=tilt,vfov_deg=vfov)
st["status_rate_history"] = (st.get("status_rate_history", []) +
[(measured_at,pan,tilt)])[-20:]
status._update_runtime_state(cam,None,None,f.wall,zoom_ratio=mag,hfov_deg=hfov,
zoom_age_sec=(step-origin)/100)
timer=st.get("zoom_step_stop_timer")
if (timer and timer.is_alive() and f.mono+1e-8>=st["zoom_step_stop_started_mono"]+timer.delay):
timer.fire()
if step%10==0:
frames.append((step+interval,step,plant.project(t)))
while frames and frames[0][0]<=step and t+1e-8>=busy_until:
due,origin,box=frames.popleft()
case.seq+=1
track=visual_tracker.VisualTrackResult(box is not None,box,.8 if box else 0.,0,.8,
case.seq,"telemetry projection",case.seq)
age=(step-origin)/100
captured_at=f.wall-age
visual=control.VisualSample(control.owner(st),case.seq,captured_at,track)
if origin%40==0 and box is not None:
detection=SimpleNamespace(boxes=before.Boxes([(*box,.8,0)]))
pp.postprocess_control(cam,case.frame,detection,True,case.seq,visual_sample=visual)
frame=SimpleNamespace(boxes=before.Boxes([]))
row=capture_control(pp,lambda: pp.postprocess_control(
cam,case.frame,frame,True,case.seq,visual_sample=visual,visual_only=True))
reason,flags=blocker(row,st)
decisions.append(dict(t=t, zoom_block_reason=reason, flags=flags,
requested=row.get("zoom_cmd"), source_seq=case.seq,
return_line=row.get("return_line"),
actual_hfov=plant.hfov,
forecast_reason=getattr(row.get("predictive_decision"),"reason",None)))
if not state.ptz_cmd_q.empty():
state.ptz_cmd_q.put_nowait(None)
io._ptz_worker()
optics_count=max(optics_count,st.get("zoom_in_pulse_count",0))
if step%5==0 or step==stop_step:
box=plant.project(t,clipped=False)
if box is None:
error,margin,px=None,-.5,None
else:
x0,y0,x1,y1=box
error=max(abs((x0+x1)/2560-.5),abs((y0+y1)/1440-.5))
margin=min(x0/1280,1-x1/1280,y0/720,1-y1/720)
px=(x1-x0,y1-y0)
rows.append(dict(t=t,hfov=plant.hfov,margin=margin,center_err=error,bbox_px=px,
mode=st.get("mode"),zoom=plant.active[2],
tele_steering=st.get("tele_steering_active",False)))
first=request_starts[0] if request_starts else None
prefix=[row for row in decisions if first is None or row["t"]<=first]
errors=[r["center_err"] for r in rows if r["center_err"] is not None]
tele = [r for r in rows if r["hfov"] < 18]
telemetry=dict(
trace={k:val for k,val in trace.items() if k!="knots"}, revision=revision,
tested_after_sha={file:row["after"] for file,row in pins.items()},
video_ms=video_ms,isapi_ms=isapi_ms,initial_fov=initial_fov,plant_stress=plant_stress,
put_rtt_ms=isapi_ms if put_rtt else 0,
observed_model=dict(profile=profile,pan_at_native10=cfg.pan_at_10,tilt_at_native10=cfg.tilt_at_10),
metrics=dict(first_zoom_s=first,positive_starts=len(request_starts),optics_confirmed_max=optics_count,
hfov_min=round(min(r["hfov"] for r in rows),3),
tele_samples=len(tele),
tele_center_pct=round(100*sum(r["center_err"] is not None and
r["center_err"]<=.1 for r in tele)/len(tele),1) if tele else None,
tele_steering_samples=sum(bool(r["tele_steering"]) for r in tele),
fully_in_frame_pct=round(100*sum(r["margin"]>=0 for r in rows)/len(rows),1),
inside_center_10pct=round(100*sum(r["center_err"] is not None and r["center_err"]<=.1 for r in rows)/len(rows),1),
min_edge_margin=round(min(r["margin"] for r in rows),4),
peak_center_error=round(max(errors),4) if errors else None,
narrow_lens_active_samples=sum(r["hfov"]<=15 and r["zoom"]>0 for r in rows),
narrowing_with_edge_clip_samples=sum(r["margin"]<0 and r["zoom"]>0 for r in rows),
first_edge_clip_s=next((r["t"] for r in rows if r["margin"]<0),None),
positive_native=dict(Counter(axes[2] for _,axes in commands if axes[2]>0)),
max_bbox_short_px=max((min(r["bbox_px"]) for r in rows if r["bbox_px"]),default=0)),
zoom_block_reason_before_first=dict(Counter(r["zoom_block_reason"] for r in prefix if r["zoom_block_reason"])),
zoom_block_reason=dict(Counter(r["zoom_block_reason"] for r in decisions if r["zoom_block_reason"])),
simultaneous_flags_before_first=dict(Counter(x for r in prefix for x in r["flags"])),
request_starts=request_starts,commands=commands,rows=rows,decisions=decisions)
return telemetry
def main():
parser=argparse.ArgumentParser(description=__doc__)
parser.add_argument("--camera",choices=sorted(FLIGHTS))
parser.add_argument("--tune",action="store_true")
parser.add_argument("--problem",action="store_true")
parser.add_argument("--tuning-json",default="{}")
parser.add_argument("--smoke",action="store_true")
parser.add_argument("--edge-stress",action="store_true")
parser.add_argument("--put-rtt",action="store_true")
parser.add_argument("--counters",action="store_true",
help="repeat the first six seconds with per-tick controller diagnostics")
args=parser.parse_args()
import torch
torch.set_num_threads(1)
dest=OUT/"final"
dest.mkdir(exist_ok=True)
profiles=read(DATA/"profiles.json")["cameras"]
field,pulses=read(FIELD),read(PULSES)
steering=read(LAB/"field-calibration-report.json")["cameras"]
jobs=[]
for camera, keys in FLIGHTS.items():
if args.camera and args.camera!=camera:
continue
traces=[trajectory(camera,next(e for e in pulses["episodes"][camera] if e["started_at"][11:19]==key),field)
for key in keys]
if args.problem:
if camera=="19":
ep=next(e for e in read(PR7)["episodes"][camera] if "05:49:18" in e["started_at"] or "12:49:18" in e["started_at"])
trace=trajectory(camera,ep,field)
for video,api in ((180,40),(220,50),(250,60)):
jobs.append((trace,video,api,59.,False))
continue
if args.tune:
trace = {**traces[0],"stress":True,"edge_stress":True,
"duration":min(5.,traces[0]["duration"])}
jobs.extend(((traces[-1],220,50,59.,False),(trace,250,60,6.,True),
({**traces[0],"stress":True,"duration":7.},250,60,12.,True)))
continue
if args.put_rtt:
for video,api in ((180,40),(250,60)):
jobs.append(({**traces[1],"duration":min(7.,traces[1]["duration"])},video,api,59.,False))
jobs.append(({**traces[0],"stress":True,"edge_stress":True,
"duration":min(5.,traces[0]["duration"])},video,api,6.,True))
continue
if args.edge_stress:
trace = {**traces[0],"stress":True,"edge_stress":True,
"duration":min(5.,traces[0]["duration"])}
for video,api in ((180,40),(250,60)):
jobs.append((trace,video,api,6.,True))
continue
if args.counters:
for trace in traces:
jobs.append(({**trace,"duration":min(6.,trace["duration"])},220,50,59.,False))
continue
if args.smoke:
jobs.append((traces[0],220,50,59.,False))
continue
for trace in traces:
for video,api in ((180,40),(220,50),(250,60)):
jobs.append((trace,video,api,59.,False))
for fov in (12.,6.):
for video,api in ((180,40),(250,60)):
stress = {**traces[0],"stress":True,"duration":min(traces[0]["duration"],7.)}
jobs.append((stress,video,api,fov,True))
summaries=[]
for trace,video,api,fov,stress in jobs:
cam=str(trace["camera"])
versions=[]
for revision in ("PR8","BT"):
configured_trace = {**trace,"tuning":json.loads(args.tuning_json)}
result=run(configured_trace,profiles[cam]["replay_settings"],steering[cam]["fit"],
revision,video,api,fov,stress,put_rtt=args.put_rtt)
name=f"{cam}-{trace['episode'][11:19].replace(':','')}-{video}-{api}-{int(fov)}-{revision}{'-counters' if args.counters else ''}{'-edge' if args.edge_stress else ''}{'-put' if args.put_rtt else '-tune' if args.tune else '-problem' if args.problem else ''}.json"
(dest/name).write_text(json.dumps(result,allow_nan=False,separators=(",",":"))+"\n",encoding="utf-8")
versions.append({k:result[k] for k in ("revision","metrics","zoom_block_reason_before_first","tested_after_sha")}|{"trace_file":name})
summaries.append(dict(camera=int(cam),episode=trace["episode"],video_ms=video,isapi_ms=api,
initial_fov=fov,stress=stress,put_rtt_ms=api if args.put_rtt else 0,runs=versions))
print(cam,trace["episode"][11:19],video,api,fov,
[(x["metrics"]["first_zoom_s"],x["metrics"]["hfov_min"],x["metrics"]["tele_center_pct"],
x["metrics"]["min_edge_margin"],x["metrics"]["tele_steering_samples"]) for x in versions],
flush=True)
name = f"comparison{('-'+args.camera) if args.camera else ''}{'-smoke' if args.smoke else ''}{'-counters' if args.counters else ''}{'-edge' if args.edge_stress else ''}{'-put' if args.put_rtt else '-tune' if args.tune else '-problem' if args.problem else ''}.json"
(dest/name).write_text(json.dumps(summaries,indent=2,allow_nan=False)+"\n",encoding="utf-8")
if __name__=="__main__":
main()

@ -1,11 +1,11 @@
from pathlib import Path
import json, os, sys
v,z,root,variant=map(Path,sys.argv[1:])
candidate=v/"ptz_predictive_zoom_candidate_20261111"
v,z,root,variant=map(Path,sys.argv[1:5])
candidate=Path(sys.argv[5]) if len(sys.argv)>5 else v/"ptz_broad_tele_20261111/candidate"
pr7=v/"ptz_zoom_threshold_diagnostics_20261111"
pins=json.loads((z/"pins.json").read_text())
dest=v/f"predictive_zoom_regression_{variant}.json"
dest=candidate.parent/f"regression_{variant}.json"
before=str(variant)=="before"
p=(v/"ptz_zoom_threshold_maintenance_20261111" if before else candidate)/"postprocess.py"
i=(v/"ptz_zoom_zero_decision_20261109" if before else candidate)/"ptz_io.py"
@ -24,6 +24,9 @@ r=(pr7/"replay.py").read_text(encoding="utf-8")
if not before:
r=r.replace(" ptz_io=load",f" load(PKG+'.zoom_controller', Path({str(z/'zoom_controller.py')!r}))\n ptz_io=load")
r=r.replace(" pp=load",f" status=load(PKG+'.status', Path({str(s)!r}))\n pp=load")
r=r.replace(" values=json.loads", """ stack.enter_context(patch.object(
config, 'PREDICTIVE_ZOOM_TELEMETRY_DIR', None, create=True))
values=json.loads""")
r=r.replace(" result=unittest.TextTestRunner", """ from track_integration import TRACE as zoom_trace, build_track_cases
integration = build_track_cases(before, pp, ptz_io, config, state, status)
for case in ("test_track_runs_burst_without_post_stop_status",
@ -31,11 +34,12 @@ if not before:
"test_track_starts_at_26_percent_of_half_frame",
"test_track_legacy_reference_at_26_percent_of_half_frame",
"test_priority_zoom_stop_honors_axis_policy",
"test_priority_zoom_stop_does_not_override_full_stop"):
"test_priority_zoom_stop_does_not_override_full_stop",
"test_transport_floor_preserves_gimbal_and_rejects_lens"):
suite.addTest(integration(case))
result=unittest.TextTestRunner""")
r=r.replace(" result_file=Path", f""" if result.wasSuccessful():
Path({str(v/"predictive_zoom_simulated_episode.jsonl")!r}).write_text(
Path({str(candidate.parent/"regression_episode.jsonl")!r}).write_text(
"".join(json.dumps(row, allow_nan=False) + "\\n" for row in zoom_trace), encoding="utf-8")
result_file=Path""")
code=compile(r,str(pr7/"replay.py"),"exec")

@ -1,14 +1,16 @@
--- a/postprocess.py
+++ b/postprocess.py
@@ -10,6 +10,7 @@
@@ -10,6 +10,9 @@
import cv2
from . import config, state, visual_control
+from .zoom_controller import ZoomFrame, controller_for_track, telemetry_tick
+from .zoom_controller import (
+ ZoomFrame, controller_for_track, expire_calibration, flush_zoom_blocks, record_zoom_blocks, telemetry_tick,
+)
from . import motion_detector
from .utils import (
clipf,
@@ -30,6 +31,7 @@
@@ -30,6 +33,7 @@
_zoom_command_lock,
zoom_step_optics_delta,
_zoom_speed_to_int,
@ -16,7 +18,7 @@
)
from .geom import ang_diff_signed, sector_contains
from .patrol import (
@@ -4597,6 +4599,7 @@
@@ -4597,6 +4601,7 @@
optics_observed_now: float | None = None,
stop_trace: list[str] | None = None,
continuation_trace: list[tuple[int, int, float, float, float, float, float]] | None = None,
@ -24,7 +26,7 @@
) -> float:
"""Break continuous zoom-in into short feedback-controlled steps."""
pulse_until = float(st.get("zoom_step_until", 0.0) or 0.0)
@@ -4706,18 +4709,23 @@
@@ -4706,18 +4711,23 @@
_finish_zoom_step(st, now, stable_key=stable_key)
return 0.0
if pulse_until <= 0.0:
@ -53,7 +55,16 @@
if not feedback_ready:
st["zoom_followup_reason"] = str(feedback_reason)
return 0.0
@@ -8778,6 +8786,74 @@
@@ -6553,6 +6563,8 @@
if (now - last) < TRACK_END_RETURN_COOLDOWN_SEC:
return False
st["last_track_end_return_at"] = now
+ flush_zoom_blocks(config, cam_idx, st, now, final=True)
+ expire_calibration(config, cam_idx, st, now, final=True)
# СРАЗУ ставим "за нято goto", чтобы второй источник не успел добавить ещё один goto
settle = float(getattr(config, "GOTO_SETTLE_SEC", 2.5))
@@ -8778,6 +8790,76 @@
react_margin = clipf(react_margin_base + react_margin_gain * vel, react_margin_base, react_margin_max)
near_react_edge = min_margin < react_margin
@ -65,6 +76,7 @@
+ predictive = controller_for_track(st, cfg_ptz)
+ if predictive is not None:
+ decision_wall = time.time()
+ expire_calibration(config, cam_idx, st, decision_wall)
+ sample_is_current = bool(
+ visual_sample is not None
+ and visual_control.current(
@ -88,13 +100,14 @@
+ detector_conf=float(st.get("visual_tracker_last_detector_conf", 0.0) or 0.0),
+ geometry_ok=bool(
+ sample_is_current and not st.get("visual_tracker_mismatch_frames")
+ and _zoom_revalidation_ready(st)
+ ),
+ rearm_ready=_zoom_revalidation_ready(st),
+ los_valid=bool(
+ st.get("aim_los_rate_valid")
+ or predictive.request is not None
+ or predictive.repeat_after_stop(st, decision_wall)
+ or (st.get("zoom_step_pending") and predictive.history)
+ or hfov_deg < predictive.cfg.tele_steering_start_fov
+ ),
+ )
+ skip_wait = predictive.repeat_after_stop(st, decision_wall)
@ -128,7 +141,7 @@
zoom_quality_ok = bool(
normal_zoom_quality_ok
or probe_allowed
@@ -8819,6 +8895,21 @@
@@ -8819,6 +8901,21 @@
pan_rate_deg_s=float(los_pan_rate_deg_s),
tilt_rate_deg_s=float(los_tilt_rate_deg_s),
)
@ -150,7 +163,7 @@
angular_rejection = {} if pulse_active else None
zoom_angular_safe = followup_ready and _zoom_angular_motion_safe(
pan_error_deg=float(los_pan_angle_deg),
@@ -8832,6 +8923,9 @@
@@ -8832,6 +8929,9 @@
)
if approach_margin is not None and followup_ready:
zoom_angular_safe = True
@ -160,7 +173,7 @@
st["zoom_safe_pred_margin"] = float(zoom_pred_margin)
st["zoom_safe_center_limit"] = float(zoom_center_max)
st["zoom_quality_ok"] = bool(zoom_quality_ok)
@@ -8952,6 +9046,7 @@
@@ -8952,6 +9052,7 @@
or bounded_identification_request
)
zoom_rearm_ready = _zoom_revalidation_ready(st)
@ -168,7 +181,7 @@
can_zoom_in = (
zoom_safe
and zoom_angular_safe
@@ -9099,6 +9194,14 @@
@@ -9099,6 +9200,14 @@
zoom_zero_reason = "deadband"
zoom_cmd = 0.0
@ -183,7 +196,7 @@
zoom_after_size_minimum = zoom_cmd
cmd_mag_est = max(
abs(float(pan_speed) + float(pan_rate_ff)),
@@ -9151,7 +9254,7 @@
@@ -9151,7 +9260,7 @@
center_err=float(center_err),
moving_out=bool(moving_out),
recent_zoom=bool(recent_zoom_steering),
@ -192,7 +205,46 @@
)
if before_recenter > 0.0 and zoom_cmd <= 0.0:
zoom_zero_reason = "recenter"
@@ -9272,6 +9375,25 @@
@@ -9159,21 +9268,27 @@
out_edge_effective
or float(min_margin) < float(getattr(config, "Z_SAFETY_EDGE", 0.05))
)
- zoom_cmd, routine_zoom_out_blocked = _gate_routine_zoom_out_hold(
+ predictive_step_back = bool(
+ predictive_decision and predictive_decision.step_back
+ and not zoom_out_emergency and not st.get("zoom_step_pending")
+ and not st.get("lens_move_put_inflight") and not st.get("zoom_stop_obligation_active")
+ and float(st.get("last_ptz_physical_cmd", (0., 0., 0.))[2]) <= 0
+ )
+ if predictive_step_back:
+ zoom_cmd = -predictive.cfg.step_back_cmd
+ routine_zoom_out_blocked = False
+ else:
+ zoom_cmd, routine_zoom_out_blocked = _gate_routine_zoom_out_hold(
+ st, now, zoom_cmd, emergency=zoom_out_emergency,
+ )
+ zoom_cmd = _gate_zoom_out_pulse(
st,
- now,
+ time.time() if predictive_step_back else now,
zoom_cmd,
emergency=zoom_out_emergency,
)
- zoom_cmd = _gate_zoom_out_pulse(
- st,
- now,
- zoom_cmd,
- emergency=zoom_out_emergency,
- )
- if float(zoom_cmd) < 0.0 and float(prev_z) >= 0.0 and not bool(
- st.get("zoom_rearm_required", False)
- ):
+ if (not predictive_step_back and float(zoom_cmd) < 0.0 and float(prev_z) >= 0.0
+ and not st.get("zoom_rearm_required", False)):
_arm_zoom_revalidation(st, now, "zoom_out", cam_idx=cam_idx)
pre_ramp_zoom_cmd = float(zoom_cmd)
if zoom_cmd > 0.0:
@@ -9272,6 +9387,25 @@
proposed_zoom_cmd = max(zoom_cmd, min(pre_ramp_zoom_cmd, pulse_cap, .12))
if _zoom_native_response_available(st, _zoom_speed_to_int(proposed_zoom_cmd), hfov_deg, shared_wide_hfov):
zoom_cmd = proposed_zoom_cmd
@ -218,7 +270,7 @@
before_shape = zoom_cmd
zoom_cmd, useful_reason = _shape_useful_zoom_pulse(
zoom_cmd,
@@ -9299,6 +9421,21 @@
@@ -9299,6 +9433,23 @@
requested_native=timing_native,
rejection=response_rejection,
)
@ -232,15 +284,17 @@
+ float(st["zoom_step_pending_hard_sec"]),
+ )
+ if predictive_decision and (
+ predictive_decision.stop or (predictive_decision.reason == "size_reached"
+ and pulse_family == "normal")
+ predictive_decision.stop or predictive_decision.reason in ("min_hfov_limit", "tele_calibration_wait")
+ or (predictive_decision.reason == "edge_margin_low"
+ and hfov_deg <= predictive.cfg.tele_pulse_fov)
+ or (predictive_decision.reason == "size_reached" and pulse_family == "normal")
+ ):
+ zoom_cmd = min(zoom_cmd, 0.0)
+ zoom_zero_reason = predictive_decision.reason
if useful_reason == "useful" and response_timing == (0.0, 0.0):
zoom_cmd = 0.0
st["zoom_useful_pulse_reason"] = "wait_response_framing"
@@ -9321,7 +9458,10 @@
@@ -9321,7 +9472,10 @@
optics_observed_now=zoom_gate_now,
stop_trace=gate_stop,
continuation_trace=continuation_trace,
@ -252,7 +306,7 @@
if gate_stop:
zoom_zero_reason = gate_stop[-1]
decision_generation = st.get("ptz_generation")
@@ -9380,6 +9520,12 @@
@@ -9380,6 +9534,12 @@
adj_pan, adj_tilt = _shape_tracking_cmd(st, now, adj_pan, adj_tilt, center_err)
pan_feedback_cmd, tilt_feedback_cmd = float(adj_pan), float(adj_tilt)
@ -265,7 +319,30 @@
adj_pan, adj_tilt = _merge_gimbal_rate_feedforward(
st,
now,
@@ -9844,6 +9990,14 @@
@@ -9398,6 +9558,22 @@
precenter_optics_at = st.get("zoom_status_at")
precenter_measured_hfov = st.get("hfov_deg")
status_observed_now = time.time()
+ tele_rate_history = tuple(st.get("status_rate_history", ())) if predictive else ()
+ if predictive and predictive_frame and predictive.optics.actual is not None:
+ st["tele_steering_active"] = False
+ aligned = next((x for x in reversed(tele_rate_history)
+ if 0 <= now - x[0] <= predictive.cfg.frame_age_s), None)
+ if aligned and status_observed_now - aligned[0] <= predictive.cfg.status_age_s:
+ steering = predictive.tele_aim(
+ predictive_frame, status_observed_now,
+ (float(aligned[1]) * _PAN_SIGN, float(aligned[2]) * _TILT_SIGN),
+ predictive_decision.ff_deg_s if predictive_decision else (0., 0.),
+ )
+ if steering is not None:
+ (pan_tele, tilt_tele), blend = steering
+ adj_pan += blend * (pan_tele - adj_pan)
+ adj_tilt += blend * (tilt_tele - adj_tilt)
+ st["tele_steering_active"] = True
guard_status_fresh = bool(
guard_status_at > 0.0
and 0.0 <= (status_observed_now - guard_status_at)
@@ -9844,12 +10020,53 @@
("brake", final_guard_active),
) if blocked)
zoom_decision = (source_seq, decision_request, zoom_zero_reason, "+".join(blockers) or "-")
@ -277,31 +354,83 @@
+ )
+ if predictive_decision.stop and predictive.request is not None:
+ stop_predictive_zoom_async(cam_idx, predictive, predictive_decision.reason)
+ if getattr(config, "PREDICTIVE_ZOOM_TELEMETRY_DIR", "logs/ptz_zoom"):
+ blocked = {}
+ if zoom_cmd <= 0.0 and raw_zoom_needed:
+ if not stable_ok:
+ blocked["centering"] = {"error": center_err, "limit": lock_gate, "stable_frames": st.get("stable_frames")}
+ if not zoom_safe:
+ blocked["bbox_check"] = {"margin": zoom_pred_margin, "limit": zoom_center_max,
+ "bbox_px": [x2-x1, y2-y1]}
+ if not zoom_angular_safe and followup_ready:
+ blocked["angle_tol"] = {"pan_deg": los_pan_angle_deg, "tilt_deg": los_tilt_angle_deg,
+ "limit_deg": angular_max}
+ if near_edge or edge_recovery_active or (
+ predictive_decision and predictive_decision.reason in ("edge_margin_low", "edge_acceleration")):
+ blocked["edge_margin"] = {"measured": min_margin,
+ "predicted": predictive_decision.margin if predictive_decision else None,
+ "required": predictive_decision.edge_margin_required if predictive_decision else None}
+ if predictive_decision and predictive_decision.reason == "min_hfov_limit":
+ blocked["min_hfov_limit"] = {"hfov": hfov_deg, "minimum": predictive.cfg.min_allowed_hfov_deg}
+ if st.get("lens_move_put_inflight"):
+ blocked["put_inflight"] = {"kind": st.get("lens_move_put_kind"), "request": decision_request}
+ with _zoom_command_lock(cam_idx):
+ record_zoom_blocks(config, cam_idx, st, time.time(), blocked)
dispatch_kwargs = {"force": force_track_ptz}
if zoom_decision is not None:
dispatch_kwargs["zoom_decision"] = zoom_decision
- _dispatch_owned_zoom_tracking(
- cam_idx, _PAN_SIGN * adj_pan, _TILT_SIGN * adj_tilt, zoom_cmd, **dispatch_kwargs
- )
+ if predictive_step_back and zoom_cmd < 0.0:
+ with _zoom_command_lock(cam_idx):
+ if not visual_control.current(cam_idx, visual_sample, time.time(), max_age=.25):
+ return
+ if not st.get("zoom_rearm_required"):
+ _arm_zoom_revalidation_locked(st, time.time(), "tele_step_back")
+ dispatch_kwargs["force"] = True
+ _dispatch_owned_zoom_tracking(
+ cam_idx, _PAN_SIGN * adj_pan, _TILT_SIGN * adj_tilt, zoom_cmd, **dispatch_kwargs
+ )
+ else:
+ _dispatch_owned_zoom_tracking(
+ cam_idx, _PAN_SIGN * adj_pan, _TILT_SIGN * adj_tilt, zoom_cmd, **dispatch_kwargs
+ )
if visual_control.enabled():
dispatch_at = time.monotonic()
samples = st.setdefault('control_dispatch_samples', [])
--- a/ptz_io.py
+++ b/ptz_io.py
@@ -14,6 +14,7 @@
@@ -14,6 +14,9 @@
from requests.adapters import HTTPAdapter
from . import config, state
+from .zoom_controller import transport_accepted, transport_confirmed, transport_stopped
+from .zoom_controller import (
+ record_zoom_blocks, transport_accepted, transport_confirmed, transport_stopped, transport_zoom_block_reason,
+)
from .utils import clipf, hfov_ptz_from_abs_zoom, hfov_ptz_from_zoom_ratio, quantf
from .geom import sector_contains, ang_diff_signed, normalize360
@@ -582,6 +583,10 @@
@@ -550,6 +553,7 @@
cmd: PTZCommand,
now: float,
*, native_cmd: int = 0,
+ put_rtt_s: float | None = None,
) -> bool:
"""Anchor a pending +zoom window to the successful camera HTTP PUT."""
if not _pending_zoom_request_matches(st, cmd):
@@ -582,6 +586,10 @@
st["zoom_last_motion_at"] = float(now)
st["zoom_followup_reason"] = "physical_pulse"
st["zoom_step_last_delivery_result"] = "physical_started"
+ transport_accepted(
+ config, cam_idx, st, request_id, native_cmd, deadline_origin, now,
+ deadline_origin + hard_sec,
+ deadline_origin + hard_sec, put_rtt_s,
+ )
logger.info(
"[ZOOM DELIVERY] cam=%s request=%s kind=%s physical start seq=%s pulse=%.3fs hard=%.3fs",
cam_idx,
@@ -744,6 +749,8 @@
@@ -744,6 +752,8 @@
float(st.get("zoom_recenter_until", 0.0) or 0.0),
float(now) + float(getattr(config, "ZOOM_STEP_RECENTER_SEC", 0.14)),
)
@ -310,7 +439,16 @@
logger.info(
"[ZOOM DELIVERY] cam=%s request=%s optics confirmed source=%s zstep=%s",
cam_idx,
@@ -922,6 +929,12 @@
@@ -800,6 +810,8 @@
reason: str,
stop_axes: Optional[tuple[int, int, int]] = None,
stop_command: Optional[MoveTuple] = None,
+ stop_issued_at: float | None = None,
+ stop_rtt_s: float | None = None,
) -> bool:
"""Finalize transport delivery; charge a pulse only with optical evidence."""
if not bool(st.get("zoom_step_pending", False)):
@@ -922,6 +934,13 @@
+ max(0.0, float(getattr(config, "ZOOM_STEP_RETRY_COOLDOWN_SEC", 0.12))),
)
@ -319,16 +457,39 @@
+ confirmed=confirmed and not discard,
+ clean=bool(not discard and kind == "normal" and delivered + 1e-6 >= full_window
+ and stop_axes is not None and stop_axes[2] == 0),
+ duration_s=delivered, stop_issued_at=stop_issued_at, stop_rtt_s=stop_rtt_s,
+ )
st["zoom_step_last_delivered_sec"] = float(delivered)
st["zoom_step_pending"] = False
st["zoom_step_pending_id"] = 0
@@ -2119,6 +2132,35 @@
if zoom_decision is not None:
return cmd_source, cmd_generation, move_seq, zoom_i
@@ -1257,6 +1276,7 @@
float(getattr(config, "PTZ_LENS_STOP_DIRECT_TIMEOUT_SEC", 0.75)),
),
)
+ stop_issued_at, stop_issued_mono = time.time(), time.monotonic()
ok, code, body = _ptz_put_priority_stop(
url,
data=xml,
@@ -1266,6 +1286,7 @@
verify=verify,
)
now = time.time()
+ stop_rtt_s = time.monotonic() - stop_issued_mono
st["zoom_stop_obligation_attempts"] = int(
st.get("zoom_stop_obligation_attempts", 0) or 0
) + 1
@@ -1328,6 +1349,7 @@
now,
reason=f"priority_stop:{reason}",
stop_axes=(pan_i, tilt_i, 0),
+ stop_issued_at=stop_issued_at, stop_rtt_s=stop_rtt_s,
)
_cancel_zoom_step_watchdog(st)
if ordering_inflight:
@@ -2121,6 +2143,35 @@
return None
+
+
+def stop_predictive_zoom_async(cam_idx, controller, reason):
+ """Order an early lens STOP independently of the ordinary MOVE queue."""
+ with _zoom_command_lock(cam_idx):
@ -356,12 +517,82 @@
+ except RuntimeError:
+ # The scheduled watchdog remains the independent hard STOP.
+ logger.exception("Unable to start early lens STOP cam=%s", cam_idx)
+
+
def stop_ptz(cam_idx: int, *, source: str = "stop", generation: Optional[int] = None) -> None:
"""
Жёсткий стоп PTZ.
@@ -3308,6 +3359,23 @@
reserve_wall = time.time()
reserve_mono = time.monotonic()
+ camera_profile = config.CAMERA_CONFIG.get(cam_idx, {})
+ floor_block = (transport_zoom_block_reason(
+ st, camera_profile, reserve_wall, _zoom_step_timing(st)[1],
+ ) if int(zoom_i) > 0 else None)
+ if floor_block:
+ if pending_matches and not st.get("zoom_step_physical_started_at"):
+ _clear_pending_zoom_delivery(st, reserve_wall, reason=floor_block, force=True)
+ pending_matches = False
+ if st.get("mode") == "TRACK":
+ record_zoom_blocks(config, cam_idx, st, reserve_wall, {
+ floor_block: {"hfov": st.get("hfov_deg"), "stage": "pre_put",
+ "request": _cmd_move_seq(cmd)},
+ })
+ zoom_i = 0
+ if bool(getattr(config, "PTZ_ZOOM_STOP_FORCE_AXES_ZERO", True)):
+ pan_i = tilt_i = 0
+ xml = f"<PTZData><pan>{pan_i}</pan><tilt>{tilt_i}</tilt><zoom>0</zoom></PTZData>"
if int(zoom_i) > 0 and pending_matches and float(
st.get("zoom_step_physical_started_at", 0.0) or 0.0
) <= 0.0:
@@ -3481,6 +3549,7 @@
sent_nonzero_move = bool(
int(pan_i) != 0 or int(tilt_i) != 0 or int(zoom_i) != 0
)
+ put_call_at, put_call_mono = time.time(), time.monotonic()
ok, code, body = _ptz_put(
url,
data=xml,
@@ -3489,6 +3558,7 @@
timeout=timeout,
verify=verify,
)
+ put_rtt_s = time.monotonic() - put_call_mono
if ok:
break
if attempt < retry_count:
@@ -3560,7 +3630,7 @@
st,
cmd,
physical_now,
- native_cmd=int(zoom_i),
+ native_cmd=int(zoom_i), put_rtt_s=put_rtt_s,
)
elif int(zoom_i) <= 0:
if float(previous_physical_zoom) > 0.0:
@@ -3571,6 +3641,8 @@
reason="physical_stop",
stop_axes=(pan_i, tilt_i, zoom_i),
stop_command=cmd,
+ stop_issued_at=put_call_at if int(zoom_i) == 0 else None,
+ stop_rtt_s=put_rtt_s,
)
elif (
int(zoom_i) == 0
--- a/status.py
+++ b/status.py
@@ -170,6 +170,13 @@
@@ -14,6 +14,9 @@
# ---------------- Публичные утилиты статуса ----------------
+from .zoom_controller import observe_calibration
+
+
def get_cached_angles(cam_id: int) -> Tuple[Optional[float], Optional[float], Optional[float]]:
"""
Вернёт (pan_deg, tilt_deg, t_epoch) из кэша, если есть (иначе None).
@@ -170,6 +173,14 @@
if isinstance(hfov_deg, (int, float)) and float(hfov_deg) > 0.0:
st["hfov_deg"] = float(hfov_deg)
st["zoom_status_at"] = zoom_observed_at
@ -372,6 +603,7 @@
+ controller = st.get("predictive_zoom_controller")
+ if controller is not None and controller.owns(st):
+ controller.optics.observe(zoom_observed_at, float(hfov_deg), float(now))
+ observe_calibration(config, cam_id, st, zoom_observed_at, float(hfov_deg), float(now))
if isinstance(zoom_raw, (int, float)):
st["zoom_position_raw"] = float(zoom_raw)
st["zoom_position_tag"] = str(zoom_tag or "")

@ -0,0 +1,120 @@
import json
from pathlib import Path
from tempfile import TemporaryDirectory
import unittest
from types import SimpleNamespace
from unittest.mock import patch
import zoom_controller as z
from fit_tele_profile import summarize
from test_zoom_controller import frame, tick
class BroadTeleTests(unittest.TestCase):
def make(self, fov, **settings):
c = z.ZoomController(z.ZoomConfig(.5, .9, **settings), (3, 100.))
c.optics.observe(99.99, fov, 100.)
return c
def test_broad_converging_entry_beyond_static_band(self):
ctrl = self.make(59)
for k, x in enumerate((.173, .167, .161)):
decision = tick(ctrl, frame(100 + .08*k, k+1,
offset=(x,0), detector_at=99.99))
self.assertEqual(decision.reason, "fast_start")
self.assertEqual(decision.gate, "initial")
self.assertGreater(decision.margin, ctrl.cfg.edge_margin)
ctrl = self.make(59)
for k in range(3):
decision = tick(ctrl, frame(100+.08*k,k+1,offset=(.161,0),detector_at=99.99))
self.assertEqual(decision.reason, "not_centered")
def test_margin_curve_and_floor_keep_legacy_wire_from_bypassing(self):
ctrl = self.make(10.6)
self.assertAlmostEqual(ctrl.cfg.margin_for(59), .16)
self.assertAlmostEqual(ctrl.cfg.margin_for(12), .32)
self.assertGreater(ctrl.cfg.margin_for(15), ctrl.cfg.margin_for(20))
decision = tick(ctrl, frame(100.08, 1, offset=(.2,0), detector_at=99.99))
self.assertEqual(decision.reason, "min_hfov_limit")
self.assertIsNone(decision.window)
self.assertTrue(decision.step_back)
profile = {"predictive_zoom": {"nominal_gain": .5, "upper_gain": .9}}
self.assertEqual(z.transport_zoom_block_reason({"predictive_zoom_hfov":(99.9,10.6)},
profile,100.,.12),"min_hfov_limit")
self.assertIsNone(z.transport_zoom_block_reason({"predictive_zoom_hfov":(99.9,59.)},
profile,100.,.40))
self.assertEqual(z.transport_zoom_block_reason({"predictive_zoom_hfov":(98.9,59.)},
profile,100.,.40),"min_hfov_limit")
self.assertIsNone(z.transport_zoom_block_reason({}, {}, 100., .40))
def test_latency_horizon_moves_prediction_in_residual_direction(self):
def command(horizon):
ctrl = self.make(14, latency_horizon_ms=horizon)
f = frame(100.20,2,offset=(.025, -.03),detector_at=99.99)
ctrl.latest_pose = (2,(.34,-.20),(.5,-.2),True)
result=ctrl.tele_aim(f,100.25,(0.,0.))
self.assertIsNotNone(result)
return result[0]
slow,fast = command(120),command(245)
self.assertGreater(fast[0],slow[0])
self.assertLess(fast[1],slow[1])
def test_measured_pulse_reports_settled_delta_rtt_and_tail(self):
ctrl = self.make(14.5)
st=dict(predictive_zoom_controller=ctrl,mode="TRACK",tracking_active=True,
ptz_generation=3,track_started_at=100.,
predictive_zoom_request=1,zoom_step_start_status_at=99.99,
zoom_step_start_hfov_deg=14.5,last_ptz_physical_cmd=(0.,0.,0.))
with patch.object(z,"_episode_telemetry") as submit:
cfg=SimpleNamespace()
z.transport_accepted(cfg,8,st,1,10,100.03,100.08,100.21,.05)
ctrl.optics.observe(100.15,13.9,100.15)
z.transport_stopped(cfg,8,st,1,100.21,reason="priority_stop:watchdog",
confirmed=True,clean=True,duration_s=.18,stop_issued_at=100.16,
stop_rtt_s=.05)
self.assertTrue(ctrl.calibration_hold(100.22))
for at,fov in ((100.25,13.6),(100.48,13.4),(100.63,13.4)):
z.observe_calibration(cfg,8,st,at,fov,at)
self.assertFalse(ctrl.calibration_hold(100.64))
row=submit.call_args.kwargs
self.assertEqual(row["outcome"],"settled")
self.assertEqual(row["put_rtt_ms"],50.)
self.assertEqual(row["stop_rtt_ms"],50.)
self.assertAlmostEqual(row["delta_hfov_deg"],-1.1)
self.assertAlmostEqual(row["post_stop_tail_deg"],.2)
self.assertAlmostEqual(row["stop_bracket_narrowing_deg"],.5)
self.assertGreater(row["effective_tele_log_rate_s"],0.)
def test_offline_fit_excludes_censored_rows_and_duplicate_requests(self):
base=dict(event="native10_pulse_calibration", cam=8, generation=3,
outcome="settled", effective_tele_log_rate_s=.4, HFOV_before=14.,
HFOV_after=13., put_rtt_ms=50., stop_rtt_ms=55., put_at=101.,
stop_status_at=101.11, stop_issued_at=101.16, HFOV_stop_sample=13.2)
records = [dict(base, track_start_utc=100.+i//2, request=i+1) for i in range(3)]
records += [records[0], dict(base, track_start_utc=102., request=5, outcome="next_pulse")]
with TemporaryDirectory() as tmp:
path=Path(tmp)/"episode.jsonl"
path.write_text("".join(json.dumps(x)+"\n" for x in records),encoding="utf-8")
result=summarize([path])
self.assertFalse(result["production_enable"])
camera=result["cameras"]["8"]
self.assertEqual(camera["qualified"],3)
self.assertEqual(camera["candidate_for_review"]["qualified_stop_brackets"],3)
def test_episode_counts_and_snapshot_values(self):
cfg=SimpleNamespace()
st=dict(ptz_generation=2,track_started_at=100.)
with patch.object(z,"_episode_telemetry") as emit:
z.record_zoom_blocks(cfg,8,st,100.1,{"angle_tol":{"pan_deg":4.}})
z.record_zoom_blocks(cfg,8,st,100.2,{"angle_tol":{"pan_deg":4.2},"put_inflight":{"request":2}})
self.assertEqual(emit.call_count,1)
z.flush_zoom_blocks(cfg,8,st,100.3,final=True)
last=emit.call_args.kwargs
self.assertEqual(last["zoom_block_reason"],{"angle_tol":2,"put_inflight":1})
self.assertEqual(last["last_values"]["angle_tol"]["pan_deg"],4.2)
self.assertTrue(last["final"])
self.assertNotIn("episode_zoom_blocks",st)
if __name__=="__main__":
unittest.main()

@ -79,7 +79,7 @@ class PredictiveZoomTests(unittest.TestCase):
pulses.append(t)
else:
if decision.stop:
self.assertEqual(decision.reason, "size_reached")
self.assertIn(decision.reason, ("size_reached", "edge_margin_low", "min_hfov_limit"))
c.stopped(active[0], now, clean=False, confirmed=False)
last_stop = now
stops.append(now)

@ -153,4 +153,20 @@ def build_track_cases(fixture, pp, ptz_io, config, state, status):
def test_priority_zoom_stop_does_not_override_full_stop(self):
self.assert_early_stop_axes(full_stop=True)
def test_transport_floor_preserves_gimbal_and_rejects_lens(self):
f, st = self.fixture, self.st
self.stack.enter_context(patch.dict(config.CAMERA_CONFIG[self.cam], {
"predictive_zoom": {"nominal_gain": .45, "upper_gain": .8},
}))
self.stack.enter_context(patch.object(config, "PTZ_ZOOM_STOP_FORCE_AXES_ZERO", False, create=True))
self.stack.enter_context(patch.object(config, "PREDICTIVE_ZOOM_TELEMETRY_DIR", None, create=True))
status._update_runtime_state(self.cam, None, None, f.wall-.05, zoom_ratio=6., hfov_deg=10.6)
command = pp._gate_zoom_in_pulse(st, f.wall, .10, True, pulse_floor=.10, pulse_timing=(.12, .18))
self.assertGreater(command, 0.)
f._enqueue(command, pan=.12, tilt=-.08)
f._flush()
self.assertFalse(st.get("zoom_step_pending"))
axes = tuple(fixture.transport.xml_axis(f.sent[-1], axis) for axis in ("pan", "tilt", "zoom"))
self.assertEqual(axes, (12, -8, 0))
return PredictiveTrack

@ -19,6 +19,22 @@ class ZoomConfig:
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
@ -48,6 +64,8 @@ class ZoomConfig:
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 (
@ -60,6 +78,17 @@ class ZoomConfig:
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
@ -74,6 +103,14 @@ class ZoomConfig:
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)))
@ -182,6 +219,7 @@ class ZoomFrame:
detector_conf: float | None
geometry_ok: bool
los_valid: bool
rearm_ready: bool = True
@dataclass(frozen=True)
@ -191,6 +229,9 @@ class ZoomDecision:
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:
@ -206,6 +247,8 @@ class ZoomController:
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:
@ -275,8 +318,42 @@ class ZoomController:
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
@ -289,17 +366,17 @@ class ZoomController:
math.isfinite(frame.tracker_score) and frame.tracker_score >= cfg.tracker_score
):
self.history.clear()
return ZoomDecision("low_tracker_score", stop=moving)
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)
return ZoomDecision("stale_or_geometry", stop=moving, gate=gate)
if not self.optics.fresh(now):
return ZoomDecision("fov_cooldown", stop=moving)
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)
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):
@ -315,15 +392,19 @@ class ZoomController:
frame = replace(frame, rates=rates)
frame = replace(frame, angles=angles)
self._record(frame, fresh)
if max(map(abs, frame.rates)) > cfg.max_residual_deg_s:
return ZoomDecision("edge_margin_low", stop=moving)
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 = cfg.steering_delay_s + (now - frame.at) + remaining
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),
@ -334,48 +415,80 @@ class ZoomController:
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)]
margin = min(0.5 - (abs(off) + size / 2 + move) * growth
for off, size, move in zip(frame.offset, frame.bbox_frac, motion))
if margin < cfg.edge_margin:
return ZoomDecision("edge_margin_low", stop=moving, margin=margin)
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 < cfg.edge_watch_margin and outward_rate > 0 and outward_accel > cfg.edge_accel:
return ZoomDecision("edge_acceleration", stop=True, margin=margin)
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 + min(1.0, wide_fraction)
* (cfg.entry_center - cfg.repeat_center)) if first else cfg.repeat_center
detector_fresh = (0 <= frame.at - frame.detector_at <= cfg.detector_age_s
and 0 <= now - frame.detector_at <= cfg.detector_age_s)
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)
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)
return ZoomDecision("size_reached", stop=True, margin=margin, gate=gate)
if now >= self.optics.active[1]:
return ZoomDecision("fov_cooldown", stop=True, margin=margin)
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(cfg.ff_gain * cfg.log_rate * math.sin(math.radians(a))
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)
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)
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)
return ZoomDecision("fov_cooldown", margin=margin, gate=gate,
edge_margin_required=required)
if not detector_fresh:
return ZoomDecision("waiting_yolo", margin=margin)
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)
if max(map(abs, frame.offset)) > center_limit or outward > cfg.max_outward_deg_s:
return ZoomDecision("not_centered", margin=margin)
return ZoomDecision("fast_start" if first else "burst", window=window, margin=margin)
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:
@ -439,10 +552,21 @@ def controller_for_track(st, camera_config):
return current
def transport_accepted(config, camera, st, request, native, put_at, ack_at, hard_end):
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,
@ -481,9 +605,24 @@ def transport_confirmed(config, st, request, at):
controller.stop_cooldowns = _cooldowns(st)
def transport_stopped(config, camera, st, request, at, *, reason, confirmed, clean):
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) or controller.request != request:
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)
@ -498,15 +637,19 @@ _telemetry_lock = Lock()
def _telemetry(config, camera, controller, event, at, **fields):
directory = getattr(config, "PREDICTIVE_ZOOM_TELEMETRY_DIR", None)
if directory is None:
_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, controller.owner, event, at, **fields, dropped_before=log.dropped)
log.submit(camera, owner, event, at, **fields, dropped_before=log.dropped)
def telemetry_tick(config, camera, controller, frame, decision, zoom, legacy_reason, at):
@ -521,4 +664,130 @@ def telemetry_tick(config, camera, controller, frame, decision, zoom, legacy_rea
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)
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)

Loading…
Cancel
Save