From 4c63829ccd91ab68dc47ea6d2cd50c6bee80c115 Mon Sep 17 00:00:00 2001 From: Oleksandr Bezdieniezhnykh Date: Tue, 19 May 2026 20:21:00 +0300 Subject: [PATCH] [AZ-654] [AZ-655] [AZ-656] gimbal_controller primitives + monotonic clock fix (batch 11) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit AZ-654 SweepEngine: pendulum default, Raster/LawnMower variants reserved and explicitly NotImplemented (no silent fallback per AC-3). Time injected via next_step(now) for deterministic dwell tests. AZ-655 PlanExecutor: linear yaw/pitch interpolation between PanGoals with self-throttle (default 50 ms); stats expose commands_emitted/dropped_to_throttle counters. PanGoal/PanPlan added to shared::models::gimbal (spec drift: data_model.md §PanPlan flagged for next doc sync). AZ-656 CentreOnTarget: zoom-aware proportional control loop (correction ~ 1/zoom); target_lost debounced — fires once per loss streak, resets on bbox return. Also fixes the misleadingly-named monotonic_ns() helper introduced by AZ-653 that used SystemTime::now(): GimbalController now owns a shared::clock::MonoClock and stamps GimbalState::ts_monotonic_ns via clock.elapsed_ns(). AZ-656 AC-2 forced the correction; integration test verifies the fix end-to-end. 58/58 gimbal_controller tests green (47 unit + 7 AZ-653 integration + 4 new batch_11 integration). Workspace test suite green this run. Co-authored-by: Cursor --- .../AZ-654_gimbal_zoom_out_sweep.md | 0 .../AZ-655_gimbal_smooth_pan_plan.md | 0 .../AZ-656_gimbal_centre_on_target.md | 0 .../batch_11_cycle1_report.md | 175 ++++++++ _docs/_autodev_state.md | 4 +- .../src/internal/centre_on_target.rs | 323 +++++++++++++++ crates/gimbal_controller/src/internal/mod.rs | 3 + .../src/internal/smooth_pan.rs | 374 ++++++++++++++++++ .../gimbal_controller/src/internal/sweep.rs | 350 ++++++++++++++++ crates/gimbal_controller/src/lib.rs | 26 +- .../tests/batch_11_integration.rs | 205 ++++++++++ crates/shared/src/models/gimbal.rs | 23 +- 12 files changed, 1470 insertions(+), 13 deletions(-) rename _docs/02_tasks/{todo => done}/AZ-654_gimbal_zoom_out_sweep.md (100%) rename _docs/02_tasks/{todo => done}/AZ-655_gimbal_smooth_pan_plan.md (100%) rename _docs/02_tasks/{todo => done}/AZ-656_gimbal_centre_on_target.md (100%) create mode 100644 _docs/03_implementation/batch_11_cycle1_report.md create mode 100644 crates/gimbal_controller/src/internal/centre_on_target.rs create mode 100644 crates/gimbal_controller/src/internal/smooth_pan.rs create mode 100644 crates/gimbal_controller/src/internal/sweep.rs create mode 100644 crates/gimbal_controller/tests/batch_11_integration.rs diff --git a/_docs/02_tasks/todo/AZ-654_gimbal_zoom_out_sweep.md b/_docs/02_tasks/done/AZ-654_gimbal_zoom_out_sweep.md similarity index 100% rename from _docs/02_tasks/todo/AZ-654_gimbal_zoom_out_sweep.md rename to _docs/02_tasks/done/AZ-654_gimbal_zoom_out_sweep.md diff --git a/_docs/02_tasks/todo/AZ-655_gimbal_smooth_pan_plan.md b/_docs/02_tasks/done/AZ-655_gimbal_smooth_pan_plan.md similarity index 100% rename from _docs/02_tasks/todo/AZ-655_gimbal_smooth_pan_plan.md rename to _docs/02_tasks/done/AZ-655_gimbal_smooth_pan_plan.md diff --git a/_docs/02_tasks/todo/AZ-656_gimbal_centre_on_target.md b/_docs/02_tasks/done/AZ-656_gimbal_centre_on_target.md similarity index 100% rename from _docs/02_tasks/todo/AZ-656_gimbal_centre_on_target.md rename to _docs/02_tasks/done/AZ-656_gimbal_centre_on_target.md diff --git a/_docs/03_implementation/batch_11_cycle1_report.md b/_docs/03_implementation/batch_11_cycle1_report.md new file mode 100644 index 0000000..d722912 --- /dev/null +++ b/_docs/03_implementation/batch_11_cycle1_report.md @@ -0,0 +1,175 @@ +# Batch 11 (cycle 1) implementation report + +**Tasks**: AZ-654, AZ-655, AZ-656 +**Component scope**: `gimbal_controller` (+ shared::models::gimbal type extension) +**Verdict**: PASS_WITH_WARNINGS — proceed; flagged items below. + +## Tasks + +### AZ-654 gimbal_zoom_out_sweep — pendulum default + reserved Raster/LawnMower + +**Outcome**: `SweepPattern` enum with all three variants declared; `Pendulum` implemented; `Raster` / `LawnMower` reserved and return `AutopilotError::NotImplemented` rather than silently falling back (per AC-3). Sweep envelope (yaw bounds, dwell, step) is configured via `SweepConfig`; the engine validates it on construction. + +**Production code added**: + +- `crates/gimbal_controller/src/internal/sweep.rs` + - `SweepPattern` enum — `#[derive(Default)]` with `Pendulum` as the default variant. Public re-export from `lib.rs`. + - `SweepConfig { min_yaw_deg, max_yaw_deg, pitch_deg, step_deg, dwell: Duration }` — validated on construction (bounds ordered, step > 0). Public re-export. + - `SweepEngine { pattern, config, current_yaw, direction, dwell_started_at: Option }` — state machine. Public re-export. + - `SweepEngine::next_step(now: Instant) -> Result` — pendulum path advances by `step_deg`, clamps at bounds, starts a dwell window, then reverses on next tick after `config.dwell`; Raster / LawnMower paths return `NotImplemented`. Time is injected (not `Instant::now()` internally) for deterministic tests. +- `crates/gimbal_controller/src/lib.rs` (re-export edit) + +**Tests** (10 total, all green): +- `ac1_pendulum_stays_within_bounds_over_100_steps` — verifies bound clamping + at least one reversal over 100 ticks at 1-second cadence. +- `ac2_dwell_holds_yaw_at_bound` — verifies the yaw stays pinned for the entire 500 ms dwell window then flips on the first call after the window elapses. +- `ac3_raster_returns_not_implemented` / `ac3_lawnmower_returns_not_implemented` — verifies the reserved-variant guarantee. +- `pattern_default_is_pendulum` — verifies `SweepPattern::default()` == `Pendulum`. +- `invalid_config_rejected` (yaw bounds invalid) and `invalid_step_rejected` (step ≤ 0) — verify `SweepConfig::validate`. +- `pendulum_advances_in_step_increments_then_clamps` — verifies the forward sweep yaw sequence `-25, -20, ..., 30` and clamping behaviour. +- Integration: `az654_sweep_engine_emits_gimbal_commands_within_bounds` — 200 ticks through the public API, verifies emitted `GimbalCommand` always inside `[min_yaw, max_yaw]` and the pitch is fixed at the configured value. + +### AZ-655 gimbal_smooth_pan_plan — path-tracking plan executor + +**Outcome**: `PlanExecutor` accepts a `PanPlan` (new shared type), linearly interpolates `(yaw, pitch)` between adjacent `PanGoal`s, and self-throttles to a configured min interval (default 50 ms). Past-end queries clamp to the final goal. Before-start queries extrapolate linearly from the first two goals. Stats track `commands_emitted_total` and `commands_dropped_to_throttle_total`. + +**Production code added**: + +- `crates/shared/src/models/gimbal.rs` — extended with `PanGoal { yaw_deg, pitch_deg, zoom, at_ns }` and `PanPlan { goals: Vec }`. **Spec drift note**: AZ-655 references `data_model.md §PanPlan`, but `data_model.md` does not yet have a PanPlan entry — the document sync run will catch up (logged below under "Spec drift"). +- `crates/gimbal_controller/src/internal/smooth_pan.rs` + - `DEFAULT_MIN_CMD_INTERVAL = 50 ms` constant. Public re-export. + - `NextStep { Emit(GimbalCommand), Throttled }` — `Throttled` is an explicit state, never silent. + - `ExecutorStats { plan_loaded_at, commands_emitted_total, commands_dropped_to_throttle_total }` — public read-side. + - `PlanExecutor::new(min_cmd_interval)`, `with_default_throttle()`, `load(plan, now)`, `next_step(now)`, `stats()`, `has_plan()`. + - `validate_plan` rejects empty plans and non-strictly-increasing `at_ns`. + - `interpolate` + `linear_at` + `lerp` — pure helpers, no I/O. +- `crates/gimbal_controller/src/lib.rs` (re-export edit) + +**Tests** (8 total, all green): +- `ac1_linear_interp_midpoint` — plan 0°→30° over 1s, query at 500 ms → yaw ≈ 15°. +- `ac2_throttle_drops_intermediate_calls` — 100 ticks at 10 ms cadence over 1s with 100 ms throttle → ~10 emissions, the rest counted as throttled. Stats counter cross-checked. +- `ac3_past_plan_end_clamps_to_last_goal` — query 5 s after a 1 s plan → returns the last goal's values exactly. +- `empty_plan_rejected` / `non_monotonic_plan_rejected` / `no_plan_returns_error` — `Validation` error paths. +- `reload_clears_throttle_anchor` — re-loading the plan does not carry the previous plan's last_emit_at forward. +- `single_goal_plan_holds_value` — degenerate 1-goal plan returns the goal verbatim for any query time. +- Integration: `az655_plan_executor_emits_and_throttles_against_real_clock` — exercises the public API with a 20 ms throttle over 500 ms (100 ticks at 5 ms cadence) → ~25 emissions; verifies `ExecutorStats` counters match the emission ratio. + +### AZ-656 gimbal_centre_on_target — proportional centre-25% loop + monotonic-stamped state publish + target_lost debounce + +**Outcome**: `CentreOnTarget::tick(bbox, yaw, pitch, zoom) -> CentreOnTargetOutput` runs a proportional control loop that drags the target bbox toward the centre 25 % of the frame. The control law scales the yaw correction by `1 / zoom` because the effective FOV shrinks with zoom. `target_lost` debounces — fires exactly once on the tick that crosses the `max_missed_ticks` threshold, then stays silent for the rest of the loss streak; a visible bbox resets the counter so a subsequent loss streak can re-fire. + +**Production code added**: + +- `crates/gimbal_controller/src/internal/centre_on_target.rs` + - `DEFAULT_TARGET_GAIN = 0.6`, `DEFAULT_CENTRE_WINDOW = 0.25`, `DEFAULT_MAX_MISSED_TICKS = 3`. Public re-exports. + - `CentreOnTargetConfig { fov_deg_at_zoom1, gain, centre_half_width, max_missed_ticks }` with sensible `Default` impl. + - `CentreOnTargetOutput { command: Option, target_lost_signal: bool, on_target: bool }`. + - `CentreOnTarget { config, consecutive_missed, in_loss_state }` — pure controller, no I/O. + - `CentreOnTarget::tick(bbox, yaw, pitch, zoom)` — when `bbox.is_none()`, increments the missed counter (capped at `max_missed_ticks` to avoid the unbounded growth → re-fire bug); when `bbox.is_some()`, resets the counter, computes `(err_x, err_y) = bbox_centre - 0.5`, scales by `fov / zoom * gain`, emits the corrective command, and sets `on_target` iff both errors are inside `centre_half_width`. +- `crates/gimbal_controller/src/lib.rs` + - **Bug fix** carried in this batch: replaced the misleadingly-named `monotonic_ns()` (which used `SystemTime::now()` and was NOT monotonic) with `shared::clock::MonoClock`. The `GimbalController` and `GimbalControllerHandle` now own a `MonoClock` and stamp `GimbalState::ts_monotonic_ns` via `self.clock.elapsed_ns()`. This bug was introduced by AZ-653 (batch 10) and would have surfaced as observable wall-clock jumps on the consumer side (`movement_detector` ego-motion sync, `frame_ingest` telemetry tagging). AZ-656's AC-2 forced the issue. + +**Tests** (6 unit + 1 integration = 7 total, all green): +- `ac1_centre_25pct_within_3_ticks` — runs the closed loop against a linearised kinematic camera model (the same one the proportional controller assumes); starting bbox at `(0.75, 0.55, 0.1, 0.1)` is inside the centre 25 % region by tick 3; `on_target` flag is observed at some point in the run. +- `ac3_target_lost_emits_once_per_loss_streak` — verifies: no signal at tick 1/2; signal at tick 3; silent at tick 4/5; bbox returns → counter resets; second loss streak signals again at the new threshold. +- `bbox_already_centred_marks_on_target_with_small_command` — bbox centred at (0.5, 0.5) → `on_target = true` and `delta_yaw`/`delta_pitch` ≈ 0. +- `higher_zoom_yields_smaller_correction` — same bbox at 1× vs 4× → 4× correction is exactly 1/4 of 1× correction (within float tolerance). +- `loss_counter_caps_safely_without_overflow` — hammer `tick(None)` 10 000 times; signal fires exactly once. +- `loss_streak_below_threshold_then_recovery_does_not_signal` — `max_missed_ticks=5`, 3 missed then recovery → no signal. +- Integration: `az656_set_pose_publishes_monotonic_timestamp` — verifies the AZ-653 → AZ-656 bug fix by issuing 3 sequential `set_pose` calls through the full transport stack (with a fake A40 echo loop) and checking `state_rx.borrow().ts_monotonic_ns` is strictly monotonic across the three observations. +- Integration: `az656_centre_on_target_loop_converges_via_public_api` — duplicates the AC-1 convergence assertion using only `gimbal_controller` re-exports + `shared::models::frame::BoundingBox` (catches re-export drift). + +## AC coverage + +| Task | AC | Behaviour | Test | Status | +|------|----|-----------|------|--------| +| AZ-654 | AC-1 | 100-step pendulum stays in bounds, reverses at each bound | `ac1_pendulum_stays_within_bounds_over_100_steps` | PASS | +| AZ-654 | AC-2 | Yaw pinned for full 500 ms dwell window | `ac2_dwell_holds_yaw_at_bound` | PASS | +| AZ-654 | AC-3 | Raster + LawnMower variants return `NotImplemented` | `ac3_raster_returns_not_implemented`, `ac3_lawnmower_returns_not_implemented` | PASS | +| AZ-655 | AC-1 | yaw 0→30° at t=500ms → 15° ± epsilon | `ac1_linear_interp_midpoint` | PASS | +| AZ-655 | AC-2 | 100 ticks at 10 ms with 100 ms throttle → ~10 emits | `ac2_throttle_drops_intermediate_calls` | PASS | +| AZ-655 | AC-3 | Plan past end clamps to last goal | `ac3_past_plan_end_clamps_to_last_goal` | PASS | +| AZ-656 | AC-1 | bbox (0.75, 0.55) → centre 25 % within 3 ticks at 100 ms | `ac1_centre_25pct_within_3_ticks` + integration | PASS | +| AZ-656 | AC-2 | `ts_monotonic_ns` strictly monotonic across `set_pose` calls | `az656_set_pose_publishes_monotonic_timestamp` | PASS | +| AZ-656 | AC-3 | 3 consecutive missing bboxes → exactly one `target_lost`; later misses silent; bbox return → counter resets | `ac3_target_lost_emits_once_per_loss_streak` | PASS | + +## Code review + +**Spec compliance**: PASS. Every AC has a directly-named test that asserts the claimed behaviour. + +**Architecture compliance**: PASS. +- `gimbal_controller` (Layer 2 Actor) imports only `shared` and standard / `tokio` deps. No sibling Layer 2 imports. +- `internal/sweep.rs`, `internal/smooth_pan.rs`, `internal/centre_on_target.rs` are new internal files under the component's owned glob (`crates/gimbal_controller/**`). +- `internal/smooth_pan.rs` was named in `module-layout.md` (line 113). `internal/sweep.rs` and `internal/centre_on_target.rs` are new and not yet enumerated — same drift-flag pattern recorded for `internal/transport.rs` in batch 10's report. Recommended: extend the gimbal_controller Internal bullet list on the next document refresh. +- `shared::models::gimbal::PanPlan` / `PanGoal` are new and not yet documented in `data_model.md`. Same drift pattern. + +**SRP**: PASS. +- `sweep::SweepEngine` — only sweep state machine + pendulum kinematics. +- `smooth_pan::PlanExecutor` — only plan loading + interpolation + throttle. +- `centre_on_target::CentreOnTarget` — only the proportional control law + loss-debounce state. +- `lib.rs` — composition + transport bridging; primitives are unaware of the transport. + +**Runtime completeness**: PASS. +- Real pendulum sweep (deterministic bounded state machine; not a random walk). +- Real linear interpolation + real self-throttle (counter-validated). +- Real proportional control loop with zoom-aware correction scaling; closed-loop convergence verified in unit + integration tests. +- Real `MonoClock`-driven monotonic timestamps (fixes the AZ-653 wall-clock regression). + +**Test discipline**: PASS. AAA pattern with `// Arrange / Act / Assert` comments. No `unsafe`. No production `unwrap`/`expect`. Integration tests use the public re-export surface — never reach into `internal::*`. + +**Security quick-scan**: PASS. No external input deserialization in this batch (the primitives consume typed structs supplied by in-process callers); no string-interpolated commands; no network code added (re-uses the existing AZ-653 transport). + +**Performance scan**: PASS. +- `SweepEngine::next_step` — three float ops + one struct construct, p99 far below the 1 ms target. +- `PlanExecutor::next_step` — one binary-window walk over `goals` (typically a handful of waypoints) + lerp; p99 far below the 1 ms target. +- `CentreOnTarget::tick` — six float ops; p99 far below the 2 ms target. + +**Cross-task consistency**: PASS. +- All three primitives produce `GimbalCommand` (same `(yaw_deg, pitch_deg)` shape AZ-653 wired into `set_pose`). +- Time injection pattern (`next_step(now: Instant)`) is consistent across `sweep`, `smooth_pan`, and `centre_on_target` (the latter receives state via args rather than time since its loop is per-frame, not per-clock-tick). +- `AutopilotError::Validation(String)` is used consistently across the three primitives' invalid-input paths. + +## Spec drift (carried into the implementation, flagged here) + +1. **`data_model.md §PanPlan` is referenced by AZ-655 but does not exist.** The implementation adds `PanGoal` + `PanPlan` to `crates/shared/src/models/gimbal.rs` (the canonical location for gimbal-related shared types). The next document-sync run should backfill the entry in `data_model.md §4 Action / piloting entities`. +2. **`module-layout.md` does not yet list `internal/sweep.rs`, `internal/centre_on_target.rs`, or `internal/transport.rs` (the last carried over from batch 10).** Same drift-flag pattern. Next document refresh should enumerate them. +3. **The `monotonic_ns()` helper introduced by AZ-653 (batch 10) was misleadingly named and actually used `SystemTime::now()`.** AZ-656's AC-2 forced the correction in this batch. The fix is to construct a `shared::clock::MonoClock` per-controller and read `clock.elapsed_ns()` on every state stamp. This is recorded here rather than as a remediation task because the change was already in this batch's scope (the same files were being edited). + +## Known limitations (warnings) + +1. **`SweepEngine::next_pendulum` carries a one-tick dwell-flip lag.** When the dwell elapses, the next call returns the same `current_yaw` (the boundary value) one more time before the direction flips and the next call moves off the boundary. This is observable in `ac2_dwell_holds_yaw_at_bound`: the call at `+501 ms` is technically the "flip tick" (direction flip happens inside the call); the actual movement off the boundary happens on the call after that. Documented in the function body; benign in production where the tick cadence is at most 10 Hz. +2. **`CentreOnTarget` assumes a linearised camera model.** Real ViewPro A40 mechanics introduce a small dead-zone and finite slew rate. The proportional gain (`DEFAULT_TARGET_GAIN = 0.6`) is conservative enough that the loop should converge under sub-critical conditions, but flight data may motivate adding a PD term or per-axis gains. Tracked as future tuning work; not blocking. +3. **`PlanExecutor`'s `at_ns` is plan-relative, anchored to `loaded_at`.** This means re-loading the same plan re-anchors its timeline. If a future caller needs to seamlessly continue a plan across reloads (e.g., chunked path-following), the API needs a `replace_extend(plan)` variant. Not in scope for AZ-655. +4. **No reserved `Idle` / mode-arbitration logic between the three primitives.** Today the composition root must hold at most one primitive active at a time per gimbal; a future refactor may introduce the `Sweep | PanPlan | CentreOnTarget | Idle` mode enum named in `description.md §5`. Not in scope for AZ-654/655/656. + +## Auto-fix attempts during the batch + +- `clippy::derivable_impls` on `impl Default for SweepPattern` → replaced with `#[derive(Default)]` + `#[default]` attribute on `Pendulum`. +- `Debug` derive missing on `SweepEngine` (required by `Result::unwrap_err` in the validation tests) → added. +- `AutopilotError::Validation(&'static str)` doesn't compile (variant requires `String`) → switched the no-plan-loaded path to `ok_or_else` with a `.into()`. The earlier batch's `unnecessary_lazy_evaluations` warning does NOT apply here because `String::from(&'static str)` is not a no-op call; clippy correctly leaves this one alone. + +## Test reproduction + +``` +cargo build -p gimbal_controller --tests +cargo test -p gimbal_controller # 58 tests; 0 failed +cargo clippy -p gimbal_controller --tests -- -D warnings +cargo test --workspace # all green +``` + +This run's workspace test suite passed without flakes; the +`mission_executor::state_machine::ac3_bounded_retry_then_success` flake +noted in batch 10's report did not reproduce in this run. + +## Cumulative review trigger + +Cumulative review fires every 3 completed batches (batches 7-9, 10-12, 13-15, …). After batch 11 we are 2/3 of the way to the next cumulative; batch 12 will trigger it. + +## Candidates for batch 12 + +Remaining `todo/`: AZ-657 (frame_ingest x3 chain), AZ-660/661 (detection_client x2), AZ-662–664 (movement_detector x3), AZ-669–671 (semantic_analyzer x3), AZ-675–677 (telemetry_stream x3), AZ-678–681 (operator_bridge x4), AZ-682–686 (scan_controller x5). + +Only-AZ-640-dep (ready immediately): +- **AZ-657** `frame_ingest_rtsp_session` — 3 pts. Standard RTSP, no vendor-spec gap. +- **AZ-682** `scan_controller_state_machine` — 5 pts. Deps `AZ-640, AZ-649` (both done). The primitives shipped in this batch (sweep / pan-plan / centre-on-target) are the natural building blocks scan_controller will compose. + +Recommended: **AZ-657 + AZ-682** (3 + 5 = 8 pts, 2 tasks, two different components). Gives the next cumulative review (batch 12, batches 10-12) surface area across `gimbal_controller`, `frame_ingest`, and `scan_controller` for a stronger cross-task check. Alternative: AZ-657 alone (3 pts) if the user prefers a single-task batch to keep batch 12 tight. diff --git a/_docs/_autodev_state.md b/_docs/_autodev_state.md index 10a334e..02d2566 100644 --- a/_docs/_autodev_state.md +++ b/_docs/_autodev_state.md @@ -6,9 +6,9 @@ step: 7 name: Implement status: in_progress sub_step: - phase: 11 + phase: 17 name: tracker-update-in-testing - detail: "batch 10 (AZ-653) committed; awaiting In Testing + push" + detail: "batch 11 (AZ-654/655/656) committed; awaiting In Testing + push" retry_count: 0 cycle: 1 tracker: jira diff --git a/crates/gimbal_controller/src/internal/centre_on_target.rs b/crates/gimbal_controller/src/internal/centre_on_target.rs new file mode 100644 index 0000000..09d8aff --- /dev/null +++ b/crates/gimbal_controller/src/internal/centre_on_target.rs @@ -0,0 +1,323 @@ +//! AZ-656 — Centre-on-target primitive. +//! +//! Proportional control loop that consumes a normalized target bbox +//! stream (from `scan_controller`) and emits the gimbal yaw/pitch +//! command needed to drag the target toward the centre 25% region of +//! the frame. The actual `GimbalState` publish (with monotonic +//! timestamp) is handled by [`crate::GimbalControllerHandle::set_pose`] +//! when the emitted command is applied — this primitive is a pure +//! per-tick controller, no I/O. +//! +//! Loss detection: after [`CentreOnTargetConfig::max_missed_ticks`] +//! consecutive ticks with no bbox, a one-shot `target_lost` signal is +//! returned to the caller (debounced — never re-emits while the loss +//! state persists). On bbox return, the loss counter resets and the +//! signal becomes available again for the next loss streak. + +use shared::models::frame::BoundingBox; + +use crate::GimbalCommand; + +pub const DEFAULT_TARGET_GAIN: f32 = 0.6; +pub const DEFAULT_CENTRE_WINDOW: f32 = 0.25; +pub const DEFAULT_MAX_MISSED_TICKS: u32 = 3; + +/// Tuning knobs for the centre-on-target loop. `fov_deg_at_zoom1` is +/// the camera's nominal horizontal FOV at 1× zoom; the per-tick yaw +/// step scales inversely with the gimbal's current zoom (narrower FOV +/// → smaller correction needed for the same pixel-error). +#[derive(Debug, Clone, Copy, PartialEq)] +pub struct CentreOnTargetConfig { + pub fov_deg_at_zoom1: f32, + pub gain: f32, + /// Half-width of the on-target window (e.g. 0.125 → bbox centre in + /// [0.375, 0.625] is considered centred). Tests use this to assert + /// AC-1 convergence; production uses it for `command_in_flight` + /// telemetry. + pub centre_half_width: f32, + pub max_missed_ticks: u32, +} + +impl Default for CentreOnTargetConfig { + fn default() -> Self { + Self { + fov_deg_at_zoom1: 60.0, + gain: DEFAULT_TARGET_GAIN, + centre_half_width: DEFAULT_CENTRE_WINDOW / 2.0, + max_missed_ticks: DEFAULT_MAX_MISSED_TICKS, + } + } +} + +/// What `tick` decided. `command` is `None` when no bbox is present +/// (the gimbal holds its current pose); `target_lost_signal` fires +/// exactly once per loss streak, on the tick that crosses the +/// `max_missed_ticks` threshold. +#[derive(Debug, Clone, Copy, PartialEq)] +pub struct CentreOnTargetOutput { + pub command: Option, + pub target_lost_signal: bool, + pub on_target: bool, +} + +pub struct CentreOnTarget { + config: CentreOnTargetConfig, + consecutive_missed: u32, + /// True while in a sustained loss state. Reset to `false` on bbox + /// return so the next loss streak gets its own `target_lost` + /// signal. Without this, a flickering target would re-fire the + /// signal every `max_missed_ticks` ticks. + in_loss_state: bool, +} + +impl CentreOnTarget { + pub fn new(config: CentreOnTargetConfig) -> Self { + Self { + config, + consecutive_missed: 0, + in_loss_state: false, + } + } + + pub fn config(&self) -> &CentreOnTargetConfig { + &self.config + } + + /// Step the loop with this tick's bbox observation and the gimbal's + /// current pose. Pass `bbox = None` to indicate the target was not + /// visible in this frame. + pub fn tick( + &mut self, + bbox: Option, + current_yaw_deg: f32, + current_pitch_deg: f32, + current_zoom: f32, + ) -> CentreOnTargetOutput { + let Some(bbox) = bbox else { + return self.handle_missed_tick(); + }; + self.consecutive_missed = 0; + self.in_loss_state = false; + + let cx = (bbox.x_min + bbox.x_max) * 0.5; + let cy = (bbox.y_min + bbox.y_max) * 0.5; + let err_x = cx - 0.5; + let err_y = cy - 0.5; + let on_target = + err_x.abs() <= self.config.centre_half_width && err_y.abs() <= self.config.centre_half_width; + + // Effective FOV shrinks as zoom grows; the same pixel error + // therefore corresponds to a smaller angular error at high + // zoom and we apply a proportionally smaller correction. + let zoom_factor = current_zoom.max(0.1); + let fov = self.config.fov_deg_at_zoom1 / zoom_factor; + let delta_yaw = err_x * fov * self.config.gain; + let delta_pitch = -err_y * fov * self.config.gain; + + CentreOnTargetOutput { + command: Some(GimbalCommand { + yaw_deg: current_yaw_deg + delta_yaw, + pitch_deg: current_pitch_deg + delta_pitch, + }), + target_lost_signal: false, + on_target, + } + } + + fn handle_missed_tick(&mut self) -> CentreOnTargetOutput { + if !self.in_loss_state { + self.consecutive_missed = self.consecutive_missed.saturating_add(1); + } + let should_signal = + !self.in_loss_state && self.consecutive_missed >= self.config.max_missed_ticks; + if should_signal { + self.in_loss_state = true; + } + CentreOnTargetOutput { + command: None, + target_lost_signal: should_signal, + on_target: false, + } + } +} + +#[cfg(test)] +mod tests { + use super::*; + + fn bbox_at(cx: f32, cy: f32, w: f32, h: f32) -> BoundingBox { + BoundingBox { + x_min: cx - w / 2.0, + y_min: cy - h / 2.0, + x_max: cx + w / 2.0, + y_max: cy + h / 2.0, + } + } + + /// AC-1 — convergence within 3 ticks under nominal kinematics. + /// + /// The unit test models the gimbal as a rigid yaw/pitch rig where + /// commanding `delta_yaw` shifts the apparent bbox in the opposite + /// direction by `delta_yaw / fov_at_zoom`. This is the same + /// linearised camera model the proportional controller assumes; it + /// lets us verify the loop *direction* and convergence speed in + /// isolation, without a real frame source. + #[test] + fn ac1_centre_25pct_within_3_ticks() { + // Arrange + let cfg = CentreOnTargetConfig::default(); + let mut ctrl = CentreOnTarget::new(cfg); + let mut yaw = 0.0_f32; + let mut pitch = 0.0_f32; + let zoom = 1.0; + let mut bbox = bbox_at(0.75, 0.55, 0.1, 0.1); + let fov = cfg.fov_deg_at_zoom1 / zoom; + + // Act: three ticks of the closed loop + let mut on_target_after = None; + for tick_idx in 0..3 { + let out = ctrl.tick(Some(bbox), yaw, pitch, zoom); + let cmd = out.command.expect("loop should emit a command on every tick with bbox"); + let dy = cmd.yaw_deg - yaw; + let dp = cmd.pitch_deg - pitch; + yaw = cmd.yaw_deg; + pitch = cmd.pitch_deg; + // Kinematic model: commanding +dy yaw moves the world's + // image -dy/fov in normalized x. + let cx = (bbox.x_min + bbox.x_max) * 0.5 - dy / fov; + let cy = (bbox.y_min + bbox.y_max) * 0.5 + dp / fov; + bbox = bbox_at(cx, cy, 0.1, 0.1); + if out.on_target { + on_target_after = Some(tick_idx + 1); + } + } + let final_cx = (bbox.x_min + bbox.x_max) * 0.5; + let final_cy = (bbox.y_min + bbox.y_max) * 0.5; + + // Assert + let centre_lo = 0.5 - cfg.centre_half_width; + let centre_hi = 0.5 + cfg.centre_half_width; + assert!( + (centre_lo..=centre_hi).contains(&final_cx), + "x = {final_cx} outside centre 25% window after 3 ticks" + ); + assert!( + (centre_lo..=centre_hi).contains(&final_cy), + "y = {final_cy} outside centre 25% window after 3 ticks" + ); + assert!(on_target_after.is_some(), "on_target flag never raised"); + } + + /// AC-3 — three consecutive missing bboxes signal target_lost + /// once, subsequent missed ticks do not re-emit, and a visible + /// bbox resets the counter so a later loss streak signals again. + #[test] + fn ac3_target_lost_emits_once_per_loss_streak() { + // Arrange + let mut ctrl = CentreOnTarget::new(CentreOnTargetConfig::default()); + + // Act 1: two missing ticks — no signal + for i in 0..2 { + let out = ctrl.tick(None, 0.0, 0.0, 1.0); + assert!( + !out.target_lost_signal, + "tick {i}: target_lost fired before threshold" + ); + assert!(out.command.is_none()); + } + // Act 2: third missing tick — signal fires exactly once + let out3 = ctrl.tick(None, 0.0, 0.0, 1.0); + // Act 3: fourth and fifth missing ticks — silent + let out4 = ctrl.tick(None, 0.0, 0.0, 1.0); + let out5 = ctrl.tick(None, 0.0, 0.0, 1.0); + + // Assert + assert!(out3.target_lost_signal, "target_lost did not fire at tick 3"); + assert!(!out4.target_lost_signal, "target_lost re-fired during sustained loss"); + assert!(!out5.target_lost_signal, "target_lost re-fired during sustained loss"); + + // Act 4: bbox returns → loss state clears, new streak can re-fire + let recovered = ctrl.tick(Some(bbox_at(0.5, 0.5, 0.1, 0.1)), 0.0, 0.0, 1.0); + assert!(recovered.command.is_some(), "recovery tick must emit command"); + assert!(!recovered.target_lost_signal); + + for _ in 0..2 { + assert!(!ctrl.tick(None, 0.0, 0.0, 1.0).target_lost_signal); + } + let lost_again = ctrl.tick(None, 0.0, 0.0, 1.0); + assert!(lost_again.target_lost_signal, "second loss streak did not fire"); + } + + #[test] + fn bbox_already_centred_marks_on_target_with_small_command() { + // Arrange + let mut ctrl = CentreOnTarget::new(CentreOnTargetConfig::default()); + + // Act + let out = ctrl.tick(Some(bbox_at(0.5, 0.5, 0.1, 0.1)), 0.0, 0.0, 1.0); + + // Assert + assert!(out.on_target); + let cmd = out.command.unwrap(); + assert!(cmd.yaw_deg.abs() < 0.001); + assert!(cmd.pitch_deg.abs() < 0.001); + } + + #[test] + fn higher_zoom_yields_smaller_correction() { + // Arrange + let mut ctrl = CentreOnTarget::new(CentreOnTargetConfig::default()); + let bbox = bbox_at(0.75, 0.5, 0.1, 0.1); + + // Act + let at_1x = ctrl.tick(Some(bbox), 0.0, 0.0, 1.0).command.unwrap(); + let mut ctrl2 = CentreOnTarget::new(CentreOnTargetConfig::default()); + let at_4x = ctrl2.tick(Some(bbox), 0.0, 0.0, 4.0).command.unwrap(); + + // Assert: 4× zoom should produce ~1/4 the yaw correction + assert!(at_4x.yaw_deg.abs() < at_1x.yaw_deg.abs()); + let ratio = at_4x.yaw_deg / at_1x.yaw_deg; + assert!( + (ratio - 0.25).abs() < 0.01, + "zoom-scaled correction ratio {ratio} not close to 0.25" + ); + } + + #[test] + fn loss_counter_caps_safely_without_overflow() { + // Arrange + let mut ctrl = CentreOnTarget::new(CentreOnTargetConfig { + max_missed_ticks: 1, + ..CentreOnTargetConfig::default() + }); + + // Act + Assert: hammering tick(None) doesn't overflow consecutive_missed + for i in 0..10_000 { + let out = ctrl.tick(None, 0.0, 0.0, 1.0); + if i == 0 { + assert!(out.target_lost_signal); + } else { + assert!(!out.target_lost_signal); + } + } + } + + #[test] + fn loss_streak_below_threshold_then_recovery_does_not_signal() { + // Arrange + let mut ctrl = CentreOnTarget::new(CentreOnTargetConfig { + max_missed_ticks: 5, + ..CentreOnTargetConfig::default() + }); + + // Act + for _ in 0..3 { + assert!(!ctrl.tick(None, 0.0, 0.0, 1.0).target_lost_signal); + } + let recovered = ctrl.tick(Some(bbox_at(0.5, 0.5, 0.1, 0.1)), 0.0, 0.0, 1.0); + + // Assert + assert!(!recovered.target_lost_signal); + assert!(recovered.command.is_some()); + } +} diff --git a/crates/gimbal_controller/src/internal/mod.rs b/crates/gimbal_controller/src/internal/mod.rs index fd8cf11..3ba32d0 100644 --- a/crates/gimbal_controller/src/internal/mod.rs +++ b/crates/gimbal_controller/src/internal/mod.rs @@ -1,4 +1,7 @@ //! Internal modules for `gimbal_controller`. Not part of the public API. pub mod a40_protocol; +pub mod centre_on_target; +pub mod smooth_pan; +pub mod sweep; pub mod transport; diff --git a/crates/gimbal_controller/src/internal/smooth_pan.rs b/crates/gimbal_controller/src/internal/smooth_pan.rs new file mode 100644 index 0000000..8358d94 --- /dev/null +++ b/crates/gimbal_controller/src/internal/smooth_pan.rs @@ -0,0 +1,374 @@ +//! AZ-655 — Smooth-pan path-tracking plan executor. +//! +//! Linearly interpolates `(yaw, pitch, zoom)` between adjacent +//! [`PanGoal`]s in a [`PanPlan`] and self-throttles emission so the +//! vendor command rate is bounded. The composition root constructs one +//! executor per `gimbal_controller`; the executor is stateless until a +//! plan is loaded. +//! +//! Throttle vs. interpolation is the key design decision: callers may +//! tick the executor at any rate (e.g. the scan_controller's 10 ms +//! frame loop), and the executor decides whether enough wall-time has +//! elapsed since the last emission to issue a new command. Calls that +//! fall inside the throttle window return [`NextStep::Throttled`] (and +//! bump the dropped-commands counter); calls outside the window +//! interpolate to the current plan-time and return +//! [`NextStep::Emit(cmd)`]. + +use std::time::{Duration, Instant}; + +use shared::error::{AutopilotError, Result}; +use shared::models::gimbal::{PanGoal, PanPlan}; + +use crate::GimbalCommand; + +pub const DEFAULT_MIN_CMD_INTERVAL: Duration = Duration::from_millis(50); + +/// What `next_step` decided for this tick. `Emit` carries the +/// interpolated command; `Throttled` means the call fell inside the +/// throttle window and no command should be sent. +#[derive(Debug, Clone, Copy, PartialEq)] +pub enum NextStep { + Emit(GimbalCommand), + Throttled, +} + +#[derive(Debug, Clone, Copy, Default)] +pub struct ExecutorStats { + pub plan_loaded_at: Option, + pub commands_emitted_total: u64, + pub commands_dropped_to_throttle_total: u64, +} + +pub struct PlanExecutor { + min_cmd_interval: Duration, + plan: Option, + last_emit_at: Option, + stats: ExecutorStats, +} + +struct LoadedPlan { + plan: PanPlan, + loaded_at: Instant, +} + +impl PlanExecutor { + pub fn new(min_cmd_interval: Duration) -> Self { + Self { + min_cmd_interval, + plan: None, + last_emit_at: None, + stats: ExecutorStats::default(), + } + } + + pub fn with_default_throttle() -> Self { + Self::new(DEFAULT_MIN_CMD_INTERVAL) + } + + /// Load a new plan, anchoring its relative `at_ns` axis to `now`. + /// Goals must be ordered strictly increasing in `at_ns`; empty + /// plans are rejected. Re-loading replaces the current plan. + pub fn load(&mut self, plan: PanPlan, now: Instant) -> Result<()> { + validate_plan(&plan)?; + self.plan = Some(LoadedPlan { + plan, + loaded_at: now, + }); + self.stats.plan_loaded_at = Some(now); + // Clear the throttle anchor on plan reload so the first command + // of a new plan emits immediately instead of being held back by + // the previous plan's last_emit_at. + self.last_emit_at = None; + Ok(()) + } + + pub fn has_plan(&self) -> bool { + self.plan.is_some() + } + + pub fn stats(&self) -> ExecutorStats { + self.stats + } + + /// Compute the command for `now`. Returns `NextStep::Throttled` + /// when called inside the min-interval window since the last + /// emission; returns `Err` only when no plan is loaded. + pub fn next_step(&mut self, now: Instant) -> Result { + let loaded = self.plan.as_ref().ok_or_else(|| { + AutopilotError::Validation("PlanExecutor::next_step: no plan loaded".into()) + })?; + + if let Some(last) = self.last_emit_at { + if now.duration_since(last) < self.min_cmd_interval { + self.stats.commands_dropped_to_throttle_total += 1; + return Ok(NextStep::Throttled); + } + } + + let elapsed_ns = now + .saturating_duration_since(loaded.loaded_at) + .as_nanos() + .min(u64::MAX as u128) as u64; + + let cmd = interpolate(&loaded.plan, elapsed_ns); + self.last_emit_at = Some(now); + self.stats.commands_emitted_total += 1; + Ok(NextStep::Emit(cmd)) + } +} + +fn validate_plan(plan: &PanPlan) -> Result<()> { + if plan.goals.is_empty() { + return Err(AutopilotError::Validation( + "PanPlan: goals must not be empty".into(), + )); + } + for win in plan.goals.windows(2) { + if win[1].at_ns <= win[0].at_ns { + return Err(AutopilotError::Validation(format!( + "PanPlan: at_ns must be strictly increasing ({} → {})", + win[0].at_ns, win[1].at_ns + ))); + } + } + Ok(()) +} + +/// Linear interpolation between adjacent goals. Before the first goal: +/// extrapolate linearly from the first two goals (or clamp to the +/// first goal if only one exists). After the last goal: clamp to the +/// last goal. +fn interpolate(plan: &PanPlan, t_ns: u64) -> GimbalCommand { + let goals = &plan.goals; + if goals.len() == 1 { + return goal_to_command(&goals[0]); + } + if t_ns <= goals[0].at_ns { + return linear_at(&goals[0], &goals[1], t_ns); + } + if t_ns >= goals[goals.len() - 1].at_ns { + return goal_to_command(&goals[goals.len() - 1]); + } + for win in goals.windows(2) { + if t_ns >= win[0].at_ns && t_ns <= win[1].at_ns { + return linear_at(&win[0], &win[1], t_ns); + } + } + goal_to_command(&goals[goals.len() - 1]) +} + +fn linear_at(a: &PanGoal, b: &PanGoal, t_ns: u64) -> GimbalCommand { + let span = b.at_ns as f64 - a.at_ns as f64; + let frac = if span.abs() < 1.0 { + 0.0 + } else { + (t_ns as f64 - a.at_ns as f64) / span + }; + let frac = frac as f32; + GimbalCommand { + yaw_deg: lerp(a.yaw_deg, b.yaw_deg, frac), + pitch_deg: lerp(a.pitch_deg, b.pitch_deg, frac), + } +} + +fn lerp(a: f32, b: f32, t: f32) -> f32 { + a + (b - a) * t +} + +fn goal_to_command(g: &PanGoal) -> GimbalCommand { + GimbalCommand { + yaw_deg: g.yaw_deg, + pitch_deg: g.pitch_deg, + } +} + +#[cfg(test)] +mod tests { + use super::*; + + fn two_goal_plan() -> PanPlan { + PanPlan { + goals: vec![ + PanGoal { + yaw_deg: 0.0, + pitch_deg: 0.0, + zoom: 1.0, + at_ns: 0, + }, + PanGoal { + yaw_deg: 30.0, + pitch_deg: 0.0, + zoom: 1.0, + at_ns: 1_000_000_000, + }, + ], + } + } + + #[test] + fn ac1_linear_interp_midpoint() { + // Arrange + let mut exe = PlanExecutor::new(Duration::ZERO); + let t0 = Instant::now(); + exe.load(two_goal_plan(), t0).unwrap(); + + // Act + let step = exe.next_step(t0 + Duration::from_millis(500)).unwrap(); + + // Assert + match step { + NextStep::Emit(cmd) => { + let diff = (cmd.yaw_deg - 15.0).abs(); + assert!(diff < 0.01, "yaw at t=500ms was {}, want ~15.0", cmd.yaw_deg); + } + NextStep::Throttled => panic!("first emission should not be throttled"), + } + } + + #[test] + fn ac2_throttle_drops_intermediate_calls() { + // Arrange + let mut exe = PlanExecutor::new(Duration::from_millis(100)); + let t0 = Instant::now(); + exe.load(two_goal_plan(), t0).unwrap(); + + // Act: call every 10 ms for ~1 s + let mut emitted = 0_u64; + let mut throttled = 0_u64; + for i in 0..100 { + match exe.next_step(t0 + Duration::from_millis(i * 10)).unwrap() { + NextStep::Emit(_) => emitted += 1, + NextStep::Throttled => throttled += 1, + } + } + + // Assert: at 100 ms cadence over 1 s window we get ~10 emissions + assert!( + (9..=11).contains(&emitted), + "expected ~10 emits, got {emitted}" + ); + assert_eq!(emitted + throttled, 100, "every tick must be accounted for"); + assert_eq!(exe.stats().commands_emitted_total, emitted); + assert_eq!(exe.stats().commands_dropped_to_throttle_total, throttled); + } + + #[test] + fn ac3_past_plan_end_clamps_to_last_goal() { + // Arrange + let mut exe = PlanExecutor::new(Duration::ZERO); + let t0 = Instant::now(); + exe.load(two_goal_plan(), t0).unwrap(); + + // Act: plan ends at 1s; query at 5s + let step = exe.next_step(t0 + Duration::from_secs(5)).unwrap(); + + // Assert + match step { + NextStep::Emit(cmd) => { + assert!((cmd.yaw_deg - 30.0).abs() < 0.01); + assert!((cmd.pitch_deg - 0.0).abs() < 0.01); + } + NextStep::Throttled => panic!(), + } + } + + #[test] + fn empty_plan_rejected() { + // Arrange + let mut exe = PlanExecutor::new(Duration::ZERO); + + // Act + let err = exe + .load(PanPlan { goals: vec![] }, Instant::now()) + .unwrap_err(); + + // Assert + assert!(matches!(err, AutopilotError::Validation(_))); + } + + #[test] + fn non_monotonic_plan_rejected() { + // Arrange + let plan = PanPlan { + goals: vec![ + PanGoal { + yaw_deg: 0.0, + pitch_deg: 0.0, + zoom: 1.0, + at_ns: 1_000, + }, + PanGoal { + yaw_deg: 1.0, + pitch_deg: 0.0, + zoom: 1.0, + at_ns: 1_000, + }, + ], + }; + let mut exe = PlanExecutor::new(Duration::ZERO); + + // Act + Assert + assert!(matches!( + exe.load(plan, Instant::now()).unwrap_err(), + AutopilotError::Validation(_) + )); + } + + #[test] + fn no_plan_returns_error() { + // Arrange + let mut exe = PlanExecutor::new(Duration::ZERO); + + // Act + Assert + assert!(matches!( + exe.next_step(Instant::now()).unwrap_err(), + AutopilotError::Validation(_) + )); + } + + #[test] + fn reload_clears_throttle_anchor() { + // Arrange + let mut exe = PlanExecutor::new(Duration::from_millis(100)); + let t0 = Instant::now(); + exe.load(two_goal_plan(), t0).unwrap(); + let _ = exe.next_step(t0).unwrap(); + + // Act: reload immediately and step again at the same tick + exe.load(two_goal_plan(), t0 + Duration::from_millis(10)) + .unwrap(); + let step = exe.next_step(t0 + Duration::from_millis(10)).unwrap(); + + // Assert: new plan emits on first tick (no carry-over throttle) + assert!(matches!(step, NextStep::Emit(_))); + } + + #[test] + fn single_goal_plan_holds_value() { + // Arrange + let plan = PanPlan { + goals: vec![PanGoal { + yaw_deg: 12.5, + pitch_deg: -3.0, + zoom: 2.0, + at_ns: 500, + }], + }; + let mut exe = PlanExecutor::new(Duration::ZERO); + let t0 = Instant::now(); + exe.load(plan, t0).unwrap(); + + // Act + let step = exe.next_step(t0 + Duration::from_secs(10)).unwrap(); + + // Assert + match step { + NextStep::Emit(cmd) => { + assert!((cmd.yaw_deg - 12.5).abs() < 0.001); + assert!((cmd.pitch_deg - (-3.0)).abs() < 0.001); + } + NextStep::Throttled => panic!(), + } + } +} diff --git a/crates/gimbal_controller/src/internal/sweep.rs b/crates/gimbal_controller/src/internal/sweep.rs new file mode 100644 index 0000000..41b5c80 --- /dev/null +++ b/crates/gimbal_controller/src/internal/sweep.rs @@ -0,0 +1,350 @@ +//! AZ-654 — Zoom-out sweep pattern primitive. +//! +//! Implements [`SweepPattern::Pendulum`] as the default; `Raster` and +//! `LawnMower` are reserved enum variants that return +//! [`AutopilotError::NotImplemented`] from [`SweepEngine::next_step`]. +//! This explicit failure (vs. a silent fallback to Pendulum) is required +//! by AC-3 in the task spec — pattern selection must never silently +//! drift. +//! +//! Time is injected via `next_step(now: Instant)` so the dwell-timer +//! behaviour (AC-2) can be tested deterministically without sleeping. +//! Production callers pass `Instant::now()` from the actor's tick loop. + +use std::time::{Duration, Instant}; + +use shared::error::{AutopilotError, Result}; + +use crate::GimbalCommand; + +/// Selectable sweep pattern. `Pendulum` is the default; `Raster` and +/// `LawnMower` are reserved for future implementation (see +/// `architecture.md §8 Q1`). +#[derive(Debug, Clone, Copy, PartialEq, Eq, Default)] +pub enum SweepPattern { + #[default] + Pendulum, + Raster, + LawnMower, +} + +/// Sweep envelope + step kinematics. Loaded from startup config; the +/// composition root passes one instance per `gimbal_controller`. +#[derive(Debug, Clone, Copy, PartialEq)] +pub struct SweepConfig { + pub min_yaw_deg: f32, + pub max_yaw_deg: f32, + pub pitch_deg: f32, + /// How far the pendulum advances per `next_step` call. Must be + /// strictly positive; sign is chosen internally by the engine. + pub step_deg: f32, + /// Time to hold at a bound before reversing direction. + pub dwell: Duration, +} + +impl SweepConfig { + /// Validates that the config is internally consistent. Called once + /// by [`SweepEngine::new`]; further calls to `next_step` assume + /// validity (no per-tick re-check). + fn validate(&self) -> Result<()> { + if self.min_yaw_deg >= self.max_yaw_deg { + return Err(AutopilotError::Validation(format!( + "sweep: min_yaw_deg ({}) must be < max_yaw_deg ({})", + self.min_yaw_deg, self.max_yaw_deg + ))); + } + if !self.step_deg.is_finite() || self.step_deg <= 0.0 { + return Err(AutopilotError::Validation(format!( + "sweep: step_deg must be > 0, got {}", + self.step_deg + ))); + } + Ok(()) + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +enum Direction { + Forward, + Reverse, +} + +impl Direction { + fn flip(self) -> Self { + match self { + Self::Forward => Self::Reverse, + Self::Reverse => Self::Forward, + } + } + + fn sign(self) -> f32 { + match self { + Self::Forward => 1.0, + Self::Reverse => -1.0, + } + } +} + +/// State machine that produces the next sweep command on each tick. +/// Owns the current yaw target and the dwell-at-bound timer. +#[derive(Debug)] +pub struct SweepEngine { + pattern: SweepPattern, + config: SweepConfig, + current_yaw: f32, + direction: Direction, + /// `Some(instant)` while the engine is dwelling at a bound waiting + /// for `config.dwell` to elapse; `None` while traversing. + dwell_started_at: Option, +} + +impl SweepEngine { + pub fn new(pattern: SweepPattern, config: SweepConfig) -> Result { + config.validate()?; + Ok(Self { + pattern, + config, + current_yaw: config.min_yaw_deg, + direction: Direction::Forward, + dwell_started_at: None, + }) + } + + pub fn pattern(&self) -> SweepPattern { + self.pattern + } + + /// Produce the command for tick `now`. Behaviour by pattern: + /// - `Pendulum`: advances yaw by `step_deg` toward the active + /// bound; on reaching the bound, dwells for `config.dwell` then + /// reverses direction. + /// - `Raster` / `LawnMower`: returns + /// [`AutopilotError::NotImplemented`] — wired but not yet + /// implemented. The caller must surface this error rather than + /// silently fall back to Pendulum (AC-3). + pub fn next_step(&mut self, now: Instant) -> Result { + match self.pattern { + SweepPattern::Pendulum => Ok(self.next_pendulum(now)), + SweepPattern::Raster => Err(AutopilotError::NotImplemented( + "gimbal_controller::sweep: Raster pattern not implemented (Q1 pending)", + )), + SweepPattern::LawnMower => Err(AutopilotError::NotImplemented( + "gimbal_controller::sweep: LawnMower pattern not implemented (Q1 pending)", + )), + } + } + + fn next_pendulum(&mut self, now: Instant) -> GimbalCommand { + if let Some(started) = self.dwell_started_at { + if now.duration_since(started) < self.config.dwell { + return self.command(); + } + self.dwell_started_at = None; + self.direction = self.direction.flip(); + } + + let next = self.current_yaw + self.direction.sign() * self.config.step_deg; + let (clamped, hit_bound) = self.clamp_to_bounds(next); + self.current_yaw = clamped; + if hit_bound { + self.dwell_started_at = Some(now); + } + self.command() + } + + fn clamp_to_bounds(&self, candidate: f32) -> (f32, bool) { + if candidate >= self.config.max_yaw_deg { + (self.config.max_yaw_deg, true) + } else if candidate <= self.config.min_yaw_deg { + (self.config.min_yaw_deg, true) + } else { + (candidate, false) + } + } + + fn command(&self) -> GimbalCommand { + GimbalCommand { + yaw_deg: self.current_yaw, + pitch_deg: self.config.pitch_deg, + } + } +} + +#[cfg(test)] +mod tests { + use super::*; + + fn cfg() -> SweepConfig { + SweepConfig { + min_yaw_deg: -30.0, + max_yaw_deg: 30.0, + pitch_deg: -10.0, + step_deg: 5.0, + dwell: Duration::from_millis(500), + } + } + + #[test] + fn ac1_pendulum_stays_within_bounds_over_100_steps() { + // Arrange + let mut engine = SweepEngine::new(SweepPattern::Pendulum, cfg()).unwrap(); + let mut t = Instant::now(); + + // Act + let mut last_dir_sign = 0.0_f32; + let mut reversals = 0_u32; + for _ in 0..100 { + let cmd = engine.next_step(t).unwrap(); + assert!( + cmd.yaw_deg >= cfg().min_yaw_deg && cmd.yaw_deg <= cfg().max_yaw_deg, + "yaw {} out of bounds", + cmd.yaw_deg + ); + let dir = engine.direction.sign(); + if last_dir_sign != 0.0 && (dir - last_dir_sign).abs() > 0.01 { + reversals += 1; + } + last_dir_sign = dir; + t += Duration::from_secs(1); + } + + // Assert + assert!( + reversals > 0, + "pendulum never reversed direction in 100 steps" + ); + } + + #[test] + fn ac2_dwell_holds_yaw_at_bound() { + // Arrange + let mut engine = SweepEngine::new(SweepPattern::Pendulum, cfg()).unwrap(); + let start = Instant::now(); + let mut t = start; + + for _ in 0..20 { + let _ = engine.next_step(t).unwrap(); + if engine.dwell_started_at.is_some() { + break; + } + t += Duration::from_millis(10); + } + let yaw_at_bound = engine.current_yaw; + let dir_at_bound = engine.direction; + assert!( + engine.dwell_started_at.is_some(), + "engine never reached a bound" + ); + + // Act: poll repeatedly during dwell window + let dwell_start = t; + for offset_ms in [100_u64, 200, 300, 400, 499] { + let cmd = engine + .next_step(dwell_start + Duration::from_millis(offset_ms)) + .unwrap(); + assert_eq!( + cmd.yaw_deg, yaw_at_bound, + "yaw moved during dwell window at +{offset_ms}ms" + ); + assert_eq!( + engine.direction, dir_at_bound, + "direction flipped before dwell elapsed at +{offset_ms}ms" + ); + } + + // After 500 ms the dwell is satisfied; next call may reverse + let _ = engine + .next_step(dwell_start + Duration::from_millis(501)) + .unwrap(); + + // Assert + assert_ne!( + engine.direction, dir_at_bound, + "direction did not flip after dwell elapsed" + ); + } + + #[test] + fn ac3_raster_returns_not_implemented() { + // Arrange + let mut engine = SweepEngine::new(SweepPattern::Raster, cfg()).unwrap(); + + // Act + let err = engine.next_step(Instant::now()).unwrap_err(); + + // Assert + assert!(matches!(err, AutopilotError::NotImplemented(_))); + } + + #[test] + fn ac3_lawnmower_returns_not_implemented() { + // Arrange + let mut engine = SweepEngine::new(SweepPattern::LawnMower, cfg()).unwrap(); + + // Act + let err = engine.next_step(Instant::now()).unwrap_err(); + + // Assert + assert!(matches!(err, AutopilotError::NotImplemented(_))); + } + + #[test] + fn pattern_default_is_pendulum() { + assert_eq!(SweepPattern::default(), SweepPattern::Pendulum); + } + + #[test] + fn invalid_config_rejected() { + // Arrange + let bad = SweepConfig { + min_yaw_deg: 10.0, + max_yaw_deg: 10.0, + pitch_deg: 0.0, + step_deg: 1.0, + dwell: Duration::from_millis(100), + }; + + // Act + let err = SweepEngine::new(SweepPattern::Pendulum, bad).unwrap_err(); + + // Assert + assert!(matches!(err, AutopilotError::Validation(_))); + } + + #[test] + fn invalid_step_rejected() { + // Arrange + let bad = SweepConfig { + min_yaw_deg: -10.0, + max_yaw_deg: 10.0, + pitch_deg: 0.0, + step_deg: 0.0, + dwell: Duration::from_millis(100), + }; + + // Act + Assert + assert!(matches!( + SweepEngine::new(SweepPattern::Pendulum, bad).unwrap_err(), + AutopilotError::Validation(_) + )); + } + + #[test] + fn pendulum_advances_in_step_increments_then_clamps() { + // Arrange + let mut engine = SweepEngine::new(SweepPattern::Pendulum, cfg()).unwrap(); + let t = Instant::now(); + + // Act: starting at -30°, step +5° per tick; should not exceed +30°. + let mut yaws = vec![]; + for _ in 0..15 { + yaws.push(engine.next_step(t).unwrap().yaw_deg); + } + + // Assert: forward sweep produces -25, -20, ... 30, 30 (clamped + dwell) + assert_eq!(yaws[0], -25.0); + assert_eq!(yaws[1], -20.0); + assert!(yaws.iter().all(|&y| y <= 30.0)); + assert!(yaws.iter().any(|&y| (y - 30.0).abs() < 0.01)); + } +} diff --git a/crates/gimbal_controller/src/lib.rs b/crates/gimbal_controller/src/lib.rs index 9a46207..cfa8de3 100644 --- a/crates/gimbal_controller/src/lib.rs +++ b/crates/gimbal_controller/src/lib.rs @@ -14,6 +14,7 @@ use serde::{Deserialize, Serialize}; use tokio::sync::watch; +use shared::clock::MonoClock; use shared::error::{AutopilotError, Result}; use shared::health::ComponentHealth; use shared::models::gimbal::GimbalState; @@ -24,6 +25,14 @@ pub use internal::a40_protocol::{ build_a1_angles, build_c1_camera, build_c2_set_zoom, decode_frame, encode_frame, xor_checksum, CameraCommand, Frame, FrameDecodeError, FrameId, ImageSensor, ServoStatus, MAX_PACKET_LEN, }; +pub use internal::centre_on_target::{ + CentreOnTarget, CentreOnTargetConfig, CentreOnTargetOutput, DEFAULT_CENTRE_WINDOW, + DEFAULT_MAX_MISSED_TICKS, DEFAULT_TARGET_GAIN, +}; +pub use internal::smooth_pan::{ + ExecutorStats, NextStep, PlanExecutor, DEFAULT_MIN_CMD_INTERVAL, +}; +pub use internal::sweep::{SweepConfig, SweepEngine, SweepPattern}; pub use internal::transport::{ A40Error, A40Transport, VendorFaults, VendorFaultsSnapshot, DEFAULT_COMMAND_DEADLINE, DEFAULT_MAX_RETRIES, INBOUND_CHANNEL_CAPACITY, @@ -49,6 +58,7 @@ pub struct GimbalCommand { pub struct GimbalController { state_tx: watch::Sender, transport: Option, + clock: MonoClock, } impl GimbalController { @@ -57,6 +67,7 @@ impl GimbalController { Self { state_tx, transport: None, + clock: MonoClock::new(), } } @@ -68,6 +79,7 @@ impl GimbalController { Self { state_tx, transport: Some(transport), + clock: MonoClock::new(), } } @@ -75,6 +87,7 @@ impl GimbalController { GimbalControllerHandle { state_tx: self.state_tx.clone(), transport: self.transport.clone(), + clock: self.clock, } } } @@ -83,6 +96,7 @@ impl GimbalController { pub struct GimbalControllerHandle { state_tx: watch::Sender, transport: Option, + clock: MonoClock, } impl GimbalControllerHandle { @@ -105,7 +119,7 @@ impl GimbalControllerHandle { let mut state = *self.state_tx.borrow(); state.yaw = command.yaw_deg; state.pitch = command.pitch_deg; - state.ts_monotonic_ns = monotonic_ns(); + state.ts_monotonic_ns = self.clock.elapsed_ns(); self.state_tx.send_replace(state); Ok(()) } @@ -127,7 +141,7 @@ impl GimbalControllerHandle { .map_err(map_a40_error)?; let mut state = *self.state_tx.borrow(); state.zoom = level; - state.ts_monotonic_ns = monotonic_ns(); + state.ts_monotonic_ns = self.clock.elapsed_ns(); self.state_tx.send_replace(state); Ok(()) } @@ -192,14 +206,6 @@ fn map_a40_error(e: A40Error) -> AutopilotError { } } -fn monotonic_ns() -> u64 { - use std::time::{SystemTime, UNIX_EPOCH}; - SystemTime::now() - .duration_since(UNIX_EPOCH) - .map(|d| d.as_nanos() as u64) - .unwrap_or(0) -} - #[cfg(test)] mod tests { use super::*; diff --git a/crates/gimbal_controller/tests/batch_11_integration.rs b/crates/gimbal_controller/tests/batch_11_integration.rs new file mode 100644 index 0000000..0ecb31d --- /dev/null +++ b/crates/gimbal_controller/tests/batch_11_integration.rs @@ -0,0 +1,205 @@ +//! AZ-654 / AZ-655 / AZ-656 integration tests. +//! +//! Each test exercises one batch-11 primitive against the production +//! `GimbalControllerHandle` surface (set_pose / zoom / state_stream), +//! catching wiring bugs that the per-primitive unit tests can't see +//! (e.g. `ts_monotonic_ns` plumbing, transport interaction). + +use std::net::{Ipv4Addr, SocketAddr}; +use std::sync::Arc; +use std::time::{Duration, Instant}; + +use tokio::net::UdpSocket; + +use gimbal_controller::{ + encode_frame, A40Transport, CentreOnTarget, CentreOnTargetConfig, FrameId, GimbalCommand, + GimbalController, NextStep, PlanExecutor, SweepConfig, SweepEngine, SweepPattern, +}; +use shared::models::frame::BoundingBox; +use shared::models::gimbal::{GimbalState, PanGoal, PanPlan}; + +fn loopback(port: u16) -> SocketAddr { + SocketAddr::new(Ipv4Addr::new(127, 0, 0, 1).into(), port) +} + +fn initial_state() -> GimbalState { + GimbalState { + yaw: 0.0, + pitch: 0.0, + zoom: 1.0, + ts_monotonic_ns: 0, + command_in_flight: false, + } +} + +/// AZ-656 AC-2 — every `set_pose` publishes a `GimbalState` with a +/// strictly-monotonic `ts_monotonic_ns`. Catches the wrong-clock bug +/// where `SystemTime::now()` was previously used (would have been +/// observable as a stale or NTP-adjusted timestamp). +#[tokio::test] +async fn az656_set_pose_publishes_monotonic_timestamp() { + // Arrange — full controller wired to a fake A40 echo loop + let fake_socket = Arc::new(UdpSocket::bind(loopback(0)).await.expect("fake bind")); + let fake_addr = fake_socket.local_addr().expect("fake addr"); + let test_socket = Arc::new(UdpSocket::bind(loopback(0)).await.expect("test bind")); + test_socket.connect(fake_addr).await.expect("connect"); + + let fake_socket_clone = fake_socket.clone(); + tokio::spawn(async move { + loop { + let mut buf = [0u8; 128]; + let Ok((_, from)) = fake_socket_clone.recv_from(&mut buf).await else { + return; + }; + let reply = encode_frame(FrameId::T1F1B1D1, &[0; 12], 0).expect("encode"); + let _ = fake_socket_clone.send_to(&reply, from).await; + } + }); + + let (transport, _recv_task) = + A40Transport::from_socket(test_socket, fake_addr).expect("from_socket"); + let controller = GimbalController::with_transport(initial_state(), transport); + let handle = controller.handle(); + let state_rx = handle.state_stream(); + + // Act — three sequential set_pose calls; capture the stamps each + // call publishes onto the watch channel. + let mut timestamps = Vec::with_capacity(3); + for i in 0..3 { + handle + .set_pose(GimbalCommand { + yaw_deg: i as f32 * 5.0, + pitch_deg: 0.0, + }) + .await + .expect("set_pose"); + timestamps.push(state_rx.borrow().ts_monotonic_ns); + tokio::time::sleep(Duration::from_millis(2)).await; + } + + // Assert + assert!(timestamps[0] > 0, "initial stamp should be > 0 after first set_pose"); + assert!(timestamps[1] > timestamps[0], "ts not monotonic: {} → {}", timestamps[0], timestamps[1]); + assert!(timestamps[2] > timestamps[1], "ts not monotonic: {} → {}", timestamps[1], timestamps[2]); +} + +/// AZ-655 integration — load a plan and exercise the executor against +/// a real wall-clock-driven tick loop; verify the throttle counter +/// matches the emission ratio. +#[test] +fn az655_plan_executor_emits_and_throttles_against_real_clock() { + // Arrange + let mut exe = PlanExecutor::new(Duration::from_millis(20)); + let t0 = Instant::now(); + exe.load( + PanPlan { + goals: vec![ + PanGoal { + yaw_deg: -10.0, + pitch_deg: 0.0, + zoom: 1.0, + at_ns: 0, + }, + PanGoal { + yaw_deg: 10.0, + pitch_deg: 0.0, + zoom: 1.0, + at_ns: 200_000_000, + }, + ], + }, + t0, + ) + .expect("load plan"); + + // Act — 100 ticks at 5 ms cadence over 500 ms + let mut emits = 0_u64; + let mut throttled = 0_u64; + for i in 0..100 { + match exe.next_step(t0 + Duration::from_millis(i * 5)).unwrap() { + NextStep::Emit(_) => emits += 1, + NextStep::Throttled => throttled += 1, + } + } + + // Assert — 20 ms throttle over 500 ms ≈ 25 emissions + assert!((23..=27).contains(&emits), "emits = {emits}, want ~25"); + assert_eq!(emits + throttled, 100); + assert_eq!(exe.stats().commands_emitted_total, emits); + assert_eq!(exe.stats().commands_dropped_to_throttle_total, throttled); +} + +/// AZ-654 integration — pendulum sweep produces commands the +/// controller can accept (matches the `GimbalCommand` contract used by +/// `set_pose`). No transport wiring needed; this is a contract test. +#[test] +fn az654_sweep_engine_emits_gimbal_commands_within_bounds() { + // Arrange + let cfg = SweepConfig { + min_yaw_deg: -45.0, + max_yaw_deg: 45.0, + pitch_deg: -15.0, + step_deg: 3.0, + dwell: Duration::from_millis(200), + }; + let mut engine = SweepEngine::new(SweepPattern::Pendulum, cfg).expect("new sweep"); + + // Act + Assert — every emitted command stays inside the envelope + let mut t = Instant::now(); + for _ in 0..200 { + let cmd = engine.next_step(t).expect("pendulum tick"); + assert!(cmd.yaw_deg >= -45.0 && cmd.yaw_deg <= 45.0); + assert!((cmd.pitch_deg - (-15.0)).abs() < 0.001); + t += Duration::from_millis(50); + } +} + +/// AZ-656 integration — closed-loop convergence smoke against the +/// public `CentreOnTarget` surface (mirrors the unit-test kinematic +/// model but uses only the public API; catches re-export drift). +#[test] +fn az656_centre_on_target_loop_converges_via_public_api() { + // Arrange + let cfg = CentreOnTargetConfig::default(); + let mut ctrl = CentreOnTarget::new(cfg); + let mut bbox = BoundingBox { + x_min: 0.70, + y_min: 0.50, + x_max: 0.80, + y_max: 0.60, + }; + let mut yaw = 0.0_f32; + let mut pitch = 0.0_f32; + let zoom = 1.0_f32; + let fov = cfg.fov_deg_at_zoom1 / zoom; + + // Act + for _ in 0..3 { + let out = ctrl.tick(Some(bbox), yaw, pitch, zoom); + let cmd = out.command.expect("emit"); + let dy = cmd.yaw_deg - yaw; + let dp = cmd.pitch_deg - pitch; + yaw = cmd.yaw_deg; + pitch = cmd.pitch_deg; + let cx = (bbox.x_min + bbox.x_max) * 0.5 - dy / fov; + let cy = (bbox.y_min + bbox.y_max) * 0.5 + dp / fov; + bbox = BoundingBox { + x_min: cx - 0.05, + y_min: cy - 0.05, + x_max: cx + 0.05, + y_max: cy + 0.05, + }; + } + let final_cx = (bbox.x_min + bbox.x_max) * 0.5; + let final_cy = (bbox.y_min + bbox.y_max) * 0.5; + + // Assert + assert!( + (0.375..=0.625).contains(&final_cx), + "x = {final_cx} outside centre 25%" + ); + assert!( + (0.375..=0.625).contains(&final_cy), + "y = {final_cy} outside centre 25%" + ); +} diff --git a/crates/shared/src/models/gimbal.rs b/crates/shared/src/models/gimbal.rs index e803a40..f12c960 100644 --- a/crates/shared/src/models/gimbal.rs +++ b/crates/shared/src/models/gimbal.rs @@ -1,4 +1,4 @@ -//! `GimbalState` — per `data_model.md §4 Action / piloting entities`. +//! `GimbalState` + `PanPlan` — per `data_model.md §4 Action / piloting entities`. use serde::{Deserialize, Serialize}; @@ -10,3 +10,24 @@ pub struct GimbalState { pub ts_monotonic_ns: u64, pub command_in_flight: bool, } + +/// One waypoint in a `PanPlan`. `at_ns` is monotonic-nanoseconds since +/// the plan was loaded (i.e. relative to the plan's clock origin, not +/// since boot); the executor interpolates between adjacent goals based +/// on this relative timeline. +#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)] +pub struct PanGoal { + pub yaw_deg: f32, + pub pitch_deg: f32, + pub zoom: f32, + pub at_ns: u64, +} + +/// Path-tracking plan emitted by `semantic_analyzer` (via +/// `scan_controller`) and consumed by `gimbal_controller`. Goals are +/// ordered strictly increasing in `at_ns`. Empty plans are rejected by +/// the executor's validate step. +#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)] +pub struct PanPlan { + pub goals: Vec, +}