Initial commit

Made-with: Cursor

_docs/02_plans/components/05_gimbal_driver/description.md

@@ -0,0 +1,95 @@
+# GimbalDriver
+
+## 1. High-Level Overview
+
+**Purpose**: Implements the ViewLink serial protocol for controlling the ViewPro A40 gimbal. Sends pan/tilt/zoom commands via UART, reads gimbal feedback (current angles), provides PID-based path following, and handles communication integrity.
+
+**Architectural Pattern**: Hardware adapter with command queue and PID controller.
+
+**Upstream dependencies**: Config helper (UART port, baud rate, PID gains, mock mode), Types helper
+
+**Downstream consumers**: ScanController
+
+## 2. Internal Interfaces
+
+### Interface: GimbalDriver
+
+| Method | Input | Output | Async | Error Types |
+|--------|-------|--------|-------|-------------|
+| `connect(port, baud)` | str, int | bool | No | UARTError |
+| `disconnect()` | — | — | No | — |
+| `is_alive()` | — | bool | No | — |
+| `set_angles(pan, tilt, zoom)` | float, float, float | bool | No | GimbalCommandError |
+| `get_state()` | — | GimbalState | No | GimbalReadError |
+| `set_sweep_target(pan)` | float | bool | No | GimbalCommandError |
+| `zoom_to_poi(pan, tilt, zoom)` | float, float, float | bool | No (blocks ~2s for zoom) | GimbalCommandError, TimeoutError |
+| `follow_path(direction, pid_error)` | (dx,dy), float | bool | No | GimbalCommandError |
+| `return_to_sweep()` | — | bool | No | GimbalCommandError |
+
+**GimbalState**:
+```
+pan: float — current pan angle (degrees)
+tilt: float — current tilt angle (degrees)
+zoom: float — current zoom level (1-40)
+last_heartbeat: float — epoch timestamp of last valid response
+```
+
+## 5. Implementation Details
+
+**ViewLink Protocol**:
+- Baud rate: 115200, 8N1
+- Command format: per ViewLink Serial Protocol V3.3.3 spec
+- Implementation note: read full spec during implementation to determine if native checksums exist. If yes, use them. If not, add CRC-16 wrapper.
+- Retry: up to 3 times on checksum failure with 10ms delay
+
+**PID Controller** (for path following):
+- Dual-axis PID (pan, tilt independently)
+- Input: error = (path_center - frame_center) in pixels
+- Output: pan/tilt angular velocity commands
+- Gains: configurable via YAML, tuned per-camera
+- Anti-windup: integral clamping
+- Update rate: 10 Hz (100ms interval)
+
+**Mock Mode** (development):
+- TCP socket client instead of UART
+- Connects to mock-gimbal service (Docker)
+- Same interface, simulated delays for zoom transition (1-2s)
+
+**Key Dependencies**:
+
+| Library | Version | Purpose |
+|---------|---------|---------|
+| pyserial | — | UART communication |
+| crcmod | — | CRC-16 if needed (determined after reading ViewLink spec) |
+| struct (stdlib) | — | Binary packet packing/unpacking |
+
+**Error Handling Strategy**:
+- UART open failure → GimbalDriver.connect() returns false → gimbal_available=false
+- Command send failure → retry 3x → GimbalCommandError
+- No heartbeat for 4s → is_alive() returns false → ScanController sets gimbal_available=false
+
+## 7. Caveats & Edge Cases
+
+**Known limitations**:
+- Zoom transition takes 1-2s physical time (40x optical)
+- PID gains need tuning on real hardware (bench testing)
+- Gimbal has physical pan/tilt limits — commands beyond limits are clamped
+
+**Physical EMI mitigation** (not software — documented here for reference):
+- Shielded UART cable, shortest run
+- Antenna ≥35cm from gimbal
+- Ferrite beads on cable near Jetson
+
+## 8. Dependency Graph
+
+**Must be implemented after**: Config helper, Types helper
+**Can be implemented in parallel with**: Tier1Detector, Tier2SpatialAnalyzer, VLMClient, OutputManager
+**Blocks**: ScanController (needs GimbalDriver for scan control)
+
+## 9. Logging Strategy
+
+| Log Level | When | Example |
+|-----------|------|---------|
+| ERROR | UART open failed, 3x retry exhausted | `UART /dev/ttyTHS1 open failed: Permission denied` |
+| WARN | Checksum failure (retrying), slow response | `Gimbal CRC failure, retry 2/3` |
+| INFO | Connected, zoom complete, mode change | `Gimbal connected at /dev/ttyTHS1 115200. Zoom to 20x complete (1.4s)` |

_docs/02_plans/components/05_gimbal_driver/tests.md

@@ -0,0 +1,378 @@
+# Test Specification — GimbalDriver
+
+## Acceptance Criteria Traceability
+
+| AC ID | Acceptance Criterion | Test IDs | Coverage |
+|-------|---------------------|----------|----------|
+| AC-16 | Gimbal control sends pan/tilt/zoom commands to ViewPro A40 | IT-01, IT-02, AT-01 | Covered |
+| AC-17 | Gimbal command latency ≤500ms from decision to physical movement | PT-01 | Covered |
+| AC-18 | Zoom transitions: medium to high zoom within 2 seconds | IT-05, PT-02, AT-02 | Covered |
+| AC-19 | Path-following accuracy: footpath stays within center 50% of frame | IT-06, AT-03 | Covered |
+| AC-20 | Smooth gimbal transitions (no jerky movements) | IT-07, PT-03 | Covered |
+
+---
+
+## Integration Tests
+
+### IT-01: Connect to UART (Mock TCP Mode)
+
+**Summary**: Verify GimbalDriver connects to mock-gimbal service via TCP socket in dev mode.
+
+**Traces to**: AC-16
+
+**Input data**:
+- Config: gimbal.mode=mock_tcp, mock_host=localhost, mock_port=9090
+- Mock gimbal TCP server running
+
+**Expected result**:
+- connect() returns true
+- is_alive() returns true
+- get_state() returns GimbalState with valid initial values
+
+**Max execution time**: 2s
+
+**Dependencies**: Mock gimbal TCP server
+
+---
+
+### IT-02: set_angles Sends Correct ViewLink Command
+
+**Summary**: Verify set_angles translates pan/tilt/zoom to a valid ViewLink serial packet.
+
+**Traces to**: AC-16
+
+**Input data**:
+- Mock server that captures raw bytes received
+- set_angles(pan=15.0, tilt=-30.0, zoom=20.0)
+
+**Expected result**:
+- Byte packet matches ViewLink protocol format (header, payload, checksum)
+- Mock server acknowledges command
+- set_angles returns true
+
+**Max execution time**: 500ms
+
+**Dependencies**: Mock gimbal server with byte capture
+
+---
+
+### IT-03: Connection Failure Returns False
+
+**Summary**: Verify connect() returns false when UART/TCP port is unavailable.
+
+**Traces to**: AC-16
+
+**Input data**:
+- Config: mock_port=9091 (no server listening)
+
+**Expected result**:
+- connect() returns false
+- is_alive() returns false
+- No crash or hang
+
+**Max execution time**: 3s (with connection timeout)
+
+**Dependencies**: None
+
+---
+
+### IT-04: Heartbeat Timeout Marks Gimbal Dead
+
+**Summary**: Verify is_alive() returns false after 4 seconds without a heartbeat response.
+
+**Traces to**: AC-16
+
+**Input data**:
+- Connected mock server that stops responding after initial connection
+- Config: gimbal_timeout_s=4
+
+**Expected result**:
+- is_alive() returns true initially
+- After 4s without heartbeat → is_alive() returns false
+
+**Max execution time**: 6s
+
+**Dependencies**: Mock gimbal server with configurable response behavior
+
+---
+
+### IT-05: zoom_to_poi Blocks Until Zoom Complete
+
+**Summary**: Verify zoom_to_poi waits for zoom transition to complete before returning.
+
+**Traces to**: AC-18
+
+**Input data**:
+- Current zoom=1.0, target zoom=20.0
+- Mock server simulates 1.5s zoom transition
+
+**Expected result**:
+- zoom_to_poi blocks for ~1.5s
+- Returns true after zoom completes
+- get_state().zoom ≈ 20.0
+
+**Max execution time**: 3s
+
+**Dependencies**: Mock gimbal server with simulated zoom delay
+
+---
+
+### IT-06: follow_path PID Updates Direction
+
+**Summary**: Verify follow_path computes PID output and sends angular velocity commands.
+
+**Traces to**: AC-19
+
+**Input data**:
+- direction=(0.7, 0.3)
+- pid_error=50.0 (pixels offset from center)
+- PID gains: P=0.5, I=0.01, D=0.1
+
+**Expected result**:
+- Pan/tilt velocity commands sent to mock server
+- Command magnitude proportional to error
+- Returns true
+
+**Max execution time**: 100ms
+
+**Dependencies**: Mock gimbal server
+
+---
+
+### IT-07: PID Anti-Windup Clamps Integral
+
+**Summary**: Verify the PID integral term does not wind up during sustained error.
+
+**Traces to**: AC-20
+
+**Input data**:
+- 100 consecutive follow_path calls with constant pid_error=200.0
+- PID integral clamp configured
+
+**Expected result**:
+- Integral term stabilizes at clamp value (does not grow unbounded)
+- Command output reaches a plateau, no overshoot oscillation
+
+**Max execution time**: 500ms
+
+**Dependencies**: Mock gimbal server
+
+---
+
+### IT-08: Retry on Checksum Failure
+
+**Summary**: Verify the driver retries up to 3 times when a checksum mismatch is detected.
+
+**Traces to**: AC-16
+
+**Input data**:
+- Mock server returns corrupted response (bad checksum) on first 2 attempts, valid on 3rd
+
+**Expected result**:
+- set_angles retries 2 times, succeeds on 3rd
+- Returns true
+- If all 3 fail, raises GimbalCommandError
+
+**Max execution time**: 500ms
+
+**Dependencies**: Mock gimbal server with configurable corruption
+
+---
+
+### IT-09: return_to_sweep Resets Zoom to Medium
+
+**Summary**: Verify return_to_sweep zooms out to medium zoom level and resumes sweep angle.
+
+**Traces to**: AC-16
+
+**Input data**:
+- Current state: zoom=20.0, pan=15.0
+- Expected return: zoom=1.0 (medium)
+
+**Expected result**:
+- Zoom command sent to return to medium zoom
+- Returns true after zoom transition completes
+
+**Max execution time**: 3s
+
+**Dependencies**: Mock gimbal server
+
+---
+
+## Performance Tests
+
+### PT-01: Command-to-Acknowledgement Latency
+
+**Summary**: Measure round-trip time from set_angles call to server acknowledgement.
+
+**Traces to**: AC-17
+
+**Load scenario**:
+- 100 sequential set_angles commands
+- Mock server with 10ms simulated processing
+- Duration: ~15s
+
+**Expected results**:
+
+| Metric | Target | Failure Threshold |
+|--------|--------|-------------------|
+| Latency (p50) | ≤50ms | >500ms |
+| Latency (p95) | ≤200ms | >500ms |
+| Latency (p99) | ≤400ms | >500ms |
+
+**Resource limits**:
+- CPU: ≤10%
+- Memory: ≤50MB
+
+---
+
+### PT-02: Zoom Transition Duration
+
+**Summary**: Measure time from zoom command to zoom-complete acknowledgement.
+
+**Traces to**: AC-18
+
+**Load scenario**:
+- 10 zoom transitions: alternating 1x→20x and 20x→1x
+- Mock server with realistic zoom delay (1-2s)
+- Duration: ~30s
+
+**Expected results**:
+
+| Metric | Target | Failure Threshold |
+|--------|--------|-------------------|
+| Transition time (p50) | ≤1.5s | >2.0s |
+| Transition time (p95) | ≤2.0s | >2.5s |
+
+**Resource limits**:
+- CPU: ≤5%
+
+---
+
+### PT-03: PID Follow Smoothness (Jerk Metric)
+
+**Summary**: Measure gimbal command smoothness during path following by computing jerk (rate of acceleration change).
+
+**Traces to**: AC-20
+
+**Load scenario**:
+- 200 PID updates at 10Hz (20s follow)
+- Path with gentle curve (sinusoidal trajectory)
+- Mock server records all received angular velocity commands
+
+**Expected results**:
+
+| Metric | Target | Failure Threshold |
+|--------|--------|-------------------|
+| Max jerk (deg/s³) | ≤50 | >200 |
+| Mean jerk (deg/s³) | ≤10 | >50 |
+
+**Resource limits**:
+- CPU: ≤15% (PID computation)
+
+---
+
+## Security Tests
+
+### ST-01: UART Buffer Overflow Protection
+
+**Summary**: Verify the driver handles oversized responses without buffer overflow.
+
+**Traces to**: AC-16
+
+**Attack vector**: Malformed or oversized serial response (EMI corruption, spoofing)
+
+**Test procedure**:
+1. Mock server sends response exceeding max expected packet size (e.g., 10KB)
+2. Mock server sends response with invalid header bytes
+
+**Expected behavior**: Driver discards oversized/malformed packets, logs warning, continues operation.
+
+**Pass criteria**: No crash, no memory corruption; is_alive() still returns true after discarding bad packet.
+
+**Fail criteria**: Buffer overflow, crash, or undefined behavior.
+
+---
+
+## Acceptance Tests
+
+### AT-01: Pan/Tilt/Zoom Control End-to-End
+
+**Summary**: Verify the full command cycle: set angles, read back state, verify match.
+
+**Traces to**: AC-16
+
+**Preconditions**:
+- GimbalDriver connected to mock server
+
+**Steps**:
+
+| Step | Action | Expected Result |
+|------|--------|-----------------|
+| 1 | set_angles(pan=10, tilt=-20, zoom=5) | Returns true |
+| 2 | get_state() | pan≈10, tilt≈-20, zoom≈5 (within tolerance) |
+| 3 | set_angles(pan=-30, tilt=0, zoom=1) | Returns true |
+| 4 | get_state() | pan≈-30, tilt≈0, zoom≈1 |
+
+---
+
+### AT-02: Zoom Transition Timing Compliance
+
+**Summary**: Verify zoom from medium to high completes within 2 seconds.
+
+**Traces to**: AC-18
+
+**Preconditions**:
+- GimbalDriver connected, current zoom=1.0
+
+**Steps**:
+
+| Step | Action | Expected Result |
+|------|--------|-----------------|
+| 1 | Record timestamp T0 | — |
+| 2 | zoom_to_poi(pan=0, tilt=-10, zoom=20) | Blocks until complete |
+| 3 | Record timestamp T1 | T1 - T0 ≤ 2.0s |
+| 4 | get_state().zoom | ≈ 20.0 |
+
+---
+
+### AT-03: Path Following Keeps Error Within Bounds
+
+**Summary**: Verify PID controller keeps path tracking error within center 50% of frame.
+
+**Traces to**: AC-19
+
+**Preconditions**:
+- Mock server simulates gimbal with realistic response dynamics
+- Trajectory: 20 waypoints along a curved path
+
+**Steps**:
+
+| Step | Action | Expected Result |
+|------|--------|-----------------|
+| 1 | Start follow_path with trajectory | PID commands issued |
+| 2 | Simulate 100 PID cycles at 10Hz | Commands recorded by mock |
+| 3 | Compute simulated frame-center error | Error < 50% of frame width for ≥90% of cycles |
+
+---
+
+## Test Data Management
+
+**Required test data**:
+
+| Data Set | Description | Source | Size |
+|----------|-------------|--------|------|
+| viewlink_packets | Reference ViewLink protocol packets for validation | Captured from spec / ArduPilot | ~10 KB |
+| pid_trajectories | Sinusoidal and curved path trajectories for PID testing | Generated | ~5 KB |
+| corrupted_responses | Oversized and malformed serial response bytes | Generated | ~1 KB |
+
+**Setup procedure**:
+1. Start mock-gimbal TCP server on configured port
+2. Initialize GimbalDriver with mock_tcp config
+3. Call connect()
+
+**Teardown procedure**:
+1. Call disconnect()
+2. Stop mock-gimbal server
+
+**Data isolation strategy**: Each test uses its own GimbalDriver instance connected to a fresh mock server. No shared state.