[AZ-654] [AZ-655] [AZ-656] gimbal_controller primitives + monotonic clock fix (batch 11)
ci/woodpecker/push/build-arm Pipeline failed

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 <cursoragent@cursor.com>
This commit is contained in:
Oleksandr Bezdieniezhnykh
2026-05-19 20:21:00 +03:00
parent 288e7f8c46
commit 4c63829ccd
12 changed files with 1470 additions and 13 deletions
@@ -1,64 +0,0 @@
# Zoom-Out Sweep Pattern
**Task**: AZ-654_gimbal_zoom_out_sweep
**Name**: Zoom-out sweep pattern primitive
**Description**: Run the zoom-out sweep pattern when `scan_controller` is in `ZoomedOut`. The exact pattern (pendulum / raster / lawn-mower) is gated by `architecture.md §8 Q1`; this task implements one selectable default with the pattern enum and exposes the choice through config.
**Complexity**: 3 points
**Dependencies**: AZ-640_initial_structure, AZ-653_gimbal_a40_transport
**Component**: gimbal_controller
**Tracker**: AZ-654
**Epic**: AZ-634
## Problem
In `ZoomedOut`, the gimbal must sweep its FOV continuously to maximise coverage. The exact pattern is an open architecture question (Q1); this task implements the `SweepEngine` abstraction and ships `Pendulum` as the safe default, with `Raster` and `LawnMower` enum variants reserved. Switching pattern is config-only — no API change to consumers.
## Outcome
- `SweepEngine::next_step(state) -> GimbalCommand` produces a sequence of yaw / pitch / zoom commands implementing the configured sweep pattern with bounded jitter and no overshoot beyond configured FOV bounds.
- Default pattern is `Pendulum`; `Raster` and `LawnMower` are wired as enum variants (one implemented; the others reserved).
- Sweep config (FOV per zoom tier, dwell time per direction, step size) is loaded from startup config.
## Scope
### Included
- `SweepPattern` enum with all three variants declared; default impl for `Pendulum`.
- `SweepEngine` struct holding the current direction + dwell counter.
- Bounded-jitter command emission.
- FOV-bound enforcement.
### Excluded
- The pattern selection rationale (Q1 — resolved separately).
- Smooth-pan plan execution (task 16).
- Centre-on-target (task 17).
## Acceptance Criteria
**AC-1: Pendulum sweep emits a bounded-jitter command stream**
Given `SweepEngine::new(SweepPattern::Pendulum, config)`
When `next_step()` is called 100 times
Then the yaw values stay within `[config.min_yaw, config.max_yaw]`, never overshoot, and reverse direction at each bound.
**AC-2: Dwell at bounds is respected**
Given a config with `dwell_ms = 500`
When the sweep reaches a yaw bound
Then `next_step()` returns the same yaw for at least 500 ms before reversing direction.
**AC-3: Pattern enum exhaustiveness**
Given the `SweepPattern` enum
When match-exhausting it in client code
Then the compiler covers `Pendulum`, `Raster`, `LawnMower` — unimplemented variants return `Err(NotImplemented)` at runtime, never silently fall back.
## Non-Functional Requirements
**Performance**
- `next_step()` p99 ≤1 ms.
**Reliability**
- Bounded jitter; no overshoot.
## Runtime Completeness
- **Named capability**: zoom-out sweep pattern (default `Pendulum`).
- **Production code that must exist**: real bounded sweep state machine.
- **Unacceptable substitutes**: random walk is not acceptable — sweep coverage must be deterministic and bounded.
@@ -1,64 +0,0 @@
# Smooth-Pan Path-Tracking Plan Executor
**Task**: AZ-655_gimbal_smooth_pan_plan
**Name**: Smooth-pan plan executor (zoom-in path-follow)
**Description**: Accept a pan plan (sequence of yaw / pitch / zoom goals with timing) from `semantic_analyzer` via `scan_controller` and execute it smoothly. Used for follow-the-footpath behaviour during the zoom-in level.
**Complexity**: 3 points
**Dependencies**: AZ-640_initial_structure, AZ-653_gimbal_a40_transport
**Component**: gimbal_controller
**Tracker**: AZ-655
**Epic**: AZ-634
## Problem
When `scan_controller` is in `ZoomedIn` and `semantic_analyzer` recommends `PanFollowFootpath`, a sequence of yaw/pitch/zoom goals with timing arrives. The executor must interpolate between goals smoothly (no step jumps) and respect the vendor's command rate — if the plan is too dense, drop the lowest-priority goals rather than blocking the queue.
## Outcome
- `PlanExecutor::load(plan: PanPlan)` accepts an ordered sequence `Vec<(yaw, pitch, zoom, at_ts)>`.
- `next_step(now)` returns the interpolated `GimbalCommand` to issue at `now`; goals past their `at_ts` are skipped; goals before `at_ts` are extrapolated linearly.
- The executor self-throttles: emits at most one command per `min_cmd_interval_ms` (default 50 ms), dropping intermediate interpolations.
- Health: `plan_loaded_at`, `commands_emitted_total`, `commands_dropped_to_throttle_total`.
## Scope
### Included
- `PanPlan` data type (`data_model.md §PanPlan`).
- Linear interpolation between adjacent goals.
- Self-throttling.
### Excluded
- Generating the plan (`semantic_analyzer`).
- Sweep pattern (task 15).
- Centre-on-target (task 17).
## Acceptance Criteria
**AC-1: Linear interpolation between goals**
Given a plan with two goals 1 s apart and yaw `0° → 30°`
When `next_step(now=500ms)` is called
Then the returned `yaw` is `15°` ± a defined epsilon.
**AC-2: Self-throttle drops intermediate calls**
Given `min_cmd_interval_ms = 100`
When `next_step()` is called every 10 ms for 1 s
Then exactly ~10 commands are emitted (the rest counted as throttled).
**AC-3: Plan past its end clamps to last goal**
Given a plan whose last `at_ts` is in the past
When `next_step(now)` is called
Then the returned command equals the last goal's `(yaw, pitch, zoom)`; no error.
## Non-Functional Requirements
**Performance**
- `next_step()` p99 ≤1 ms.
**Reliability**
- Throttle drops are counted, never silent.
## Runtime Completeness
- **Named capability**: smooth-pan plan execution + interpolation.
- **Production code that must exist**: real interpolation; real self-throttle.
- **Unacceptable substitutes**: dispatching every plan goal directly without interpolation/throttling is not acceptable (causes jerky panning).
@@ -1,64 +0,0 @@
# Centre-On-Target Primitive + GimbalState Publish
**Task**: AZ-656_gimbal_centre_on_target
**Name**: Centre-on-target primitive + timestamped GimbalState publish
**Description**: During `TargetFollow`, accept a centre-on-target stream (target bbox normalized) from `scan_controller` and command the gimbal to keep the target inside the centre 25 % of frame while visible. Stamp every emitted command + reported state with a monotonic timestamp so `movement_detector` can synchronise.
**Complexity**: 3 points
**Dependencies**: AZ-640_initial_structure, AZ-653_gimbal_a40_transport
**Component**: gimbal_controller
**Tracker**: AZ-656
**Epic**: AZ-634
## Problem
During target-follow, the gimbal must continuously re-aim to keep the target inside the centre 25 % of frame. The control loop must converge without overshoot, and every emitted command + every reported `GimbalState` must carry a monotonic timestamp so `movement_detector` can synchronise gimbal motion with the per-frame ego-motion estimate.
## Outcome
- `CentreOnTarget::tick(bbox_normalized, current_state) -> GimbalCommand` produces the yaw/pitch command needed to nudge the target toward frame centre; convergence within ≤3 ticks under nominal latency.
- Reported `GimbalState { yaw, pitch, zoom, ts_monotonic, command_in_flight }` is published on the state channel for `frame_ingest` (telemetry tagging) and `movement_detector` (ego-motion sync) consumption.
- If the target bbox is missing for 3 consecutive ticks, emit a `target_lost` signal to `scan_controller`.
## Scope
### Included
- Centre-25% control loop (proportional, configurable gain).
- Monotonic timestamp stamping (single source of truth: `Instant::now()` at emit point).
- `GimbalState` publisher.
- `target_lost` signal on 3 consecutive missing bboxes.
### Excluded
- Target-follow state ownership (`scan_controller`).
- Sweep (task 15) and pan plan (task 16).
## Acceptance Criteria
**AC-1: Centre convergence**
Given a target initially at bbox `(0.7, 0.5, 0.1, 0.1)` (right side of frame) and a healthy A40
When `tick()` is invoked over 3 cycles at 100 ms each
Then by the third cycle the target bbox centre is within the centre 25 % region.
**AC-2: GimbalState carries monotonic timestamp**
Given a sequence of `tick()` calls
When the resulting `GimbalState` is observed
Then `ts_monotonic` is strictly monotonically increasing across observations.
**AC-3: Target loss signals after 3 missing ticks**
Given the target bbox stream goes empty
When 3 consecutive ticks have no bbox
Then a `target_lost` signal is published exactly once; subsequent ticks do not re-emit.
## Non-Functional Requirements
**Performance**
- `tick()` p99 ≤2 ms.
- Centre convergence within ≤3 ticks at 10 Hz.
**Reliability**
- `target_lost` debounced — never spurious.
## Runtime Completeness
- **Named capability**: target-follow centre-25% loop + timestamped GimbalState publish.
- **Production code that must exist**: real control loop; real monotonic timestamping.
- **Unacceptable substitutes**: open-loop "send target position once" is not acceptable — the loop must close.