Class OdometryGuard

java.lang.Object
frc.spectrumLib.localization.OdometryGuard
public final class OdometryGuard extends Object
Screens each odometry sample before PoseFusion sees it, so a device that drops off CAN cannot move the pose. Pure logic on logged inputs, so it replays and is unit tested.

When the CANivore's link to SystemCore drops, every device on it reads 0 for up to 1.3 s (TXMAN1 Q34, Q19, r16). Integrated as-is, that moved the fused pose 45 m.

Modules: one counts for a sample only when it is connected, did not read a dead 0, and its travel since its last accepted sample fits maxSpeed over the elapsed time. Its baseline advances only when it is used, so the first good sample after a gap carries the whole gap. The motion is solved by least squares from the modules used (two or more) and handed on as one consistent set of module positions; with fewer, the translation holds and vision holds the pose. A module re-baselines, without integrating, after a reported device reset, or after its readings have been steady but implausible for REBASELINE_SECONDS (a reset nobody reported).

Heading, in order: the primary gyro (absolute, so after a gap it snaps back to where the gyro, which kept running, says the robot points), the backup gyro's change, the wheels' rotation, else none, and with no heading and no translation the sample is dropped.

  • Constructor Details

  • Method Details

    • markResets

      public void markResets(boolean primaryReset, boolean[] moduleResets)
      Notes device resets reported since the last loop; each takes effect on that device's next reading, which becomes its new baseline.

    • getHeading

      public double getHeading()
      The heading handed to the estimators, radians.

    • getBackupShadowHeading

      public double getBackupShadowHeading()
      The backup gyro on the primary's heading, aligned once at the first reading of both.

    • update

      public Optional<OdometryGuard.Output> update(OdometryGuard.Sample s)
      Screens one sample. Empty when it can move nothing.