Class ImuMountCheck

java.lang.Object
frc.spectrumLib.localization.ImuMountCheck
All Implemented Interfaces:
Check
public final class ImuMountCheck extends Object implements Check
Checks, from logged inputs, that the gyros are mounted the way the code is told, against the drivetrain's own wheel motion. Each check waits for enough evidence, then holds a verdict. As a pre-match Check, a reversed yaw sense fails, any other wrong mount warns, and a robot that has not driven enough since boot to judge anything is skipped.
  • Pigeon level: at rest, mount-corrected pitch and roll near 0.
  • Pigeon forward: while the wheels accelerate hard, the Pigeon's X/Y acceleration points the same way. A Pigeon turned on its mount reads the turn here.
  • Yaw sense, both gyros: the heading turns the way the wheels turn the robot.
  • Onboard IMU orientation: at rest, gravity on the axis its mount setting implies. WPILib applies the mount orientation to yaw only, so the raw accelerometer shows how SystemCore really sits.

Evidence counts only when the gyro and the wheels move together. Wheels spinning on a cart, pushing a wall, or a robot carried by hand give none, so a gyro that simply does not follow the wheels is never called wrong. Rests count only shortly after such driving, so the robot is on the floor. A check reads wrong only on clear contrary evidence: a reversed yaw, a consistent acceleration direction far from the wheels', a pitch, roll or gravity axis a mount error away.

Each rest is judged on its own, and a rest-based check reads wrong only when every rest does. A mount error tilts every rest alike, while a robot tipped on a bump or lifted with its wheels still tilts only that rest.

Call update(ImuMountCheck.Inputs) every loop, enabled or not: most evidence comes from driving.

  • Constructor Details

    • ImuMountCheck

      public ImuMountCheck(boolean imuFlat, double pigeonMountYawDeg)
      Parameters:
      imuFlat - the onboard IMU's mount setting is flat (gravity on its Z), else vertical
      pigeonMountYawDeg - the Pigeon's MountPoseYaw, for the reason a wrong forward gives

  • Method Details

    • update

      public void update(ImuMountCheck.Inputs in)

    • pigeonYawSense

      public ImuMountCheck.YawSense pigeonYawSense()

    • imuYawSense

      public ImuMountCheck.YawSense imuYawSense()

    • pigeonRestPitchDeg

      public double pigeonRestPitchDeg()
      Mean pitch over the most level rest; NaN before a rest.

    • pigeonRestRollDeg

      public double pigeonRestRollDeg()
      Mean roll over the most level rest; NaN before a rest.

    • pigeonLevel

      public ImuMountCheck.Verdict pigeonLevel()

    • pigeonForwardErrorDeg

      public double pigeonForwardErrorDeg()
      How far the Pigeon's X axis sits from the robot's forward, degrees CCW; NaN before data.

    • pigeonForwardConsistency

      public double pigeonForwardConsistency()
      1 when every sample put the Pigeon at the same angle from the wheels, near 0 if random.

    • pigeonForward

      public ImuMountCheck.Verdict pigeonForward()

    • imuGravityZShare

      public double imuGravityZShare()
      Gravity's share on the onboard IMU's Z axis, 0 to 1, over the rest closest to its mount setting; NaN before a rest that read gravity.

    • imuOrientation

      public ImuMountCheck.Verdict imuOrientation()

    • evaluate

      public CheckResult evaluate(double nowSeconds)
      A reversed yaw sense fails: the heading, and so field-relative driving, turns the wrong way. Any other wrong mount warns, naming the setting to look at with the measured numbers.
      Specified by:
      evaluate in interface Check
      Parameters:
      nowSeconds - this loop's robot time
      Returns:
      the verdict