Class CameraHeadingCheck

java.lang.Object
frc.spectrumLib.checks.CameraHeadingCheck
All Implemented Interfaces:
Check
public final class CameraHeadingCheck extends Object implements Check
Every camera that solves a full pose from tags agrees on the robot's heading, with the gyro and with each other.

For each multi-tag frame the caller passes the camera's heading and the gyro's yaw at the same moment. The difference is the camera's offset from the gyro, which stays constant while the robot turns, so frames from different moments and cameras can be compared. Each camera's offset is the median over the last WINDOW_SECONDS. A wrong mount yaw shifts one camera's offset by the error: TXMAN1 Q30's flipped yaw sign put LL-Left 172 deg from LL-Back, and the pose was seeded backwards from it.

The check fails when two cameras' offsets differ by more than FAIL_DEG, or once the pose heading has been seeded, when a camera's offset differs from the fused heading's by that much. It cannot tell which camera of a disagreeing pair is wrong; it names both. Only multi-tag frames count: a single tag's heading can flip, and MegaTag2-style solves take the heading from the robot, so they must not be passed in.

Only a camera the robot fuses can fail the check. One that is only logged (a SystemCore camera with a placeholder mount, an Orin camera switched off) warns at most: its heading cannot move the pose, so it is no reason to call the robot NO-GO.

  • Field Summary

    Fields
    Modifier and Type
    Field
    Description
    static final double
    FAIL_DEG
    Above this, a heading is wrong enough to send an auto the wrong way, and well above mount error: TXMAN1's LL-Left yaw, 5.5 deg off, read 7-9 deg from LL-Back in matches.
    static final double
    HOLD_SECONDS
    A worse verdict must last this long before it is reported.
    static final double
    MAX_YAW_RATE
    Faster than this (rad/s), latency between the frame and the gyro reading would add error.
    static final int
    MIN_FRAMES
    Frames a camera needs in the window before it is judged.
    static final double
    WARN_DEG
    Above this, a camera's mount yaw is probably off and worth solving again.
    static final double
    WINDOW_SECONDS
    Frames older than this are dropped, so a fixed or re-seeded heading clears soon.

  • Constructor Summary

    Constructors
    Constructor
    Description
    CameraHeadingCheck()
     

  • Method Summary

    Modifier and Type
    Method
    Description
    void
    addFrame(String camera, boolean cameraFused, double cameraHeadingDeg, double gyroYawDeg, int tagCount, double yawRateRadPerSec, double now)
    Records one camera frame, saying whether the robot fuses that camera.
    void
    addFrame(String camera, double cameraHeadingDeg, double gyroYawDeg, int tagCount, double yawRateRadPerSec, double now)
    Records one camera frame.
    CheckResult
    evaluate(double now)
    Judges the robot now.
    void
    setRobotHeading(double fusedHeadingDeg, double gyroYawDeg)
    The robot's fused heading now, with the gyro's yaw now, so each camera can be compared with the heading the robot will drive by.

    Methods inherited from class Object

    clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

    Methods inherited from interface Check

    observe

  • Field Details

    • FAIL_DEG

      public static final double FAIL_DEG
      Above this, a heading is wrong enough to send an auto the wrong way, and well above mount error: TXMAN1's LL-Left yaw, 5.5 deg off, read 7-9 deg from LL-Back in matches. Q30's sign error read 172.
      See Also:

    • WARN_DEG

      public static final double WARN_DEG
      Above this, a camera's mount yaw is probably off and worth solving again. Before TXMAN1 Q13 and practice r16, LL-Back and LL-Left read 4.4 and 5.0 deg apart.
      See Also:

    • WINDOW_SECONDS

      public static final double WINDOW_SECONDS
      Frames older than this are dropped, so a fixed or re-seeded heading clears soon.
      See Also:

    • MIN_FRAMES

      public static final int MIN_FRAMES
      Frames a camera needs in the window before it is judged.
      See Also:

    • MAX_YAW_RATE

      public static final double MAX_YAW_RATE
      Faster than this (rad/s), latency between the frame and the gyro reading would add error.
      See Also:

    • HOLD_SECONDS

      public static final double HOLD_SECONDS
      A worse verdict must last this long before it is reported. While the windows refill after the heading changes under the cameras (in the simulator, the robot placed without its gyro turning), cameras' medians flip at different moments and briefly disagree; a wrong mount disagrees for good.
      See Also:

  • Constructor Details

    • CameraHeadingCheck

      public CameraHeadingCheck()

  • Method Details

    • addFrame

      public void addFrame(String camera, double cameraHeadingDeg, double gyroYawDeg, int tagCount, double yawRateRadPerSec, double now)
      Records one camera frame. Ignored unless it saw two or more tags and the robot was turning slower than MAX_YAW_RATE.
      Parameters:
      camera - the camera's name
      cameraHeadingDeg - the robot heading the camera solved
      gyroYawDeg - the gyro's yaw at the frame
      tagCount - tags in the solve
      yawRateRadPerSec - the robot's turn rate now
      now - this loop's robot time

    • addFrame

      public void addFrame(String camera, boolean cameraFused, double cameraHeadingDeg, double gyroYawDeg, int tagCount, double yawRateRadPerSec, double now)
      Records one camera frame, saying whether the robot fuses that camera. Ignored unless it saw two or more tags and the robot was turning slower than MAX_YAW_RATE.
      Parameters:
      camera - the camera's name
      cameraFused - whether any of the camera's sources is fused into the pose now
      cameraHeadingDeg - the robot heading the camera solved
      gyroYawDeg - the gyro's yaw at the frame
      tagCount - tags in the solve
      yawRateRadPerSec - the robot's turn rate now
      now - this loop's robot time

    • setRobotHeading

      public void setRobotHeading(double fusedHeadingDeg, double gyroYawDeg)
      The robot's fused heading now, with the gyro's yaw now, so each camera can be compared with the heading the robot will drive by. Call every loop; NaN until the heading is seeded.
      Parameters:
      fusedHeadingDeg - the fused pose's heading, or NaN if not seeded
      gyroYawDeg - the gyro's yaw

    • evaluate

      public CheckResult evaluate(double now)
      Description copied from interface: Check
      Judges the robot now. Called only while disabled, and not every loop: PreMatchChecks spreads the checks across loops.
      Specified by:
      evaluate in interface Check
      Parameters:
      now - this loop's robot time
      Returns:
      the verdict