Log zoom framing and size controller thresholds
parent
fbf8017665
commit
273fb58c31
@ -1,3 +1,4 @@
|
|||||||
integration/ptz_zoom_zero_decision/server.patch text eol=lf -whitespace
|
integration/ptz_zoom_zero_decision/server.patch text eol=lf -whitespace
|
||||||
integration/ptz_native_floor_continuation/server.patch text eol=lf -whitespace
|
integration/ptz_native_floor_continuation/server.patch text eol=lf -whitespace
|
||||||
integration/ptz_same_native_diagnostics/server.patch text eol=lf -whitespace
|
integration/ptz_same_native_diagnostics/server.patch text eol=lf -whitespace
|
||||||
|
integration/ptz_zoom_threshold_diagnostics/server.patch text eol=lf -whitespace
|
||||||
|
|||||||
@ -0,0 +1,12 @@
|
|||||||
|
{
|
||||||
|
"requires": "PR #6",
|
||||||
|
"files": {
|
||||||
|
"ptz_tracker_modular/postprocess.py": {
|
||||||
|
"before": "d85439504ebf56fc39ee8a883c44b1bfc6db721df938aedc85d6c6484cb97776",
|
||||||
|
"after": "776b5b337e5f898e69770fec517c4459560f4cc6adeb951af40ac8f0b2178534"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"unchanged": {
|
||||||
|
"ptz_tracker_modular/ptz_io.py": "13d31539193b1a773a5d1db2187cc2a0c8db67d0aacaad89082face64df07499"
|
||||||
|
}
|
||||||
|
}
|
||||||
@ -0,0 +1,314 @@
|
|||||||
|
--- a/ptz_tracker_modular/postprocess.py
|
||||||
|
+++ b/ptz_tracker_modular/postprocess.py
|
||||||
|
@@ -2,6 +2,7 @@
|
||||||
|
import time
|
||||||
|
import math
|
||||||
|
import logging
|
||||||
|
+import json
|
||||||
|
from typing import Any, List, Tuple
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
@@ -454,6 +455,7 @@
|
||||||
|
hfov_deg: float,
|
||||||
|
vfov_deg: float,
|
||||||
|
max_error_deg: float | None = None,
|
||||||
|
+ rejection: dict | None = None,
|
||||||
|
) -> bool:
|
||||||
|
"""Allow a lens pulse only when angular steering reserve is available."""
|
||||||
|
max_error = max(
|
||||||
|
@@ -486,7 +488,7 @@
|
||||||
|
float(tilt_error_deg) * float(tilt_rate_deg_s) > 0.0
|
||||||
|
and abs(float(tilt_rate_deg_s)) > max_outward
|
||||||
|
)
|
||||||
|
- return bool(
|
||||||
|
+ allowed = bool(
|
||||||
|
not pan_escaping
|
||||||
|
and not tilt_escaping
|
||||||
|
and abs(float(pan_error_deg)) <= pan_limit
|
||||||
|
@@ -494,6 +496,23 @@
|
||||||
|
and abs(predicted_pan) <= pan_limit
|
||||||
|
and abs(predicted_tilt) <= tilt_limit
|
||||||
|
)
|
||||||
|
+ if not allowed and rejection is not None:
|
||||||
|
+ rejection.update(
|
||||||
|
+ failed=[name for name, failed in (
|
||||||
|
+ ("pan_outward_rate", pan_escaping),
|
||||||
|
+ ("tilt_outward_rate", tilt_escaping),
|
||||||
|
+ ("pan_error", not abs(float(pan_error_deg)) <= pan_limit),
|
||||||
|
+ ("tilt_error", not abs(float(tilt_error_deg)) <= tilt_limit),
|
||||||
|
+ ("predicted_pan", not abs(predicted_pan) <= pan_limit),
|
||||||
|
+ ("predicted_tilt", not abs(predicted_tilt) <= tilt_limit),
|
||||||
|
+ ) if failed],
|
||||||
|
+ pan_error_deg=pan_error_deg, tilt_error_deg=tilt_error_deg,
|
||||||
|
+ pan_rate_deg_s=pan_rate_deg_s, tilt_rate_deg_s=tilt_rate_deg_s,
|
||||||
|
+ predicted_pan_deg=predicted_pan, predicted_tilt_deg=predicted_tilt,
|
||||||
|
+ pan_limit_deg=pan_limit, tilt_limit_deg=tilt_limit,
|
||||||
|
+ outward_rate_limit_deg_s=max_outward, prediction_s=horizon,
|
||||||
|
+ )
|
||||||
|
+ return allowed
|
||||||
|
|
||||||
|
|
||||||
|
def _zoom_probe_used_for_current_track(st: dict) -> bool:
|
||||||
|
@@ -4877,12 +4896,21 @@
|
||||||
|
return min(cap, max(command, minimum)), "useful"
|
||||||
|
|
||||||
|
|
||||||
|
+def _zoom_response_fallback(
|
||||||
|
+ timing: Tuple[float, float], rejection: dict | None, failed: str, **values,
|
||||||
|
+) -> Tuple[float, float]:
|
||||||
|
+ if rejection is not None and timing == (0.0, 0.0):
|
||||||
|
+ rejection.update(check=failed, **values)
|
||||||
|
+ return timing
|
||||||
|
+
|
||||||
|
+
|
||||||
|
def _select_zoom_response_timing(
|
||||||
|
st: dict, *, useful: bool, hfov_deg: float, wide_hfov_deg: float,
|
||||||
|
center_err: float, margin: float, image_rate: float,
|
||||||
|
approach_margin: float | None = None,
|
||||||
|
fast_approach: bool = False,
|
||||||
|
requested_native: int | None = None,
|
||||||
|
+ rejection: dict | None = None,
|
||||||
|
) -> Tuple[float, float]:
|
||||||
|
"""Explore at wide angle; reuse optically proven timing after narrowing.
|
||||||
|
|
||||||
|
@@ -4897,10 +4925,13 @@
|
||||||
|
"""
|
||||||
|
if requested_native is not None:
|
||||||
|
if st.get("zoom_no_effect_blocked", False):
|
||||||
|
- return 0.0, 0.0
|
||||||
|
+ return _zoom_response_fallback((0.0, 0.0), rejection, "no_effect_limit")
|
||||||
|
native = int(requested_native)
|
||||||
|
if not _zoom_native_response_available(st, native, hfov_deg, wide_hfov_deg):
|
||||||
|
- return 0.0, 0.0
|
||||||
|
+ return _zoom_response_fallback(
|
||||||
|
+ (0.0, 0.0), rejection, "native_profile",
|
||||||
|
+ native=native, hfov_deg=hfov_deg, wide_hfov_deg=wide_hfov_deg,
|
||||||
|
+ )
|
||||||
|
proof_native = int(st.get("zoom_response_native_cmd", 0) or 0)
|
||||||
|
retry_native = int(st.get("zoom_response_retry_native_cmd", 0) or 0)
|
||||||
|
legacy_proof10 = native == 10 and proof_native == 0
|
||||||
|
@@ -4923,9 +4954,9 @@
|
||||||
|
try:
|
||||||
|
learned = float(st.get("zoom_response_effective_sec", 0.0) or 0.0)
|
||||||
|
if not math.isfinite(learned):
|
||||||
|
- return 0.0, 0.0
|
||||||
|
+ return _zoom_response_fallback((0.0, 0.0), rejection, "invalid_learned_duration")
|
||||||
|
except (TypeError, ValueError, OverflowError):
|
||||||
|
- return 0.0, 0.0
|
||||||
|
+ return _zoom_response_fallback((0.0, 0.0), rejection, "invalid_learned_duration")
|
||||||
|
fallback = (0.0, 0.0) if learned > base else (base, hard)
|
||||||
|
try:
|
||||||
|
hfov, wide, error, edge, rate = map(float, (hfov_deg, wide_hfov_deg, center_err, margin, image_rate))
|
||||||
|
@@ -4936,14 +4967,20 @@
|
||||||
|
video_lag = float(getattr(config, "AIM_GIMBAL_FF_VIDEO_LATENCY_SEC", 0.45))
|
||||||
|
stop_timeout = float(getattr(config, "PTZ_LENS_STOP_DIRECT_TIMEOUT_SEC", 0.75))
|
||||||
|
if not all(math.isfinite(v) for v in (base, hard, hfov, wide, error, edge, rate, extra, max_pulse, max_hard, ratio, learned, video_lag, stop_timeout)):
|
||||||
|
- return fallback
|
||||||
|
+ return _zoom_response_fallback(fallback, rejection, "invalid_timing_inputs")
|
||||||
|
max_pulse, max_hard, ratio = min(0.34, max_pulse), min(0.40, max_hard), max(0.85, ratio)
|
||||||
|
video_lag, stop_timeout = max(0.0, video_lag), max(0.0, stop_timeout)
|
||||||
|
approach = approach_margin is not None and math.isfinite(approach_margin) and .16 <= approach_margin <= .5
|
||||||
|
+ center_limit = .12 if approach else .065
|
||||||
|
if not (0.0 < hfov <= wide < 180.0
|
||||||
|
- and 0.0 <= error <= (.12 if approach else .065)
|
||||||
|
+ and 0.0 <= error <= center_limit
|
||||||
|
and edge >= 0.30 and 0.0 <= rate <= 0.04):
|
||||||
|
- return fallback
|
||||||
|
+ return _zoom_response_fallback(
|
||||||
|
+ fallback, rejection, "framing",
|
||||||
|
+ hfov_deg=hfov, wide_hfov_deg=wide, center_error=error, center_limit=center_limit,
|
||||||
|
+ margin=edge, margin_min=.30, image_rate=rate, image_rate_max=.04,
|
||||||
|
+ approach_margin=approach_margin,
|
||||||
|
+ )
|
||||||
|
attempts = min(2, max(0, int(st.get("zoom_no_effect_count", 0) or 0)))
|
||||||
|
if hfov >= wide * ratio:
|
||||||
|
pulse = min(max_pulse, max(base + attempts * max(0.0, extra), learned))
|
||||||
|
@@ -4951,7 +4988,13 @@
|
||||||
|
if not (base < learned <= 0.34 and (
|
||||||
|
approach or (error <= 0.04 and edge >= 0.40 and rate <= 0.02)
|
||||||
|
)):
|
||||||
|
- return fallback
|
||||||
|
+ return _zoom_response_fallback(
|
||||||
|
+ fallback, rejection, "narrow_fov",
|
||||||
|
+ hfov_deg=hfov, wide_cutoff_deg=wide * ratio, learned_s=learned,
|
||||||
|
+ base_s=base, learned_max_s=.34, approach=approach,
|
||||||
|
+ approach_margin=approach_margin, center_error=error, center_limit=.04,
|
||||||
|
+ margin=edge, margin_min=.40, image_rate=rate, image_rate_max=.02,
|
||||||
|
+ )
|
||||||
|
# A nominal duration learned at wide FOV may become ineffective
|
||||||
|
# after narrowing (or may have benefited from STOP transport lag).
|
||||||
|
# Repeating it until the no-effect latch fires is not adaptation.
|
||||||
|
@@ -4988,16 +5031,24 @@
|
||||||
|
# increase either configured hard ceiling or the compiled 400ms limit.
|
||||||
|
deadline = min(max_hard, max(hard, pulse + 0.06))
|
||||||
|
if pulse <= base or deadline < pulse or pulse + 1e-9 < learned:
|
||||||
|
- return fallback
|
||||||
|
+ return _zoom_response_fallback(
|
||||||
|
+ fallback, rejection, "duration",
|
||||||
|
+ pulse_s=pulse, base_s=base, hard_s=deadline, learned_s=learned,
|
||||||
|
+ )
|
||||||
|
# Stress the projection with 2x apparent offset/box/rate and include
|
||||||
|
# video lag + STOP network timeout. This is a conservative software
|
||||||
|
# envelope, not a measured bound on this particular lens's zoom rate.
|
||||||
|
# The ordinary enlarged-bbox predictor is also independently checked.
|
||||||
|
- if 2.0 * edge - 0.5 - 2.0 * rate * (deadline + video_lag + stop_timeout) < 0.10:
|
||||||
|
- return fallback
|
||||||
|
+ projected_margin = 2.0 * edge - 0.5 - 2.0 * rate * (deadline + video_lag + stop_timeout)
|
||||||
|
+ if projected_margin < 0.10:
|
||||||
|
+ return _zoom_response_fallback(
|
||||||
|
+ fallback, rejection, "projected_margin",
|
||||||
|
+ projected_margin=projected_margin, margin_min=.10, margin=edge,
|
||||||
|
+ image_rate=rate, hard_s=deadline, video_lag_s=video_lag, stop_timeout_s=stop_timeout,
|
||||||
|
+ )
|
||||||
|
return pulse, deadline
|
||||||
|
except (TypeError, ValueError, OverflowError):
|
||||||
|
- return fallback
|
||||||
|
+ return _zoom_response_fallback(fallback, rejection, "invalid_timing_inputs")
|
||||||
|
|
||||||
|
|
||||||
|
def _apply_zoom_in_minimum(
|
||||||
|
@@ -8516,7 +8567,9 @@
|
||||||
|
tilt_speed = 0.0
|
||||||
|
# === zoom logic (V3): target-size + alpha-beta filter + hysteresis ===
|
||||||
|
state_zoom = int(st.get("zoom_state", 0) or 0)
|
||||||
|
- can_switch = (now - float(st.get("zoom_last_change", 0.0) or 0.0)) >= float(getattr(config, "Z_MIN_SWITCH_INTERVAL", 0.50))
|
||||||
|
+ switch_age = now - float(st.get("zoom_last_change", 0.0) or 0.0)
|
||||||
|
+ switch_interval = float(getattr(config, "Z_MIN_SWITCH_INTERVAL", 0.50))
|
||||||
|
+ can_switch = switch_age >= switch_interval
|
||||||
|
|
||||||
|
gate_dyn = max(0.05, _Z_CENTER_GATE_IN + _Z_CENTER_GATE_SCALE * (1.0 - clipf(w_smooth, 0.0, 1.0)))
|
||||||
|
center_good = measured_center_err < gate_dyn
|
||||||
|
@@ -8766,6 +8819,7 @@
|
||||||
|
pan_rate_deg_s=float(los_pan_rate_deg_s),
|
||||||
|
tilt_rate_deg_s=float(los_tilt_rate_deg_s),
|
||||||
|
)
|
||||||
|
+ 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),
|
||||||
|
tilt_error_deg=float(los_tilt_angle_deg),
|
||||||
|
@@ -8774,6 +8828,7 @@
|
||||||
|
hfov_deg=float(hfov_deg),
|
||||||
|
vfov_deg=float(vfov_deg),
|
||||||
|
max_error_deg=float(angular_max),
|
||||||
|
+ rejection=angular_rejection,
|
||||||
|
)
|
||||||
|
if approach_margin is not None and followup_ready:
|
||||||
|
zoom_angular_safe = True
|
||||||
|
@@ -8961,6 +9016,8 @@
|
||||||
|
elif can_switch and int(st.get("zoom_v3_neutral_votes", 0) or 0) >= confirm_frames:
|
||||||
|
next_state = 0
|
||||||
|
|
||||||
|
+ size_votes = (st.get("zoom_v3_in_votes", 0), st.get("zoom_v3_out_votes", 0),
|
||||||
|
+ st.get("zoom_v3_neutral_votes", 0))
|
||||||
|
if next_state != state_zoom:
|
||||||
|
st["zoom_state"] = next_state
|
||||||
|
st["zoom_last_change"] = now
|
||||||
|
@@ -8968,19 +9025,21 @@
|
||||||
|
st["zoom_int"] = 0.0
|
||||||
|
|
||||||
|
err_size = z_target - size_ctrl
|
||||||
|
- if abs(err_size) < float(getattr(config, "Z_DEADBAND", 0.010)):
|
||||||
|
+ size_deadband = float(getattr(config, "Z_DEADBAND", 0.010))
|
||||||
|
+ if abs(err_size) < size_deadband:
|
||||||
|
err_size = 0.0
|
||||||
|
st["zoom_int"] = clipf(float(st.get("zoom_int", 0.0) or 0.0) + err_size * 0.04, -_Z_INT_CLAMP, _Z_INT_CLAMP)
|
||||||
|
zoom_pi = _Z_P * err_size + _Z_I * st["zoom_int"]
|
||||||
|
zoom_ff = _Z_FF_GAIN * (-float(size_rate))
|
||||||
|
- if size_err_abs <= float(getattr(config, "ZOOM_V3_BRAKE_ERR", 0.030)):
|
||||||
|
+ brake_err = float(getattr(config, "ZOOM_V3_BRAKE_ERR", 0.030))
|
||||||
|
+ if size_err_abs <= brake_err:
|
||||||
|
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)
|
||||||
|
zoom_cmd *= brake_k
|
||||||
|
+ zoom_before_size_state = zoom_cmd
|
||||||
|
|
||||||
|
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
|
||||||
|
@@ -9005,6 +9064,7 @@
|
||||||
|
if zoom_cmd < 0.0:
|
||||||
|
zoom_cmd = 0.0
|
||||||
|
|
||||||
|
+ zoom_after_size_state = zoom_cmd
|
||||||
|
prev_z = float(st.get("zoom_prev_cmd", 0.0) or 0.0)
|
||||||
|
max_step = float(getattr(config, "ZOOM_V3_CMD_MAX_STEP", 0.04))
|
||||||
|
positive_slew_limit = prev_z + max_step
|
||||||
|
@@ -9018,6 +9078,7 @@
|
||||||
|
and approach_margin is not None and .16 <= approach_margin <= .5):
|
||||||
|
positive_slew_limit = max(positive_slew_limit, min(.12, float(getattr(config, "ZOOM_STEP_MAX_IN_CMD", .12))))
|
||||||
|
zoom_cmd = clipf(zoom_cmd, prev_z - max_step, positive_slew_limit)
|
||||||
|
+ zoom_after_size_slew = zoom_cmd
|
||||||
|
if (prev_z * zoom_cmd) < 0.0 and abs(err_size) < (lock_err_thr * 2.0):
|
||||||
|
sign_cd = float(getattr(config, "ZOOM_V3_SIGN_COOLDOWN_SEC", 0.80))
|
||||||
|
last_flip = float(st.get("zoom_v3_last_sign_flip_t", 0.0) or 0.0)
|
||||||
|
@@ -9038,6 +9099,7 @@
|
||||||
|
zoom_zero_reason = "deadband"
|
||||||
|
zoom_cmd = 0.0
|
||||||
|
|
||||||
|
+ zoom_after_size_minimum = zoom_cmd
|
||||||
|
cmd_mag_est = max(
|
||||||
|
abs(float(pan_speed) + float(pan_rate_ff)),
|
||||||
|
abs(float(tilt_speed) + float(tilt_rate_ff)),
|
||||||
|
@@ -9225,6 +9287,8 @@
|
||||||
|
st["zoom_useful_pulse_reason"] = useful_reason
|
||||||
|
if before_shape > 0.0 and zoom_cmd <= 0.0:
|
||||||
|
zoom_zero_reason = useful_reason
|
||||||
|
+ response_rejection = {} if decision_request and useful_reason == "useful" else None
|
||||||
|
+ timing_native = _zoom_native_timing_request(st, zoom_cmd)
|
||||||
|
response_timing = _select_zoom_response_timing(
|
||||||
|
st, useful=useful_reason == "useful",
|
||||||
|
hfov_deg=float(hfov_deg), wide_hfov_deg=float(shared_wide_hfov),
|
||||||
|
@@ -9232,7 +9296,8 @@
|
||||||
|
margin=float(edge_risk_margin), image_rate=max(float(vel), float(fast_drift)),
|
||||||
|
approach_margin=approach_margin,
|
||||||
|
fast_approach=bool(fast_approach and normal_zoom_quality_ok and pulse_family == "normal"),
|
||||||
|
- requested_native=_zoom_native_timing_request(st, zoom_cmd),
|
||||||
|
+ requested_native=timing_native,
|
||||||
|
+ rejection=response_rejection,
|
||||||
|
)
|
||||||
|
if useful_reason == "useful" and response_timing == (0.0, 0.0):
|
||||||
|
zoom_cmd = 0.0
|
||||||
|
@@ -9259,6 +9324,7 @@
|
||||||
|
)
|
||||||
|
if gate_stop:
|
||||||
|
zoom_zero_reason = gate_stop[-1]
|
||||||
|
+ decision_generation = st.get("ptz_generation")
|
||||||
|
if (
|
||||||
|
probe_start_candidate
|
||||||
|
and zoom_cmd > 0.0
|
||||||
|
@@ -9797,6 +9863,32 @@
|
||||||
|
cam_idx, prepared_fps, visual_only, max(0., time.time()-now)*1000.,
|
||||||
|
st.get('control_updates', 0), st.get('control_visual_updates', 0))
|
||||||
|
ingest_detection(alarm_eligible)
|
||||||
|
+ if zoom_decision is not None and zoom_zero_reason in ("response_framing", "size_controller"):
|
||||||
|
+ if zoom_zero_reason == "response_framing":
|
||||||
|
+ detail = {"native": timing_native, **(response_rejection or {})}
|
||||||
|
+ else:
|
||||||
|
+ detail = dict(
|
||||||
|
+ size=size_ctrl, size_target=z_target, zoom_in_below=z_band_min - _Z_SIGN_HYST,
|
||||||
|
+ size_error=err_size, size_deadband=size_deadband, brake_error=brake_err,
|
||||||
|
+ pi=zoom_pi, feed_forward=zoom_ff, previous_state=state_zoom,
|
||||||
|
+ next_state=next_state, can_switch=can_switch, want_in=want_in, want_out=want_out,
|
||||||
|
+ in_votes=size_votes[0], out_votes=size_votes[1], neutral_votes=size_votes[2],
|
||||||
|
+ confirm_frames=confirm_frames, switch_age_s=switch_age, switch_interval_s=switch_interval,
|
||||||
|
+ can_zoom_in=can_zoom_in, followup_ready=followup_ready, angular_safe=zoom_angular_safe,
|
||||||
|
+ angular=angular_rejection,
|
||||||
|
+ before_state=zoom_before_size_state, after_state=zoom_after_size_state,
|
||||||
|
+ previous_zoom=prev_z, slew_min=prev_z - max_step, slew_max=positive_slew_limit,
|
||||||
|
+ after_slew=zoom_after_size_slew, command_min=_Z_MIN_CMD,
|
||||||
|
+ after_minimum=zoom_after_size_minimum, before_recenter=before_recenter,
|
||||||
|
+ before_ramp=pre_ramp_zoom_cmd, before_shape=before_shape,
|
||||||
|
+ center_error=center_err, center_priority_error=center_prio_err,
|
||||||
|
+ margin=min_margin, center_priority_margin=center_prio_margin,
|
||||||
|
+ )
|
||||||
|
+ logger.info(
|
||||||
|
+ "[ZOOM THRESHOLD] cam=%s frame=%s request=%s gen=%s reason=%s detail=%s",
|
||||||
|
+ cam_idx, source_seq, decision_request, decision_generation, zoom_zero_reason,
|
||||||
|
+ json.dumps(detail, separators=(",", ":"), default=str),
|
||||||
|
+ )
|
||||||
|
if continuation_trace:
|
||||||
|
generation, native, proposed, held, until, pulse_sec, hard_sec = continuation_trace[0]
|
||||||
|
logger.info(
|
||||||
Loading…
Reference in New Issue