azaion/gps-denied-onboard
1
0
Fork 0
mirror of https://github.com/azaion/gps-denied-onboard.git synced 2026-06-22 15:11:12 +00:00
Code Issues Packages Projects Releases Wiki Activity

[AZ-388] C5 AC-5.2 no-estimate fallback detector + signal emission

Browse Source 
Implements Invariant 9 / AC-5.2: when current_estimate cannot return a
fresh output for >= state.no_estimate_fallback_s (default 3.0 s), emit
ONE engagement signal (FDR kind=c5.state.no_estimate_fallback_engaged
+ GCS STATUSTEXT severity CRITICAL); on recovery, ONE recovery signal
(FDR kind=c5.state.no_estimate_fallback_recovered + STATUSTEXT NOTICE).
Rate-limited via single _in_fallback latch (AC-2: 30 s sustained
no-estimate still emits exactly one engagement).

New FallbackWatcher class owns the state machine; estimator wires it
through constructor + current_estimate entry/success hooks. Public
check_fallback_state(now_ns) watchdog (NFR p99 <= 5 us) + subscribe
APIs let C8 outbound react without coupling C5 to a concrete GCS
adapter at construction. Severity enum extended with CRITICAL=2 and
NOTICE=5 to match MAVLink MAV_SEVERITY.

18 new unit tests across all 8 ACs, deterministic synthetic clock,
integration tests patch monotonic_ns through GtsamIsam2StateEstimator
to drive AC-7 iSAM2 leg (ESKF leg deferred to AZ-386).

Full suite: 607 passed, 2 skipped.

Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Oleksandr Bezdieniezhnykh
2026-05-11 06:53:22 +03:00
parent b3ad94c155
commit 31a300f8a2
7 changed files with 826 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
_docs/02_tasks/done/AZ-388_c5_ac52_fallback.md
+85
View File
@@ -0,0 +1,85 @@
# C5 AC-5.2 fallback — 3 s no-estimate detector + signal emission
**Task**: AZ-388_c5_ac52_fallback
**Name**: C5 AC-5.2 fallback path — 3 s no-estimate detector + downstream signal
**Description**: Implement the AC-5.2 contract: when `current_estimate()` cannot return a fresh `EstimatorOutput` for ≥3 s (config `state.no_estimate_fallback_s`, default 3.0) — either because every call raised `EstimatorFatalError` OR the keyframe window has been empty — emit ONE downstream signal `kind="c5.state.no_estimate_fallback_engaged"` to FDR + GCS STATUSTEXT (severity CRITICAL) instructing C8 outbound to switch to FC IMU-only emission. The signal is emitted ONCE per fallback engagement (rate-limited by an `_in_fallback` boolean); on recovery (a fresh successful `current_estimate()`), emit ONE recovery signal `kind="c5.state.no_estimate_fallback_recovered"`. Add a private rolling counter `_last_successful_estimate_ns`; check at the top of every `current_estimate()` call AND on a separate watchdog tick (driven by C8 outbound's 5 Hz call cadence; the watchdog is implemented as a method `check_fallback_state(now_ns) -> bool` returning the current fallback state).
**Complexity**: 3 points
**Dependencies**: AZ-384 (`current_estimate` body), AZ-386 (same hook for ESKF), AZ-273 (FDR), AZ-272, AZ-390 (E-C8 — `GcsAdapter` Protocol surface), AZ-397 (E-C8 — `QgcTelemetryAdapter` concrete STATUSTEXT broadcast), AZ-263, AZ-269, AZ-266
**Component**: c5_state (epic AZ-260 / E-C5)
**Tracker**: AZ-388
**Epic**: AZ-260 (E-C5)
### Document Dependencies
- `_docs/02_document/contracts/c5_state/state_estimator_protocol.md` — Invariant 9.
- `_docs/02_document/components/07_c5_state/description.md` — § 5 (AC-5.2 fallback).
## Problem
Without this task, a sustained iSAM2 numerical failure or empty keyframe window would leave the system silently emitting stale or no estimates; C8 outbound would have no signal to switch to FC IMU-only; FC would receive degraded-quality estimates indefinitely.
## Outcome
- `_last_successful_estimate_ns` private counter (set on every successful `current_estimate()`).
- `_in_fallback` boolean (latched on engagement, cleared on recovery).
- Hook in `current_estimate()` (both estimators):
- On entry: if `now_ns - _last_successful_estimate_ns > no_estimate_fallback_s * 1e9` AND not `_in_fallback` → engage fallback (emit signal, set flag, log).
- On successful return: if `_in_fallback` → emit recovery signal, clear flag, log.
- New public method `check_fallback_state(now_ns) -> bool`: idempotent watchdog check returning the current fallback state. C8 outbound calls this at its 5 Hz cadence to drive the FC IMU-only switch even when no `current_estimate()` is being called.
- Engagement signal: FDR `kind="c5.state.no_estimate_fallback_engaged"` + GCS STATUSTEXT (CRITICAL severity, message "Onboard estimator lost; FC IMU-only").
- Recovery signal: FDR `kind="c5.state.no_estimate_fallback_recovered"` + GCS STATUSTEXT (NOTICE severity).
- Configurable threshold; AC-5.2 default 3.0 s.
- Unit tests: 3 s no estimate → engagement signal fires once; sustained no-estimate over 30 s → still ONE engagement signal (rate-limited); successful estimate after engagement → recovery signal fires once; `check_fallback_state` returns correct state from external watchdog.
## Scope
### Included
- `_last_successful_estimate_ns` counter + `_in_fallback` flag.
- Hooks in both estimators' `current_estimate`.
- Public `check_fallback_state(now_ns)` watchdog API.
- Engagement + recovery signal emission.
- Rate-limiting (one signal per state transition).
- Unit tests across both estimators.
### Excluded
- The actual FC IMU-only emission — owned by AZ-261 (C8 outbound).
- C5-IT-05 component-internal acceptance test — deferred to E-BBT.
## Acceptance Criteria
**AC-1: Engagement after 3 s of no estimate** — synthetic timeline with no successful `current_estimate` for 3.5 s → ONE engagement signal fires.
**AC-2: Engagement is one-shot** — sustained 30 s no-estimate → still ONE engagement signal (rate-limited).
**AC-3: Recovery signal** — after engagement, a successful `current_estimate` → ONE recovery signal.
**AC-4: `check_fallback_state` watchdog** — even without `current_estimate` being called, the watchdog method correctly reports `True` after 3 s.
**AC-5: GCS STATUSTEXT severity correct** — engagement = CRITICAL; recovery = NOTICE.
**AC-6: Configurable threshold** — `no_estimate_fallback_s = 5.0` → engagement at 5 s instead of 3 s.
**AC-7: Both estimators participate** — iSAM2 + ESKF both fire engagement / recovery signals correctly.
**AC-8: FDR record shapes** — engagement: `{reason: "no_successful_estimate_for_s"}`; recovery: `{recovered_after_s}`.
## Non-Functional Requirements
- `check_fallback_state` p99 ≤ 5 µs.
- Hook overhead in `current_estimate` < 10 µs.
## Constraints
- One signal per state transition (rate-limited).
- Both estimators MUST participate.
- Config threshold MUST be respected.
## Risks & Mitigation
- **Risk: `_last_successful_estimate_ns` race with watchdog** — single-writer thread; both update from the same thread (composition root binds C5 + C8 outbound 5 Hz tick handler).
## Runtime Completeness
- **Named capability**: AC-5.2 fallback detector.
- **Production code**: real counter, real signal emission, real watchdog method.
- **Unacceptable substitutes**: spamming engagement signals on every check (rate-limit violation); silently dropping recovery (would leave C8 in IMU-only forever).