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.
141 lines
6.7 KiB
Diff
141 lines
6.7 KiB
Diff
--- a/postprocess.py
|
|
+++ b/postprocess.py
|
|
@@ -761,6 +761,7 @@
|
|
edge_margin_min: float = 0.30
|
|
center_max_norm: float = 0.12
|
|
image_rate_max_norm_s: float = 0.04
|
|
+ image_rate_min_interval_s: float = 0.04
|
|
|
|
|
|
_PTZ17_ZOOM = _PTZ17ZoomTuning()
|
|
@@ -3320,6 +3321,55 @@
|
|
# TRACK/ACQUIRE boundary.
|
|
|
|
|
|
+def _update_image_rate_ema(st: dict, dx_norm: float, dy_norm: float, dt_s: float) -> None:
|
|
+ """Update X-right/Y-down rates in full frame fractions per second."""
|
|
+ image_rate_max = max(0.1, float(getattr(config, "AIM_IMAGE_RATE_MAX", 2.0)))
|
|
+ vx_i = clipf(dx_norm / dt_s, -image_rate_max, image_rate_max)
|
|
+ vy_i = clipf(dy_norm / dt_s, -image_rate_max, image_rate_max)
|
|
+ vel_smooth = clipf(float(getattr(config, "VEL_SMOOTH", 0.85)), 0.0, 0.99)
|
|
+ st["vx"] = vel_smooth * st["vx"] + (1.0 - vel_smooth) * vx_i
|
|
+ st["vy"] = vel_smooth * st["vy"] + (1.0 - vel_smooth) * vy_i
|
|
+ fast_smooth = clipf(float(getattr(config, "AIM_IMAGE_RATE_FAST_SMOOTH", 0.45)), 0.0, 0.95)
|
|
+ st["vx_fast"] = fast_smooth * float(st.get("vx_fast", 0.0) or 0.0) + (1.0 - fast_smooth) * vx_i
|
|
+ st["vy_fast"] = fast_smooth * float(st.get("vy_fast", 0.0) or 0.0) + (1.0 - fast_smooth) * vy_i
|
|
+
|
|
+
|
|
+@dataclass(frozen=True, slots=True)
|
|
+class _ImageRateSample:
|
|
+ latest_at_s: float
|
|
+ baseline_at_s: float
|
|
+ x_norm: float
|
|
+ y_norm: float
|
|
+ have_rate: bool
|
|
+
|
|
+
|
|
+def _update_ptz17_image_rate(st: dict, measured_at_s: float, x_norm: float, y_norm: float) -> str:
|
|
+ """Use a causal baseline and accumulate short samples without inventing elapsed time."""
|
|
+ if not all(map(math.isfinite, (measured_at_s, x_norm, y_norm))):
|
|
+ return "invalid"
|
|
+ previous = st.get("ptz17_image_rate_ref")
|
|
+ if previous is None:
|
|
+ st["ptz17_image_rate_ref"] = _ImageRateSample(measured_at_s, measured_at_s, x_norm, y_norm, False)
|
|
+ return "seed"
|
|
+ if measured_at_s <= previous.latest_at_s:
|
|
+ return "noncausal"
|
|
+ dt_s = measured_at_s - previous.baseline_at_s
|
|
+ dx_norm, dy_norm = x_norm - previous.x_norm, y_norm - previous.y_norm
|
|
+ if dt_s < _PTZ17_ZOOM.image_rate_min_interval_s:
|
|
+ # Advance the ordering barrier without moving the derivative baseline.
|
|
+ st["ptz17_image_rate_ref"] = _ImageRateSample(
|
|
+ measured_at_s, previous.baseline_at_s, previous.x_norm, previous.y_norm, previous.have_rate,
|
|
+ )
|
|
+ # Use the prior rate only if the whole displacement fits the motion gate.
|
|
+ rate_max = min(0.08, float(getattr(config, "ZOOM_STEP_USEFUL_IMAGE_RATE_MAX", 0.08)))
|
|
+ if previous.have_rate and abs(dx_norm) + abs(dy_norm) <= rate_max * dt_s:
|
|
+ return "short_checked"
|
|
+ return "short"
|
|
+ _update_image_rate_ema(st, dx_norm, dy_norm, dt_s)
|
|
+ st["ptz17_image_rate_ref"] = _ImageRateSample(measured_at_s, measured_at_s, x_norm, y_norm, True)
|
|
+ return "updated"
|
|
+
|
|
+
|
|
def _reset_aim_motion_feedforward(st: dict, now: float) -> None:
|
|
"""Clear stale target-motion estimates without disturbing TRACK/lens state.
|
|
|
|
@@ -3333,6 +3383,7 @@
|
|
its feedback barrier instead of erasing the lens transaction.
|
|
"""
|
|
st["prev_t"] = None
|
|
+ st.pop("ptz17_image_rate_ref", None)
|
|
st["prev_cx"] = 0.5
|
|
st["prev_cy"] = 0.5
|
|
st["vx"] = 0.0
|
|
@@ -3416,6 +3467,7 @@
|
|
) -> None:
|
|
"""Discard motion history that must not cross a new TRACK boundary."""
|
|
st["prev_t"] = None
|
|
+ st.pop("ptz17_image_rate_ref", None)
|
|
st["prev_cx"] = 0.5
|
|
st["prev_cy"] = 0.5
|
|
st["vx"] = 0.0
|
|
@@ -8907,18 +8959,12 @@
|
|
min_margin = min(left, right, top, bottom)
|
|
|
|
prev_t = st.get("prev_t")
|
|
- dt = None
|
|
- if prev_t is not None:
|
|
- dt = max(now - float(prev_t), 1e-3)
|
|
- image_rate_max = max(0.1, float(getattr(config, "AIM_IMAGE_RATE_MAX", 2.0)))
|
|
- vx_i = clipf((cxn - st["prev_cx"]) / dt, -image_rate_max, image_rate_max)
|
|
- vy_i = clipf((cyn - st["prev_cy"]) / dt, -image_rate_max, image_rate_max)
|
|
- vel_smooth = clipf(float(getattr(config, "VEL_SMOOTH", 0.85)), 0.0, 0.99)
|
|
- st["vx"] = vel_smooth * st["vx"] + (1.0 - vel_smooth) * vx_i
|
|
- st["vy"] = vel_smooth * st["vy"] + (1.0 - vel_smooth) * vy_i
|
|
- fast_smooth = clipf(float(getattr(config, "AIM_IMAGE_RATE_FAST_SMOOTH", 0.45)), 0.0, 0.95)
|
|
- st["vx_fast"] = fast_smooth * float(st.get("vx_fast", 0.0) or 0.0) + (1.0 - fast_smooth) * vx_i
|
|
- st["vy_fast"] = fast_smooth * float(st.get("vy_fast", 0.0) or 0.0) + (1.0 - fast_smooth) * vy_i
|
|
+ dt = max(now - float(prev_t), 1e-3) if prev_t is not None else None
|
|
+ image_rate_sample = "legacy"
|
|
+ if cam_idx == _PTZ17_CAM_IDX:
|
|
+ image_rate_sample = _update_ptz17_image_rate(st, now, cxn, cyn)
|
|
+ elif dt is not None:
|
|
+ _update_image_rate_ema(st, cxn - st["prev_cx"], cyn - st["prev_cy"], dt)
|
|
|
|
# Convert image displacement to a true line-of-sight angle before any
|
|
# steering decisions. Normalized pixels are not a physical motion unit:
|
|
@@ -9705,6 +9751,7 @@
|
|
)
|
|
zoom_rearm_ready = _zoom_revalidation_ready(st)
|
|
stable_ok = stable_ok or predictive_entry or predictive_continuing
|
|
+ image_rate_ready = image_rate_sample in ("legacy", "updated", "short_checked")
|
|
can_zoom_in = (
|
|
zoom_safe
|
|
and zoom_angular_safe
|
|
@@ -9716,6 +9763,7 @@
|
|
or bounded_identification_request
|
|
)
|
|
and fast_drift <= float(getattr(config, "ZOOM_IN_FAST_DRIFT_MAX", 0.45))
|
|
+ and image_rate_ready
|
|
and now >= float(st.get("focus_settle_until", 0.0) or 0.0)
|
|
)
|
|
|
|
@@ -10739,6 +10787,7 @@
|
|
("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))),
|
|
+ ("image_rate_sample", not image_rate_ready),
|
|
("focus", now < float(st.get("focus_settle_until", 0.0) or 0.0)),
|
|
("coast", visual_control_coast),
|
|
("stale", visual_control_stale),
|
|
@@ -10760,6 +10809,8 @@
|
|
center_error_norm=max(abs(float(measured_dx)), abs(float(measured_dy))),
|
|
edge_margin_norm=float(edge_risk_margin), approach_margin_norm=approach_margin,
|
|
image_rate_norm_s=max(float(vel), float(fast_drift)),
|
|
+ image_rate_sample=image_rate_sample,
|
|
+ frame_clock="visual_receipt" if visual_only else "control_entry",
|
|
zoom_requested=bool(raw_zoom_needed), zoom_before_gate=ptz17_zoom_before_gate,
|
|
zoom_command=float(zoom_cmd), proposed_native=int(timing_native),
|
|
reason=zoom_zero_reason if zoom_cmd <= 0.0 else "positive_command",
|