|
|
--- a/postprocess.py
|
|
|
+++ b/postprocess.py
|
|
|
@@ -10,6 +10,9 @@
|
|
|
import cv2
|
|
|
|
|
|
from . import config, state, visual_control
|
|
|
+from .zoom_controller import (
|
|
|
+ ZoomFrame, controller_for_track, expire_calibration, flush_zoom_blocks, record_zoom_blocks, telemetry_tick,
|
|
|
+)
|
|
|
from . import motion_detector
|
|
|
from .utils import (
|
|
|
clipf,
|
|
|
@@ -30,6 +33,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 +4601,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 +4711,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
|
|
|
@@ -6553,6 +6563,8 @@
|
|
|
if (now - last) < TRACK_END_RETURN_COOLDOWN_SEC:
|
|
|
return False
|
|
|
st["last_track_end_return_at"] = now
|
|
|
+ flush_zoom_blocks(config, cam_idx, st, now, final=True)
|
|
|
+ expire_calibration(config, cam_idx, st, now, final=True)
|
|
|
|
|
|
# СРАЗУ ставим "за нято goto", чтобы второй источник не успел добавить ещё один goto
|
|
|
settle = float(getattr(config, "GOTO_SETTLE_SEC", 2.5))
|
|
|
@@ -8778,6 +8790,76 @@
|
|
|
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()
|
|
|
+ expire_calibration(config, cam_idx, st, decision_wall)
|
|
|
+ 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")
|
|
|
+ ),
|
|
|
+ rearm_ready=_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)
|
|
|
+ or hfov_deg < predictive.cfg.tele_steering_start_fov
|
|
|
+ ),
|
|
|
+ )
|
|
|
+ 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 +8901,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 +8929,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 +9052,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 +9200,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 +9260,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"
|
|
|
@@ -9159,21 +9268,27 @@
|
|
|
out_edge_effective
|
|
|
or float(min_margin) < float(getattr(config, "Z_SAFETY_EDGE", 0.05))
|
|
|
)
|
|
|
- zoom_cmd, routine_zoom_out_blocked = _gate_routine_zoom_out_hold(
|
|
|
+ predictive_step_back = bool(
|
|
|
+ predictive_decision and predictive_decision.step_back
|
|
|
+ and not zoom_out_emergency and not st.get("zoom_step_pending")
|
|
|
+ and not st.get("lens_move_put_inflight") and not st.get("zoom_stop_obligation_active")
|
|
|
+ and float(st.get("last_ptz_physical_cmd", (0., 0., 0.))[2]) <= 0
|
|
|
+ )
|
|
|
+ if predictive_step_back:
|
|
|
+ zoom_cmd = -predictive.cfg.step_back_cmd
|
|
|
+ routine_zoom_out_blocked = False
|
|
|
+ else:
|
|
|
+ zoom_cmd, routine_zoom_out_blocked = _gate_routine_zoom_out_hold(
|
|
|
+ st, now, zoom_cmd, emergency=zoom_out_emergency,
|
|
|
+ )
|
|
|
+ zoom_cmd = _gate_zoom_out_pulse(
|
|
|
st,
|
|
|
- now,
|
|
|
+ time.time() if predictive_step_back else now,
|
|
|
zoom_cmd,
|
|
|
emergency=zoom_out_emergency,
|
|
|
)
|
|
|
- zoom_cmd = _gate_zoom_out_pulse(
|
|
|
- st,
|
|
|
- now,
|
|
|
- zoom_cmd,
|
|
|
- emergency=zoom_out_emergency,
|
|
|
- )
|
|
|
- if float(zoom_cmd) < 0.0 and float(prev_z) >= 0.0 and not bool(
|
|
|
- st.get("zoom_rearm_required", False)
|
|
|
- ):
|
|
|
+ if (not predictive_step_back and float(zoom_cmd) < 0.0 and float(prev_z) >= 0.0
|
|
|
+ and not st.get("zoom_rearm_required", False)):
|
|
|
_arm_zoom_revalidation(st, now, "zoom_out", cam_idx=cam_idx)
|
|
|
pre_ramp_zoom_cmd = float(zoom_cmd)
|
|
|
if zoom_cmd > 0.0:
|
|
|
@@ -9272,6 +9387,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 +9433,23 @@
|
|
|
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 in ("min_hfov_limit", "tele_calibration_wait")
|
|
|
+ or (predictive_decision.reason == "edge_margin_low"
|
|
|
+ and hfov_deg <= predictive.cfg.tele_pulse_fov)
|
|
|
+ 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 +9472,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 +9534,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,
|
|
|
@@ -9398,6 +9558,22 @@
|
|
|
precenter_optics_at = st.get("zoom_status_at")
|
|
|
precenter_measured_hfov = st.get("hfov_deg")
|
|
|
status_observed_now = time.time()
|
|
|
+ tele_rate_history = tuple(st.get("status_rate_history", ())) if predictive else ()
|
|
|
+ if predictive and predictive_frame and predictive.optics.actual is not None:
|
|
|
+ st["tele_steering_active"] = False
|
|
|
+ aligned = next((x for x in reversed(tele_rate_history)
|
|
|
+ if 0 <= now - x[0] <= predictive.cfg.frame_age_s), None)
|
|
|
+ if aligned and status_observed_now - aligned[0] <= predictive.cfg.status_age_s:
|
|
|
+ steering = predictive.tele_aim(
|
|
|
+ predictive_frame, status_observed_now,
|
|
|
+ (float(aligned[1]) * _PAN_SIGN, float(aligned[2]) * _TILT_SIGN),
|
|
|
+ predictive_decision.ff_deg_s if predictive_decision else (0., 0.),
|
|
|
+ )
|
|
|
+ if steering is not None:
|
|
|
+ (pan_tele, tilt_tele), blend = steering
|
|
|
+ adj_pan += blend * (pan_tele - adj_pan)
|
|
|
+ adj_tilt += blend * (tilt_tele - adj_tilt)
|
|
|
+ st["tele_steering_active"] = True
|
|
|
guard_status_fresh = bool(
|
|
|
guard_status_at > 0.0
|
|
|
and 0.0 <= (status_observed_now - guard_status_at)
|
|
|
@@ -9844,12 +10020,53 @@
|
|
|
("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)
|
|
|
+ if getattr(config, "PREDICTIVE_ZOOM_TELEMETRY_DIR", "logs/ptz_zoom"):
|
|
|
+ blocked = {}
|
|
|
+ if zoom_cmd <= 0.0 and raw_zoom_needed:
|
|
|
+ if not stable_ok:
|
|
|
+ blocked["centering"] = {"error": center_err, "limit": lock_gate, "stable_frames": st.get("stable_frames")}
|
|
|
+ if not zoom_safe:
|
|
|
+ blocked["bbox_check"] = {"margin": zoom_pred_margin, "limit": zoom_center_max,
|
|
|
+ "bbox_px": [x2-x1, y2-y1]}
|
|
|
+ if not zoom_angular_safe and followup_ready:
|
|
|
+ blocked["angle_tol"] = {"pan_deg": los_pan_angle_deg, "tilt_deg": los_tilt_angle_deg,
|
|
|
+ "limit_deg": angular_max}
|
|
|
+ if near_edge or edge_recovery_active or (
|
|
|
+ predictive_decision and predictive_decision.reason in ("edge_margin_low", "edge_acceleration")):
|
|
|
+ blocked["edge_margin"] = {"measured": min_margin,
|
|
|
+ "predicted": predictive_decision.margin if predictive_decision else None,
|
|
|
+ "required": predictive_decision.edge_margin_required if predictive_decision else None}
|
|
|
+ if predictive_decision and predictive_decision.reason == "min_hfov_limit":
|
|
|
+ blocked["min_hfov_limit"] = {"hfov": hfov_deg, "minimum": predictive.cfg.min_allowed_hfov_deg}
|
|
|
+ if st.get("lens_move_put_inflight"):
|
|
|
+ blocked["put_inflight"] = {"kind": st.get("lens_move_put_kind"), "request": decision_request}
|
|
|
+ with _zoom_command_lock(cam_idx):
|
|
|
+ record_zoom_blocks(config, cam_idx, st, time.time(), blocked)
|
|
|
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 predictive_step_back and zoom_cmd < 0.0:
|
|
|
+ with _zoom_command_lock(cam_idx):
|
|
|
+ if not visual_control.current(cam_idx, visual_sample, time.time(), max_age=.25):
|
|
|
+ return
|
|
|
+ if not st.get("zoom_rearm_required"):
|
|
|
+ _arm_zoom_revalidation_locked(st, time.time(), "tele_step_back")
|
|
|
+ dispatch_kwargs["force"] = True
|
|
|
+ _dispatch_owned_zoom_tracking(
|
|
|
+ cam_idx, _PAN_SIGN * adj_pan, _TILT_SIGN * adj_tilt, zoom_cmd, **dispatch_kwargs
|
|
|
+ )
|
|
|
+ else:
|
|
|
+ _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_io.py
|
|
|
+++ b/ptz_io.py
|
|
|
@@ -14,6 +14,9 @@
|
|
|
from requests.adapters import HTTPAdapter
|
|
|
|
|
|
from . import config, state
|
|
|
+from .zoom_controller import (
|
|
|
+ record_zoom_blocks, transport_accepted, transport_confirmed, transport_stopped, transport_zoom_block_reason,
|
|
|
+)
|
|
|
from .utils import clipf, hfov_ptz_from_abs_zoom, hfov_ptz_from_zoom_ratio, quantf
|
|
|
from .geom import sector_contains, ang_diff_signed, normalize360
|
|
|
|
|
|
@@ -550,6 +553,7 @@
|
|
|
cmd: PTZCommand,
|
|
|
now: float,
|
|
|
*, native_cmd: int = 0,
|
|
|
+ put_rtt_s: float | None = None,
|
|
|
) -> bool:
|
|
|
"""Anchor a pending +zoom window to the successful camera HTTP PUT."""
|
|
|
if not _pending_zoom_request_matches(st, cmd):
|
|
|
@@ -582,6 +586,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, put_rtt_s,
|
|
|
+ )
|
|
|
logger.info(
|
|
|
"[ZOOM DELIVERY] cam=%s request=%s kind=%s physical start seq=%s pulse=%.3fs hard=%.3fs",
|
|
|
cam_idx,
|
|
|
@@ -744,6 +752,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,
|
|
|
@@ -800,6 +810,8 @@
|
|
|
reason: str,
|
|
|
stop_axes: Optional[tuple[int, int, int]] = None,
|
|
|
stop_command: Optional[MoveTuple] = None,
|
|
|
+ stop_issued_at: float | None = None,
|
|
|
+ stop_rtt_s: float | None = None,
|
|
|
) -> bool:
|
|
|
"""Finalize transport delivery; charge a pulse only with optical evidence."""
|
|
|
if not bool(st.get("zoom_step_pending", False)):
|
|
|
@@ -922,6 +934,13 @@
|
|
|
+ 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),
|
|
|
+ duration_s=delivered, stop_issued_at=stop_issued_at, stop_rtt_s=stop_rtt_s,
|
|
|
+ )
|
|
|
st["zoom_step_last_delivered_sec"] = float(delivered)
|
|
|
st["zoom_step_pending"] = False
|
|
|
st["zoom_step_pending_id"] = 0
|
|
|
@@ -1257,6 +1276,7 @@
|
|
|
float(getattr(config, "PTZ_LENS_STOP_DIRECT_TIMEOUT_SEC", 0.75)),
|
|
|
),
|
|
|
)
|
|
|
+ stop_issued_at, stop_issued_mono = time.time(), time.monotonic()
|
|
|
ok, code, body = _ptz_put_priority_stop(
|
|
|
url,
|
|
|
data=xml,
|
|
|
@@ -1266,6 +1286,7 @@
|
|
|
verify=verify,
|
|
|
)
|
|
|
now = time.time()
|
|
|
+ stop_rtt_s = time.monotonic() - stop_issued_mono
|
|
|
st["zoom_stop_obligation_attempts"] = int(
|
|
|
st.get("zoom_stop_obligation_attempts", 0) or 0
|
|
|
) + 1
|
|
|
@@ -1328,6 +1349,7 @@
|
|
|
now,
|
|
|
reason=f"priority_stop:{reason}",
|
|
|
stop_axes=(pan_i, tilt_i, 0),
|
|
|
+ stop_issued_at=stop_issued_at, stop_rtt_s=stop_rtt_s,
|
|
|
)
|
|
|
_cancel_zoom_step_watchdog(st)
|
|
|
if ordering_inflight:
|
|
|
@@ -2121,6 +2143,35 @@
|
|
|
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:
|
|
|
"""
|
|
|
Жёсткий стоп PTZ.
|
|
|
@@ -3308,6 +3359,23 @@
|
|
|
|
|
|
reserve_wall = time.time()
|
|
|
reserve_mono = time.monotonic()
|
|
|
+ camera_profile = config.CAMERA_CONFIG.get(cam_idx, {})
|
|
|
+ floor_block = (transport_zoom_block_reason(
|
|
|
+ st, camera_profile, reserve_wall, _zoom_step_timing(st)[1],
|
|
|
+ ) if int(zoom_i) > 0 else None)
|
|
|
+ if floor_block:
|
|
|
+ if pending_matches and not st.get("zoom_step_physical_started_at"):
|
|
|
+ _clear_pending_zoom_delivery(st, reserve_wall, reason=floor_block, force=True)
|
|
|
+ pending_matches = False
|
|
|
+ if st.get("mode") == "TRACK":
|
|
|
+ record_zoom_blocks(config, cam_idx, st, reserve_wall, {
|
|
|
+ floor_block: {"hfov": st.get("hfov_deg"), "stage": "pre_put",
|
|
|
+ "request": _cmd_move_seq(cmd)},
|
|
|
+ })
|
|
|
+ zoom_i = 0
|
|
|
+ if bool(getattr(config, "PTZ_ZOOM_STOP_FORCE_AXES_ZERO", True)):
|
|
|
+ pan_i = tilt_i = 0
|
|
|
+ xml = f"<PTZData><pan>{pan_i}</pan><tilt>{tilt_i}</tilt><zoom>0</zoom></PTZData>"
|
|
|
if int(zoom_i) > 0 and pending_matches and float(
|
|
|
st.get("zoom_step_physical_started_at", 0.0) or 0.0
|
|
|
) <= 0.0:
|
|
|
@@ -3481,6 +3549,7 @@
|
|
|
sent_nonzero_move = bool(
|
|
|
int(pan_i) != 0 or int(tilt_i) != 0 or int(zoom_i) != 0
|
|
|
)
|
|
|
+ put_call_at, put_call_mono = time.time(), time.monotonic()
|
|
|
ok, code, body = _ptz_put(
|
|
|
url,
|
|
|
data=xml,
|
|
|
@@ -3489,6 +3558,7 @@
|
|
|
timeout=timeout,
|
|
|
verify=verify,
|
|
|
)
|
|
|
+ put_rtt_s = time.monotonic() - put_call_mono
|
|
|
if ok:
|
|
|
break
|
|
|
if attempt < retry_count:
|
|
|
@@ -3560,7 +3630,7 @@
|
|
|
st,
|
|
|
cmd,
|
|
|
physical_now,
|
|
|
- native_cmd=int(zoom_i),
|
|
|
+ native_cmd=int(zoom_i), put_rtt_s=put_rtt_s,
|
|
|
)
|
|
|
elif int(zoom_i) <= 0:
|
|
|
if float(previous_physical_zoom) > 0.0:
|
|
|
@@ -3571,6 +3641,8 @@
|
|
|
reason="physical_stop",
|
|
|
stop_axes=(pan_i, tilt_i, zoom_i),
|
|
|
stop_command=cmd,
|
|
|
+ stop_issued_at=put_call_at if int(zoom_i) == 0 else None,
|
|
|
+ stop_rtt_s=put_rtt_s,
|
|
|
)
|
|
|
elif (
|
|
|
int(zoom_i) == 0
|
|
|
--- a/status.py
|
|
|
+++ b/status.py
|
|
|
@@ -14,6 +14,9 @@
|
|
|
|
|
|
|
|
|
# ---------------- Публичные утилиты статуса ----------------
|
|
|
+from .zoom_controller import observe_calibration
|
|
|
+
|
|
|
+
|
|
|
def get_cached_angles(cam_id: int) -> Tuple[Optional[float], Optional[float], Optional[float]]:
|
|
|
"""
|
|
|
Вернёт (pan_deg, tilt_deg, t_epoch) из кэша, если есть (иначе None).
|
|
|
@@ -170,6 +173,14 @@
|
|
|
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))
|
|
|
+ observe_calibration(config, cam_id, st, 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 "")
|