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

407 lines
17 KiB
Diff

--- a/ptz_tracker_modular/postprocess.py
+++ b/ptz_tracker_modular/postprocess.py
@@ -4357,6 +4357,7 @@
max_image_rate: float,
max_outward_rate: float,
err_growth_tol: float,
+ settled_sample: list[bool] | None = None,
) -> bool:
"""Return True only after a tight, non-diverging image-space hold.
@@ -4381,6 +4382,8 @@
and outward_rate <= max(0.0, float(max_outward_rate))
and non_diverging
)
+ if settled_sample is not None:
+ settled_sample.append(bool(settled))
previous_frames = int(st.get(frames_key, 0) or 0)
if settled:
frames = previous_frames + 1
@@ -4411,6 +4414,7 @@
vx: float,
vy: float,
center_gate: float | None = None,
+ settled_sample: list[bool] | None = None,
) -> bool:
return _update_center_lock_state(
st,
@@ -4436,6 +4440,7 @@
max_image_rate=float(getattr(config, "ZOOM_CENTER_LOCK_MAX_IMAGE_RATE", 0.28)),
max_outward_rate=float(getattr(config, "ZOOM_CENTER_LOCK_MAX_OUTWARD_RATE", 0.08)),
err_growth_tol=float(getattr(config, "ZOOM_CENTER_LOCK_ERR_GROWTH_TOL", 0.018)),
+ settled_sample=settled_sample,
)
@@ -4553,6 +4558,12 @@
st["acquire_zoom_prev_err"] = None
+def _reject_zoom_step(reason: str, trace: list[str] | None) -> float:
+ if trace is not None:
+ trace.append(reason)
+ return 0.0
+
+
def _gate_zoom_in_pulse(
st: dict,
now: float,
@@ -4565,6 +4576,7 @@
pulse_floor: float = 0.0,
pulse_timing: Tuple[float, float] | None = None,
optics_observed_now: float | None = None,
+ stop_trace: list[str] | None = None,
) -> float:
"""Break continuous zoom-in into short feedback-controlled steps."""
pulse_until = float(st.get("zoom_step_until", 0.0) or 0.0)
@@ -4581,15 +4593,15 @@
return float(zoom_cmd if can_zoom_in else 0.0)
if bool(st.get("zoom_no_effect_blocked", False)):
st["zoom_followup_reason"] = "no_effect_limit"
- return 0.0
+ return _reject_zoom_step("no_effect_limit", stop_trace)
if not can_zoom_in:
if pulse_until > 0.0 or bool(st.get("zoom_step_pending", False)):
_finish_zoom_step(st, now, stable_key=stable_key)
- return 0.0
+ return _reject_zoom_step("permission", stop_trace)
if bool(st.get("zoom_step_pending", False)):
if bool(st.get("zoom_step_cancel_requested", False)):
- return 0.0
+ return _reject_zoom_step("cancel_requested", stop_trace)
if pulse_timing is not None:
try:
authorized = tuple(map(float, pulse_timing))
@@ -4602,7 +4614,7 @@
timing_ok = False
if not timing_ok:
_finish_zoom_step(st, now, stable_key=stable_key)
- return 0.0
+ return _reject_zoom_step("pending_timing", stop_trace)
useful_cmd = float(st.get("zoom_step_pending_useful_cmd", 0.0) or 0.0)
if useful_cmd > 0.0:
# A useful-pulse caller revalidates its entire framing envelope on
@@ -4614,7 +4626,7 @@
or float(getattr(config, "ZOOM_STEP_MAX_IN_CMD", 0.12)) < useful_cmd
):
_finish_zoom_step(st, now, stable_key=stable_key)
- return 0.0
+ return _reject_zoom_step("pending_native_floor", stop_trace)
zoom_cmd = useful_cmd
physical_started = float(st.get("zoom_step_physical_started_at", 0.0) or 0.0)
if physical_started > 0.0:
@@ -4624,7 +4636,7 @@
float(getattr(config, "ZOOM_STEP_MAX_IN_CMD", 0.18)),
)
_finish_zoom_step(st, now, stable_key=stable_key)
- return 0.0
+ return _reject_zoom_step("pulse_window_ended", stop_trace)
expires_at = float(st.get("zoom_step_pending_expires_at", 0.0) or 0.0)
if expires_at > 0.0 and float(now) >= expires_at:
_clear_pending_zoom_request(
@@ -4633,7 +4645,7 @@
reason="delivery_timeout",
retry_cooldown=True,
)
- return 0.0
+ return _reject_zoom_step("delivery_timeout", stop_trace)
# Keep authorizing the same request until the worker either delivers it
# or a newer safety command cancels it. No counters are consumed here.
return min(
@@ -8614,6 +8626,7 @@
)
if approach_margin is not None:
lock_gate = max(lock_gate, .12)
+ center_sample = []
center_locked_for_zoom = _update_zoom_center_lock(
st,
now,
@@ -8625,6 +8638,7 @@
vx=float(zoom_vx),
vy=float(zoom_vy),
center_gate=lock_gate,
+ settled_sample=center_sample,
)
use_zoom_v3 = bool(getattr(config, "ZOOM_V3_ENABLE", True))
@@ -8867,6 +8881,9 @@
and now >= float(st.get("focus_settle_until", 0.0) or 0.0)
)
+ center_settled = center_sample[0] if center_sample else None
+ track_stable = int(st.get("stable_frames", 0) or 0) >= min_stable
+
# A safe probe is an explicit bounded identification request, not merely
# permission for the ordinary size controller. Without this request a
# tiny distant aircraft could pass every probe gate yet never zoom because
@@ -8929,6 +8946,7 @@
if size_err_abs <= float(getattr(config, "ZOOM_V3_BRAKE_ERR", 0.030)):
zoom_ff = 0.0
zoom_cmd = clipf(zoom_pi + zoom_ff, _Z_SPEED_LIMIT_OUT, _Z_SPEED_LIMIT_IN)
+ zoom_zero_reason = "size_controller"
if not out_edge_crit:
brake_err = float(getattr(config, "ZOOM_V3_BRAKE_ERR", 0.030))
brake_k = clipf(abs(err_size) / max(1e-6, brake_err), 0.0, 1.0)
@@ -8936,6 +8954,7 @@
if st.get("zoom_lock", False) and not bounded_identification_request:
zoom_cmd = min(zoom_cmd, -0.12) if out_edge_effective else 0.0
+ zoom_zero_reason = "zoom_lock"
st["zoom_int"] *= 0.90
else:
if st["zoom_state"] > 0:
@@ -8947,6 +8966,7 @@
if normal_size_hold and not out_edge_effective:
st["zoom_state"] = 0
zoom_cmd = 0.0
+ zoom_zero_reason = "size_hold"
st["zoom_int"] *= 0.70
st["zoom_size_rate"] = float(st.get("zoom_size_rate", 0.0) or 0.0) * 0.6
if cruise_active and (not approaching) and (not out_edge_effective) and (not near_react_edge):
@@ -8973,6 +8993,7 @@
last_flip = float(st.get("zoom_v3_last_sign_flip_t", 0.0) or 0.0)
if (now - last_flip) < sign_cd:
zoom_cmd = 0.0
+ zoom_zero_reason = "sign_cooldown"
else:
st["zoom_v3_last_sign_flip_t"] = now
@@ -8983,6 +9004,8 @@
elif z_state < 0:
zoom_cmd = -_Z_MIN_CMD
else:
+ if zoom_cmd != 0.0:
+ zoom_zero_reason = "deadband"
zoom_cmd = 0.0
cmd_mag_est = max(
@@ -8993,10 +9016,14 @@
gate_v = float(getattr(config, "Z_VELOCITY_GATE", 0.45))
over_v = max(0.0, cmd_mag_est - gate_v)
zoom_cmd *= clipf(1.0 - over_v / max(1e-6, gate_v * 1.2), 0.0, 1.0)
+ if zoom_cmd == 0.0:
+ zoom_zero_reason = "steering_speed"
err_gate = float(getattr(config, "Z_ERR_GATE", 0.18))
if (max(abs(sm_dx), abs(sm_dy)) > err_gate) and (zoom_cmd > 0.0):
over_e = max(0.0, max(abs(sm_dx), abs(sm_dy)) - err_gate)
zoom_cmd *= clipf(1.0 - over_e / max(1e-6, err_gate * 1.5), 0.0, 1.0)
+ if zoom_cmd == 0.0:
+ zoom_zero_reason = "center_error"
# Center-first policy: when target is noticeably off-center, prioritize pan/tilt over zoom-in.
center_prio_err = float(getattr(config, "ZOOM_CENTER_PRIORITY_ERR", 0.11))
center_prio_margin = float(getattr(config, "ZOOM_CENTER_PRIORITY_MARGIN", 0.14))
@@ -9007,19 +9034,26 @@
zoom_cmd *= clipf(min_margin / max(1e-6, center_prio_margin), 0.0, 1.0)
if near_edge and zoom_cmd > 0.0:
zoom_cmd = 0.0
+ zoom_zero_reason = "near_edge"
if zoom_cmd > 0.0:
vel_ref = float(getattr(config, "ZOOM_IN_SCALE_VEL_REF", 0.22))
vel_k = clipf(1.0 - vel / max(1e-6, vel_ref), float(getattr(config, "ZOOM_IN_SCALE_MIN", 0.20)), 1.0)
zoom_cmd *= min(zoom_safe_scale, vel_k)
-
+ if zoom_cmd == 0.0:
+ zoom_zero_reason = "framing_scale"
+
+ before_edge = zoom_cmd
zoom_cmd = _apply_predictive_edge_zoom_out(
zoom_cmd,
risk_margin=edge_risk_margin,
react_margin=react_margin,
moving_out=moving_out,
)
+ if before_edge > 0.0 and zoom_cmd <= 0.0:
+ zoom_zero_reason = "predictive_edge"
recent_zoom_steering = _recent_zoom_steering_active(st, now, zoom_cmd)
+ before_recenter = zoom_cmd
zoom_cmd = _apply_zoom_recenter_guard(
zoom_cmd,
center_err=float(center_err),
@@ -9027,6 +9061,8 @@
recent_zoom=bool(recent_zoom_steering),
approach_margin=approach_margin,
)
+ if before_recenter > 0.0 and zoom_cmd <= 0.0:
+ zoom_zero_reason = "recenter"
zoom_out_emergency = bool(
out_edge_effective
or float(min_margin) < float(getattr(config, "Z_SAFETY_EDGE", 0.05))
@@ -9114,6 +9150,8 @@
request_id_before_gate = int(st.get("zoom_step_request_id", 0) or 0)
with _zoom_command_lock(cam_idx):
zoom_gate_now = time.time()
+ decision_request = int(st.get("zoom_step_physical_request_id", 0) or
+ st.get("zoom_step_pending_id", 0) or 0)
pulse_family = str(st.get("zoom_step_pending_kind") or pulse_kind)
pulse_cap = float(getattr(config, "ZOOM_STEP_MAX_IN_CMD", 0.12))
if "probe" in pulse_family:
@@ -9142,6 +9180,7 @@
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
+ before_shape = zoom_cmd
zoom_cmd, useful_reason = _shape_useful_zoom_pulse(
zoom_cmd,
can_zoom_in=can_zoom_in,
@@ -9154,6 +9193,8 @@
approach_margin=approach_margin,
)
st["zoom_useful_pulse_reason"] = useful_reason
+ if before_shape > 0.0 and zoom_cmd <= 0.0:
+ zoom_zero_reason = useful_reason
response_timing = _select_zoom_response_timing(
st, useful=useful_reason == "useful",
hfov_deg=float(hfov_deg), wide_hfov_deg=float(shared_wide_hfov),
@@ -9166,6 +9207,8 @@
if useful_reason == "useful" and response_timing == (0.0, 0.0):
zoom_cmd = 0.0
st["zoom_useful_pulse_reason"] = "wait_response_framing"
+ zoom_zero_reason = "response_framing"
+ gate_stop = [] if decision_request else None
zoom_cmd = _gate_zoom_in_pulse(
st,
# Keep new-request expiry tied to the frame that authorized
@@ -9180,7 +9223,10 @@
pulse_floor=(zoom_cmd if useful_reason == "useful" else 0.0),
pulse_timing=response_timing,
optics_observed_now=zoom_gate_now,
+ stop_trace=gate_stop,
)
+ if gate_stop:
+ zoom_zero_reason = gate_stop[-1]
if (
probe_start_candidate
and zoom_cmd > 0.0
@@ -9433,6 +9479,7 @@
or visual_control_coast != was_visual_control_coast
)
if visual_control_coast:
+ before_coast = zoom_cmd
adj_pan, adj_tilt, zoom_cmd = _bound_visual_coast_commands(
st,
adj_pan,
@@ -9463,7 +9510,11 @@
# Appearance continuity is sufficient for bounded steering only.
# It never authorizes an alarm without a current detector match.
alarm_eligible = False
+ if before_coast > 0.0 and zoom_cmd <= 0.0:
+ zoom_zero_reason = "visual_coast"
elif visual_control_stale:
+ if zoom_cmd > 0.0:
+ zoom_zero_reason = "visual_stale"
# Stale detector validation makes image-space steering unsafe, but a
# previously calculated edge-recovery zoom-out only widens the field
# of view. Preserve that bounded negative command while cancelling
@@ -9532,6 +9583,7 @@
# immediately by the forced command below.
if float(zoom_cmd) > 0.0:
zoom_cmd = 0.0
+ zoom_zero_reason = "final_brake"
st["zoom_prev_cmd"] = 0.0
st["stable_frames"] = 0
@@ -9673,7 +9725,33 @@
_arm_zoom_revalidation_locked(st, time.time(), 'visual_control_expired')
stop_ptz(cam_idx, source='stop', generation=int(st.get('ptz_generation', 0) or 0))
return
- _dispatch_owned_zoom_tracking(cam_idx, _PAN_SIGN * adj_pan, _TILT_SIGN * adj_tilt, zoom_cmd, force=force_track_ptz)
+ zoom_decision = None
+ if decision_request and zoom_cmd <= 0.0:
+ blockers = tuple(name for name, blocked in (
+ ("quality", not zoom_quality_ok),
+ ("framing", not zoom_safe),
+ ("followup", not followup_ready),
+ ("angle", not zoom_angular_safe),
+ ("center_sample_unknown", center_settled is None),
+ ("center_sample", center_settled is False),
+ ("center_history", center_settled is True and not center_locked_for_zoom),
+ ("track_history", not track_stable),
+ ("rearm", not zoom_rearm_ready),
+ ("edge", near_edge or edge_recovery_active),
+ ("lock", st.get("zoom_lock", False) and not bounded_identification_request),
+ ("drift", fast_drift > float(getattr(config, "ZOOM_IN_FAST_DRIFT_MAX", 0.45))),
+ ("focus", now < float(st.get("focus_settle_until", 0.0) or 0.0)),
+ ("coast", visual_control_coast),
+ ("stale", visual_control_stale),
+ ("brake", final_guard_active),
+ ) if blocked)
+ zoom_decision = (source_seq, decision_request, zoom_zero_reason, "+".join(blockers) or "-")
+ 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 visual_control.enabled():
dispatch_at = time.monotonic()
samples = st.setdefault('control_dispatch_samples', [])
--- a/ptz_tracker_modular/ptz_io.py
+++ b/ptz_tracker_modular/ptz_io.py
@@ -1903,10 +1903,11 @@
source: Optional[str] = None,
generation: Optional[int] = None,
preset_floor_checked: bool = False,
+ zoom_decision: Optional[Tuple[Optional[int], int, str, str]] = None,
) -> None:
"""Serialize frame commands with the independent lens hard-stop timer."""
with _zoom_command_lock(cam_idx):
- _move_ptz_locked(
+ queued = _move_ptz_locked(
cam_idx,
pan,
tilt,
@@ -1915,6 +1916,15 @@
source=source,
generation=generation,
preset_floor_checked=preset_floor_checked,
+ zoom_decision=zoom_decision,
+ )
+ if zoom_decision is not None and queued is not None:
+ cmd_source, generation, seq, native_zoom = queued
+ frame, request, reason, blockers = zoom_decision
+ logger.info(
+ "[ZOOM DECISION] cam=%s frame=%s request=%s source=%s gen=%s seq=%s "
+ "queued_zoom=%s reason=%s blockers=%s",
+ cam_idx, frame, request, cmd_source, generation, seq, native_zoom, reason, blockers,
)
@@ -1929,7 +1939,8 @@
generation: Optional[int] = None,
preset_floor_checked: bool = False,
hard_stop: bool = False,
-) -> None:
+ zoom_decision: Optional[Tuple[Optional[int], int, str, str]] = None,
+) -> Optional[Tuple[str, int, int, int]]:
"""
Отправка relative/continuous PTZ-команды.
@@ -2105,6 +2116,9 @@
# The MOVE is already queued. Report watchdog setup separately;
# pretending that the MOVE was dropped would corrupt dedupe state.
logger.error("[ZOOM WATCHDOG] setup failed cam=%s: %s", cam_idx, exc)
+ if zoom_decision is not None:
+ return cmd_source, cmd_generation, move_seq, zoom_i
+ return None
def stop_ptz(cam_idx: int, *, source: str = "stop", generation: Optional[int] = None) -> None:
@@ -2253,6 +2267,7 @@
force: bool = False,
source: Optional[str] = None,
generation: Optional[int] = None,
+ zoom_decision: Optional[Tuple[Optional[int], int, str, str]] = None,
) -> None:
"""
Обёртка над move_ptz.
@@ -2295,6 +2310,7 @@
source=source,
generation=generation,
preset_floor_checked=True,
+ zoom_decision=zoom_decision,
)