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