From e71eba5f2c695dae974102b42796cfce250ac79f Mon Sep 17 00:00:00 2001 From: Claude Date: Tue, 4 Aug 2026 02:29:44 +0000 Subject: [PATCH 4/5] fix(ui): reconcileSelfYaw holds the yaw correction through motion via YawReconcileGate (F2) Route the ~10 Hz reconcile tick through YawReconcileGate instead of the binary locked/not-locked gate that zeroed the correction on any non-LOCKED state. Now HOLD (return, keep _selfYawCorrectionRad) during RESOLVING_MOTION so the lens keeps its drift correction while the user aims/walks, RECONCILE when LOCKED, RESET on re-establish or no pose. Fixes the anchors sliding off the FPV lens under motion. androidApp (not compilable here); the gate policy + the hold-through-motion behavior are jvmTest-verified in the engine YawReconcileGate change. Co-Authored-By: Claude Opus 4.8 Claude-Session: https://claude.ai/code/session_01WppuiKZt4CuQxX4N7k6SVR --- .../aether/mofe/platform/MofeEngineHost.kt | 30 ++++++++++++------- 1 file changed, 19 insertions(+), 11 deletions(-) diff --git a/androidApp/src/main/java/com/aether/mofe/platform/MofeEngineHost.kt b/androidApp/src/main/java/com/aether/mofe/platform/MofeEngineHost.kt index b3ae0d9..2878ed2 100644 --- a/androidApp/src/main/java/com/aether/mofe/platform/MofeEngineHost.kt +++ b/androidApp/src/main/java/com/aether/mofe/platform/MofeEngineHost.kt @@ -9,6 +9,8 @@ import com.aether.mofe.engine.MultiObserverFusionEngine import com.aether.mofe.engine.MultilaterationSolver import com.aether.mofe.engine.SelfHeadingObserver import com.aether.mofe.engine.YawDriftReconciler +import com.aether.mofe.engine.YawReconcileAction +import com.aether.mofe.engine.YawReconcileGate import com.aether.mofe.model.AnchorHealth import com.aether.mofe.model.ConstellationFrameState import com.aether.mofe.model.DeviceId @@ -275,19 +277,25 @@ class MofeEngineHost( private val yawReconciler = YawDriftReconciler() @Volatile private var _selfYawCorrectionRad = 0.0 /** Slowly-reconciled absolute-yaw correction (rad): ROTATE the predicted self facing by −this - * about world up to counter gyro yaw drift. 0 until the frame is LOCKED with a ≥2-anchor AoA - * consensus, so it never engages before there is a trustworthy heading reference. */ + * about world up to counter gyro yaw drift. 0 until the frame first LOCKs with a ≥2-anchor AoA + * consensus, so it never engages before there is a trustworthy heading reference; once engaged it + * is HELD through motion ([ConstellationFrameState.RESOLVING_MOTION]) rather than dropped. */ fun selfYawCorrectionRad(): Double = _selfYawCorrectionRad - /** One ~10 Hz reconcile tick: fuse the fresh self-AoA sightings to the solved reference anchors - * into a robust heading-error consensus, and fold it slowly into [yawReconciler]. Gated on a - * LOCKED frame; resets (correction → 0) whenever the frame is not locked, so a re-solve or mesh - * switch never leaves a stale correction on the view. Runs on the engine dispatcher. */ + /** One ~10 Hz reconcile tick, gated by [YawReconcileGate] on the constellation frame state: + * RECONCILE (LOCKED) fuses fresh self-AoA sightings to the solved reference anchors into a robust + * heading-error consensus and folds it slowly into [yawReconciler]; HOLD (RESOLVING_MOTION) keeps + * the last correction while the observer moves — dropping it there is what slid the anchors off the + * lens; RESET (CONVERGING/RESOLVING, or no pose) forgets it so a re-solve/mesh switch never leaves a + * stale offset. View-layer only, engine dispatcher. */ private fun reconcileSelfYaw(eng: MultiObserverFusionEngine) { val self = _selfPose - val locked = runCatching { eng.constellationFrameState() }.getOrNull() == ConstellationFrameState.LOCKED - if (self == null || !locked) { - yawReconciler.reset(); _selfYawCorrectionRad = 0.0; return + val frame = runCatching { eng.constellationFrameState() }.getOrNull() + if (self == null || frame == null) { yawReconciler.reset(); _selfYawCorrectionRad = 0.0; return } + when (YawReconcileGate.decide(frame)) { + YawReconcileAction.RESET -> { yawReconciler.reset(); _selfYawCorrectionRad = 0.0; return } + YawReconcileAction.HOLD -> return // keep _selfYawCorrectionRad as-is (see YawReconcileGate) + YawReconcileAction.RECONCILE -> {} // fold the fresh AoA consensus below } val az = runCatching { eng.selfAoaBearings() }.getOrDefault(emptyMap()) val el = runCatching { eng.selfAoaElevations() }.getOrDefault(emptyMap()) @@ -300,8 +308,8 @@ class MofeEngineHost( if (dir.magnitude < 1e-6) null else SelfHeadingObserver.Sighting(dir, a, e, weight = (cos(a) * cos(e)).coerceAtLeast(0.05)) } - // No ≥2-anchor consensus this tick ⇒ HOLD the last correction (don't snap to 0 — the frame - // is still locked, anchors are just momentarily un-fused). Reset only happens on unlock above. + // Even while LOCKED, a tick with no ≥2-anchor consensus ⇒ HOLD the last correction (don't snap + // to 0 — anchors are just momentarily un-fused). RESET only comes from the gate (re-establish). val c = SelfHeadingObserver.fusedYawError(self.orientation, sightings) ?: return yawReconciler.observe(c.errorRad, dtSeconds = 0.096, spreadRad = c.spreadRad) _selfYawCorrectionRad = yawReconciler.correctionRad -- 2.43.0