--- 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, )