Aether

package com.aether.mofe.engine

import com.aether.mofe.model.ConstellationFrameState

/** What the view-layer self-yaw reconcile should do this tick (see [YawReconcileGate]). */

enum class YawReconcileAction { RESET, HOLD, RECONCILE }

/**

* Decides what the slow view-layer self-yaw reconcile ([YawDriftReconciler]) does for a given

* constellation frame state — pure so the policy is unit-testable away from the engine host.

*

* WHY (F2 · lens accuracy under motion). The reconcile corrects the residual absolute-yaw drift the

* gyro-integrated FPV attitude accumulates. The first cut engaged ONLY on a [ConstellationFrameState.LOCKED]

* frame and snapped the correction to 0 on every other state. But a hand-held, magnetometer-less phone

* spends much of the time AIMING/WALKING in [ConstellationFrameState.RESOLVING_MOTION] (the motion

* override of a previously-locked frame) — exactly when yaw drift is worst — so the correction kept

* vanishing to 0 the moment the user moved, and the anchors slid off the lens. That is the on-device

* "the lens doesn't track the anchors" symptom.

*

* Policy:

* - [ConstellationFrameState.LOCKED] → RECONCILE: trustworthy heading, fold fresh AoA consensus in.

* - [ConstellationFrameState.RESOLVING_MOTION] → HOLD: the frame was locked and the observer is just

* moving; the last correction is still approximately valid, so KEEP it (a held ~correct offset beats

* snapping to 0 and letting the view slide) rather than reconciling against motion-corrupted bearings.

* - [ConstellationFrameState.CONVERGING] / [ConstellationFrameState.RESOLVING] → RESET: the frame is

* (re)establishing and there is no trustworthy heading, so forget the correction — a re-solve or mesh

* switch must never leave a stale offset on the view.

*

* HOLD keeps [YawDriftReconciler.initialized] set, so when the frame re-locks the reconcile resumes

* low-passing from the held value instead of re-snapping — no visible jump across a motion episode.

* View-layer only: this never touches the EKF, so it cannot reintroduce the aim jitter that retired the

* old absolute-yaw-into-EKF path. Sustained motion escalates RESOLVING_MOTION → RESOLVING, which resets.

*/

object YawReconcileGate {

fun decide(frame: ConstellationFrameState): YawReconcileAction = when (frame) {

ConstellationFrameState.LOCKED -> YawReconcileAction.RECONCILE

ConstellationFrameState.RESOLVING_MOTION -> YawReconcileAction.HOLD

ConstellationFrameState.CONVERGING -> YawReconcileAction.RESET

ConstellationFrameState.RESOLVING -> YawReconcileAction.RESET

}

}

package com.aether.mofe.engine

import com.aether.mofe.model.ConstellationFrameState

import kotlin.math.PI

import kotlin.math.abs

import kotlin.test.Test

import kotlin.test.assertEquals

import kotlin.test.assertTrue

/**

* F2 · lens accuracy under motion. [YawReconcileGate] decides whether the view-layer self-yaw

* reconcile reconciles, HOLDs, or resets for each constellation frame state. The mapping tests pin the

* policy; the modeled-loop test proves the behavioural point — HOLD keeps a good correction across a

* motion episode where the old reset-to-0 would have dropped it (the on-device "lens slides while I

* move" symptom).

*/

class YawReconcileGateTest {

@Test

fun locked_reconciles() {

assertEquals(YawReconcileAction.RECONCILE, YawReconcileGate.decide(ConstellationFrameState.LOCKED))

}

@Test

fun motion_holds_not_resets() {

// The crux: moving (the motion override of a locked frame) must NOT zero the correction.

assertEquals(YawReconcileAction.HOLD, YawReconcileGate.decide(ConstellationFrameState.RESOLVING_MOTION))

}

@Test

fun establishing_states_reset() {

assertEquals(YawReconcileAction.RESET, YawReconcileGate.decide(ConstellationFrameState.CONVERGING))

assertEquals(YawReconcileAction.RESET, YawReconcileGate.decide(ConstellationFrameState.RESOLVING))

}

@Test

fun every_frame_state_is_mapped() {

// Exhaustive: no state falls through (guards against a new enum value being silently dropped).

for (state in ConstellationFrameState.entries) {

YawReconcileGate.decide(state) // throws on an unmapped state if `when` ever loses exhaustiveness

}

assertEquals(4, ConstellationFrameState.entries.size, "new frame state? decide() must handle it")

}

// ── modeled host loop: hold-through-motion preserves the correction ───────────────────────────

/** Model of MofeEngineHost.reconcileSelfYaw's per-tick decision, driven by the gate. */

private fun tick(

rec: YawDriftReconciler,

frame: ConstellationFrameState,

errorRad: Double,

spreadRad: Double = 0.0,

dt: Double = 0.096,

): Double = when (YawReconcileGate.decide(frame)) {

YawReconcileAction.RESET -> { rec.reset(); 0.0 }

YawReconcileAction.HOLD -> rec.correctionRad // keep the last correction untouched

YawReconcileAction.RECONCILE -> rec.observe(errorRad, dt, spreadRad)

}

@Test

fun hold_through_motion_keeps_the_correction_reset_only_on_reestablish() {

val rec = YawDriftReconciler()

val err = 15.0 * PI / 180.0

// LOCKED: first consensus snaps the correction onto the ~15° error.

val afterLock = tick(rec, ConstellationFrameState.LOCKED, err)

assertTrue(abs(afterLock - err) < 1e-9, "locks onto the error (got ${afterLock * 180 / PI}°)")

// RESOLVING_MOTION for several ticks (the user is aiming/walking): correction HELD, not zeroed.

var c = afterLock

repeat(5) { c = tick(rec, ConstellationFrameState.RESOLVING_MOTION, errorRad = 0.0) }

assertTrue(abs(c - err) < 1e-9, "held through motion (got ${c * 180 / PI}°), NOT snapped to 0")

// Back to LOCKED: reconcile RESUMES from the held value (no re-snap jump) — still ~15°.

val afterRelock = tick(rec, ConstellationFrameState.LOCKED, err)

assertTrue(abs(afterRelock - err) < 1e-6, "resumes from held value, no jump")

// A genuine re-establish forgets it, so a re-solve/mesh switch leaves no stale offset.

val afterResolve = tick(rec, ConstellationFrameState.RESOLVING, errorRad = err)

assertEquals(0.0, afterResolve, "reset on re-establish")

assertTrue(!rec.initialized, "reset clears initialization so the next lock re-snaps cleanly")

}

}