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.
378 lines
18 KiB
Diff
378 lines
18 KiB
Diff
--- a/postprocess.py
|
|
+++ b/postprocess.py
|
|
@@ -10,6 +10,7 @@
|
|
import cv2
|
|
|
|
from . import config, state, visual_control
|
|
+from .zoom_controller import ZoomFrame, controller_for_track, telemetry_tick
|
|
from . import motion_detector
|
|
from .utils import (
|
|
clipf,
|
|
@@ -30,6 +31,7 @@
|
|
_zoom_command_lock,
|
|
zoom_step_optics_delta,
|
|
_zoom_speed_to_int,
|
|
+ stop_predictive_zoom_async,
|
|
)
|
|
from .geom import ang_diff_signed, sector_contains
|
|
from .patrol import (
|
|
@@ -4597,6 +4599,7 @@
|
|
optics_observed_now: float | None = None,
|
|
stop_trace: list[str] | None = None,
|
|
continuation_trace: list[tuple[int, int, float, float, float, float, float]] | None = None,
|
|
+ predictive=None,
|
|
) -> float:
|
|
"""Break continuous zoom-in into short feedback-controlled steps."""
|
|
pulse_until = float(st.get("zoom_step_until", 0.0) or 0.0)
|
|
@@ -4706,18 +4709,23 @@
|
|
_finish_zoom_step(st, now, stable_key=stable_key)
|
|
return 0.0
|
|
if pulse_until <= 0.0:
|
|
- if (
|
|
+ observed = now if optics_observed_now is None else optics_observed_now
|
|
+ early_repeat = bool(predictive and predictive.repeat_after_stop(st, observed))
|
|
+ if not early_repeat and (
|
|
float(now) < cooldown_until
|
|
or float(now) < float(st.get("zoom_recenter_until", 0.0) or 0.0)
|
|
):
|
|
return 0.0
|
|
- if (
|
|
+ if early_repeat:
|
|
+ # The original transaction is already committed. Consume its single
|
|
+ # credit under the transport lock, before publishing a new request.
|
|
+ predictive.confirmed_stop = None
|
|
+ st["zoom_feedback_required_after"] = 0.0
|
|
+ elif (
|
|
int(st.get("zoom_in_pulse_count", 0) or 0) > 0
|
|
or float(st.get("zoom_feedback_required_after", 0.0) or 0.0) > 0.0
|
|
):
|
|
- feedback_ready, feedback_reason = _zoom_optics_feedback_state(
|
|
- st, now if optics_observed_now is None else optics_observed_now,
|
|
- )
|
|
+ feedback_ready, feedback_reason = _zoom_optics_feedback_state(st, observed)
|
|
if not feedback_ready:
|
|
st["zoom_followup_reason"] = str(feedback_reason)
|
|
return 0.0
|
|
@@ -8778,6 +8786,74 @@
|
|
react_margin = clipf(react_margin_base + react_margin_gain * vel, react_margin_base, react_margin_max)
|
|
near_react_edge = min_margin < react_margin
|
|
|
|
+ predictive = None
|
|
+ predictive_frame = None
|
|
+ predictive_decision = None
|
|
+ if cfg_ptz.get("predictive_zoom") is not None:
|
|
+ with _zoom_command_lock(cam_idx):
|
|
+ predictive = controller_for_track(st, cfg_ptz)
|
|
+ if predictive is not None:
|
|
+ decision_wall = time.time()
|
|
+ sample_is_current = bool(
|
|
+ visual_sample is not None
|
|
+ and visual_control.current(
|
|
+ cam_idx, visual_sample, decision_wall,
|
|
+ max_age=predictive.cfg.frame_age_s,
|
|
+ )
|
|
+ )
|
|
+ predictive_frame = ZoomFrame(
|
|
+ at=now, seq=int(source_seq or 0),
|
|
+ offset=(measured_dx, measured_dy),
|
|
+ bbox_frac=(w_frac, h_frac), bbox_px=(x2 - x1, y2 - y1),
|
|
+ angles=(los_pan_angle_deg, los_tilt_angle_deg),
|
|
+ rates=(los_pan_rate_deg_s, los_tilt_rate_deg_s),
|
|
+ aspect=h / w,
|
|
+ tracker_score=(
|
|
+ float(st.get("visual_tracker_score", 0.0) or 0.0)
|
|
+ if current_track_source == "visual" else None
|
|
+ ),
|
|
+ detector_at=(last_visual_zoom_validation if normal_zoom_quality_ok
|
|
+ and last_visual_zoom_validation == last_visual_control_validation else 0.0),
|
|
+ detector_conf=float(st.get("visual_tracker_last_detector_conf", 0.0) or 0.0),
|
|
+ geometry_ok=bool(
|
|
+ sample_is_current and not st.get("visual_tracker_mismatch_frames")
|
|
+ and _zoom_revalidation_ready(st)
|
|
+ ),
|
|
+ los_valid=bool(
|
|
+ st.get("aim_los_rate_valid")
|
|
+ or predictive.request is not None
|
|
+ or predictive.repeat_after_stop(st, decision_wall)
|
|
+ or (st.get("zoom_step_pending") and predictive.history)
|
|
+ ),
|
|
+ )
|
|
+ skip_wait = predictive.repeat_after_stop(st, decision_wall)
|
|
+ same_pending = bool(
|
|
+ st.get("zoom_step_pending")
|
|
+ and st.get("predictive_zoom_request") == st.get("zoom_step_pending_id")
|
|
+ and not st.get("zoom_step_cancel_requested")
|
|
+ and not st.get("zoom_step_discard_on_finalize")
|
|
+ )
|
|
+ predictive_decision = predictive.decide(
|
|
+ predictive_frame, decision_wall,
|
|
+ first=pulse_count == 0 and predictive.first_ack_at is None,
|
|
+ cooldown=not skip_wait and decision_wall < max(
|
|
+ float(st.get("zoom_step_cooldown_until", 0.0) or 0.0),
|
|
+ float(st.get("zoom_recenter_until", 0.0) or 0.0),
|
|
+ ),
|
|
+ transport_clear=bool(
|
|
+ same_pending or skip_wait or (pulse_count == 0
|
|
+ and not st.get("zoom_feedback_required_after")
|
|
+ and not st.get("zoom_step_feedback_pending"))
|
|
+ or _zoom_optics_feedback_state(st, decision_wall)[0]
|
|
+ ) and not (
|
|
+ st.get("zoom_stop_obligation_active")
|
|
+ or st.get("lens_move_stop_required")
|
|
+ or st.get("zoom_step_cancel_requested")
|
|
+ or st.get("zoom_no_effect_blocked")
|
|
+ or (st.get("zoom_step_pending") and not same_pending)
|
|
+ ),
|
|
+ )
|
|
+
|
|
zoom_quality_ok = bool(
|
|
normal_zoom_quality_ok
|
|
or probe_allowed
|
|
@@ -8819,6 +8895,21 @@
|
|
pan_rate_deg_s=float(los_pan_rate_deg_s),
|
|
tilt_rate_deg_s=float(los_tilt_rate_deg_s),
|
|
)
|
|
+ predictive_entry = bool(
|
|
+ predictive_decision and predictive_decision.window
|
|
+ and normal_zoom_quality_ok and not bounded_identification_request
|
|
+ and not st.get("zoom_probe_active", False)
|
|
+ and not _zoom_identify_matches_current_track(st)
|
|
+ and not st.get("zoom_stop_obligation_active")
|
|
+ and not st.get("zoom_step_cancel_requested")
|
|
+ )
|
|
+ predictive_continuing = bool(
|
|
+ predictive_decision and same_pending and not predictive_decision.stop
|
|
+ and (predictive_decision.window or predictive_decision.reason == "pulse_active")
|
|
+ and normal_zoom_quality_ok and not bounded_identification_request
|
|
+ )
|
|
+ if (predictive_entry and (skip_wait or same_pending)) or predictive_continuing:
|
|
+ followup_ready = True
|
|
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),
|
|
@@ -8832,6 +8923,9 @@
|
|
)
|
|
if approach_margin is not None and followup_ready:
|
|
zoom_angular_safe = True
|
|
+ if (predictive_entry or predictive_continuing) and followup_ready:
|
|
+ zoom_angular_safe = True
|
|
+ zoom_safe, zoom_safe_scale = True, 1.0
|
|
st["zoom_safe_pred_margin"] = float(zoom_pred_margin)
|
|
st["zoom_safe_center_limit"] = float(zoom_center_max)
|
|
st["zoom_quality_ok"] = bool(zoom_quality_ok)
|
|
@@ -8952,6 +9046,7 @@
|
|
or bounded_identification_request
|
|
)
|
|
zoom_rearm_ready = _zoom_revalidation_ready(st)
|
|
+ stable_ok = stable_ok or predictive_entry or predictive_continuing
|
|
can_zoom_in = (
|
|
zoom_safe
|
|
and zoom_angular_safe
|
|
@@ -9099,6 +9194,14 @@
|
|
zoom_zero_reason = "deadband"
|
|
zoom_cmd = 0.0
|
|
|
|
+ predictive_drive = bool(
|
|
+ (predictive_entry or predictive_continuing) and can_zoom_in and raw_zoom_needed
|
|
+ and prev_z >= 0.0 and not want_out
|
|
+ and not normal_size_hold and not edge_recovery_active
|
|
+ and float(getattr(config, "ZOOM_STEP_MAX_IN_CMD", 0.12)) >= 0.10
|
|
+ )
|
|
+ if predictive_drive:
|
|
+ zoom_cmd = 0.10
|
|
zoom_after_size_minimum = zoom_cmd
|
|
cmd_mag_est = max(
|
|
abs(float(pan_speed) + float(pan_rate_ff)),
|
|
@@ -9151,7 +9254,7 @@
|
|
center_err=float(center_err),
|
|
moving_out=bool(moving_out),
|
|
recent_zoom=bool(recent_zoom_steering),
|
|
- approach_margin=approach_margin,
|
|
+ approach_margin=(predictive_decision.margin if predictive_drive else approach_margin),
|
|
)
|
|
if before_recenter > 0.0 and zoom_cmd <= 0.0:
|
|
zoom_zero_reason = "recenter"
|
|
@@ -9272,6 +9375,25 @@
|
|
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
|
|
+ # A separately checked predictive window supplies the normal native-10
|
|
+ # shape. Preserve the original ticket and timing on subsequent frames.
|
|
+ predictive_active = bool(
|
|
+ predictive_continuing and predictive_drive and can_zoom_in and zoom_cmd > 0.0
|
|
+ and pulse_family == "normal" and st.get("zoom_step_pending")
|
|
+ and predictive.owns(st)
|
|
+ and st.get("zoom_step_pending_id") == decision_request
|
|
+ and st.get("predictive_zoom_request") == decision_request
|
|
+ and 0 <= zoom_gate_now - now <= predictive.cfg.frame_age_s
|
|
+ )
|
|
+ predictive_start = bool(
|
|
+ predictive_entry and predictive_drive and pulse_family == "normal"
|
|
+ and zoom_cmd > 0.0 and not st.get("zoom_step_pending") and predictive.owns(st)
|
|
+ and not st.get("zoom_stop_obligation_active") and not st.get("lens_move_stop_required")
|
|
+ and 0 <= zoom_gate_now - now <= predictive.cfg.frame_age_s
|
|
+ and 0 <= zoom_gate_now - predictive_frame.detector_at <= predictive.cfg.detector_age_s
|
|
+ and predictive.optics.fresh(zoom_gate_now)
|
|
+ and (not skip_wait or predictive.repeat_after_stop(st, zoom_gate_now))
|
|
+ )
|
|
before_shape = zoom_cmd
|
|
zoom_cmd, useful_reason = _shape_useful_zoom_pulse(
|
|
zoom_cmd,
|
|
@@ -9299,6 +9421,21 @@
|
|
requested_native=timing_native,
|
|
rejection=response_rejection,
|
|
)
|
|
+ if predictive_start or (predictive_active and not near_edge and not edge_recovery_active
|
|
+ and zoom_rearm_ready and not normal_size_hold and not want_out):
|
|
+ zoom_cmd = 0.10
|
|
+ useful_reason = "useful"
|
|
+ st["zoom_useful_pulse_reason"] = useful_reason
|
|
+ response_timing = predictive_decision.window if predictive_start else (
|
|
+ float(st["zoom_step_pending_pulse_sec"]),
|
|
+ float(st["zoom_step_pending_hard_sec"]),
|
|
+ )
|
|
+ if predictive_decision and (
|
|
+ predictive_decision.stop or (predictive_decision.reason == "size_reached"
|
|
+ and pulse_family == "normal")
|
|
+ ):
|
|
+ zoom_cmd = min(zoom_cmd, 0.0)
|
|
+ zoom_zero_reason = predictive_decision.reason
|
|
if useful_reason == "useful" and response_timing == (0.0, 0.0):
|
|
zoom_cmd = 0.0
|
|
st["zoom_useful_pulse_reason"] = "wait_response_framing"
|
|
@@ -9321,7 +9458,10 @@
|
|
optics_observed_now=zoom_gate_now,
|
|
stop_trace=gate_stop,
|
|
continuation_trace=continuation_trace,
|
|
- )
|
|
+ predictive=predictive if predictive_start else None,
|
|
+ )
|
|
+ if predictive_start and zoom_cmd > 0.0 and st["zoom_step_request_id"] > request_id_before_gate:
|
|
+ st["predictive_zoom_request"] = st["zoom_step_pending_id"]
|
|
if gate_stop:
|
|
zoom_zero_reason = gate_stop[-1]
|
|
decision_generation = st.get("ptz_generation")
|
|
@@ -9380,6 +9520,12 @@
|
|
|
|
adj_pan, adj_tilt = _shape_tracking_cmd(st, now, adj_pan, adj_tilt, center_err)
|
|
pan_feedback_cmd, tilt_feedback_cmd = float(adj_pan), float(adj_tilt)
|
|
+ if predictive_active and predictive.request is not None and zoom_cmd > 0.0:
|
|
+ pan_optical, tilt_optical = predictive_decision.ff_deg_s
|
|
+ pan_ref = max(1.0, float(cfg_ptz.get("ptz_pan_max_speed_deg_s", getattr(config, "AIM_PAN_SPEED_DEG_S", 160.0))))
|
|
+ tilt_ref = max(1.0, float(cfg_ptz.get("ptz_tilt_max_speed_deg_s", getattr(config, "AIM_TILT_SPEED_DEG_S", 120.0))))
|
|
+ pan_rate_ff += pan_optical / pan_ref
|
|
+ tilt_rate_ff += tilt_optical / tilt_ref
|
|
adj_pan, adj_tilt = _merge_gimbal_rate_feedforward(
|
|
st,
|
|
now,
|
|
@@ -9844,6 +9990,14 @@
|
|
("brake", final_guard_active),
|
|
) if blocked)
|
|
zoom_decision = (source_seq, decision_request, zoom_zero_reason, "+".join(blockers) or "-")
|
|
+ if predictive_decision is not None:
|
|
+ with _zoom_command_lock(cam_idx):
|
|
+ telemetry_tick(
|
|
+ config, cam_idx, predictive, predictive_frame, predictive_decision,
|
|
+ zoom_cmd, zoom_zero_reason, time.time(),
|
|
+ )
|
|
+ if predictive_decision.stop and predictive.request is not None:
|
|
+ stop_predictive_zoom_async(cam_idx, predictive, predictive_decision.reason)
|
|
dispatch_kwargs = {"force": force_track_ptz}
|
|
if zoom_decision is not None:
|
|
dispatch_kwargs["zoom_decision"] = zoom_decision
|
|
--- a/ptz_io.py
|
|
+++ b/ptz_io.py
|
|
@@ -14,6 +14,7 @@
|
|
from requests.adapters import HTTPAdapter
|
|
|
|
from . import config, state
|
|
+from .zoom_controller import transport_accepted, transport_confirmed, transport_stopped
|
|
from .utils import clipf, hfov_ptz_from_abs_zoom, hfov_ptz_from_zoom_ratio, quantf
|
|
from .geom import sector_contains, ang_diff_signed, normalize360
|
|
|
|
@@ -582,6 +583,10 @@
|
|
st["zoom_last_motion_at"] = float(now)
|
|
st["zoom_followup_reason"] = "physical_pulse"
|
|
st["zoom_step_last_delivery_result"] = "physical_started"
|
|
+ transport_accepted(
|
|
+ config, cam_idx, st, request_id, native_cmd, deadline_origin, now,
|
|
+ deadline_origin + hard_sec,
|
|
+ )
|
|
logger.info(
|
|
"[ZOOM DELIVERY] cam=%s request=%s kind=%s physical start seq=%s pulse=%.3fs hard=%.3fs",
|
|
cam_idx,
|
|
@@ -744,6 +749,8 @@
|
|
float(st.get("zoom_recenter_until", 0.0) or 0.0),
|
|
float(now) + float(getattr(config, "ZOOM_STEP_RECENTER_SEC", 0.14)),
|
|
)
|
|
+ if reason == "status_poll":
|
|
+ transport_confirmed(config, st, request_id, now)
|
|
logger.info(
|
|
"[ZOOM DELIVERY] cam=%s request=%s optics confirmed source=%s zstep=%s",
|
|
cam_idx,
|
|
@@ -922,6 +929,12 @@
|
|
+ max(0.0, float(getattr(config, "ZOOM_STEP_RETRY_COOLDOWN_SEC", 0.12))),
|
|
)
|
|
|
|
+ transport_stopped(
|
|
+ config, cam_idx, st, request_id, now, reason=reason,
|
|
+ confirmed=confirmed and not discard,
|
|
+ clean=bool(not discard and kind == "normal" and delivered + 1e-6 >= full_window
|
|
+ and stop_axes is not None and stop_axes[2] == 0),
|
|
+ )
|
|
st["zoom_step_last_delivered_sec"] = float(delivered)
|
|
st["zoom_step_pending"] = False
|
|
st["zoom_step_pending_id"] = 0
|
|
@@ -2119,6 +2132,35 @@
|
|
if zoom_decision is not None:
|
|
return cmd_source, cmd_generation, move_seq, zoom_i
|
|
return None
|
|
+
|
|
+
|
|
+def stop_predictive_zoom_async(cam_idx, controller, reason):
|
|
+ """Order an early lens STOP independently of the ordinary MOVE queue."""
|
|
+ with _zoom_command_lock(cam_idx):
|
|
+ st = state.ptz_states.get(cam_idx, {})
|
|
+ request = controller.request
|
|
+ if (not controller.owns(st) or request is None
|
|
+ or st.get("zoom_step_physical_request_id") != request
|
|
+ or st.get("predictive_zoom_early_stop") == request):
|
|
+ return
|
|
+ st["predictive_zoom_early_stop"] = request
|
|
+ st["zoom_step_cancel_requested"] = True
|
|
+
|
|
+ def stop_if_current():
|
|
+ with _zoom_command_lock(cam_idx):
|
|
+ current = state.ptz_states.get(cam_idx, {})
|
|
+ if (controller.owns(current)
|
|
+ and current.get("zoom_step_physical_request_id") == request):
|
|
+ _deliver_priority_lens_stop_locked(
|
|
+ cam_idx, current, reason=f"predictive:{reason}",
|
|
+ guard_inflight_after_put=_generic_put_has_lens_motion(current),
|
|
+ )
|
|
+
|
|
+ try:
|
|
+ threading.Thread(target=stop_if_current, name="ptz-predictive-stop", daemon=True).start()
|
|
+ except RuntimeError:
|
|
+ # The scheduled watchdog remains the independent hard STOP.
|
|
+ logger.exception("Unable to start early lens STOP cam=%s", cam_idx)
|
|
|
|
|
|
def stop_ptz(cam_idx: int, *, source: str = "stop", generation: Optional[int] = None) -> None:
|
|
--- a/status.py
|
|
+++ b/status.py
|
|
@@ -170,6 +170,13 @@
|
|
if isinstance(hfov_deg, (int, float)) and float(hfov_deg) > 0.0:
|
|
st["hfov_deg"] = float(hfov_deg)
|
|
st["zoom_status_at"] = zoom_observed_at
|
|
+ if config.CAMERA_CONFIG.get(int(cam_id), {}).get("predictive_zoom") is not None:
|
|
+ previous = st.get("predictive_zoom_hfov", (0.0, 0.0))
|
|
+ if zoom_observed_at > previous[0]:
|
|
+ st["predictive_zoom_hfov"] = (zoom_observed_at, float(hfov_deg))
|
|
+ controller = st.get("predictive_zoom_controller")
|
|
+ if controller is not None and controller.owns(st):
|
|
+ controller.optics.observe(zoom_observed_at, float(hfov_deg), float(now))
|
|
if isinstance(zoom_raw, (int, float)):
|
|
st["zoom_position_raw"] = float(zoom_raw)
|
|
st["zoom_position_tag"] = str(zoom_tag or "")
|