Bug #66 » 0004-fix-ui-reconcileSelfYaw-holds-the-yaw-correction-thr.patch
| androidApp/src/main/java/com/aether/mofe/platform/MofeEngineHost.kt | ||
|---|---|---|
|
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
|
||
| ... | ... | |
|
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())
|
||
| ... | ... | |
|
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
|
||