(#108) Add tilt and freefall detection for ignoring bump odometry

src/main/deploy/constants/comp/DriveConstants.json

@@ -58,5 +58,17 @@
     "kG": 0.0,
     "kV": 0.82219,
     "kA": 0.0
+  },
+  "bumpOdometryIgnoreTime": {
+    "value": 0.25,
+    "unit": "Second"
+  },
+  "maxOdometryTilt": {
+    "value": 5.0,
+    "unit": "Degree"
+  },
+  "maxOdometryZAcceleration": {
+    "value": 5.0,
+    "unit": "Meter per Second per Second"
   }
 }

src/main/deploy/constants/test_drivebase/DriveConstants.json

@@ -58,5 +58,17 @@
     "kG": 0.0,
     "kV": 0.75722,
     "kA": 0.0
+  },
+  "bumpOdometryIgnoreTime": {
+    "value": 0.25,
+    "unit": "Second"
+  },
+  "maxOdometryTilt": {
+    "value": 5.0,
+    "unit": "Degree"
+  },
+  "maxOdometryZAcceleration": {
+    "value": 5.0,
+    "unit": "Meter per Second per Second"
   }
 }

src/main/java/frc/robot/constants/drive/DriveConstants.java

@@ -6,13 +6,15 @@
 import static edu.wpi.first.units.Units.MetersPerSecondPerSecond;
 import static edu.wpi.first.units.Units.RadiansPerSecond;
 import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond;
+import static edu.wpi.first.units.Units.Seconds;
 
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularAcceleration;
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.units.measure.LinearAcceleration;
 import edu.wpi.first.units.measure.LinearVelocity;
+import edu.wpi.first.units.measure.Time;
 import frc.robot.util.PIDGains;
 
 /**
@@ -56,4 +58,23 @@ public class DriveConstants {
   public PIDGains steerGains = new PIDGains(100, 0.0, 0.5, 0.1, 0.0, 2.49, 0.0);
 
   public PIDGains driveGains = new PIDGains(0.1, 0.0, 0.0, 0.20845, 0.0, 0.75722, 0.0);
+
+  /**
+   * How much time after we last detected that we were on the bump to continue ignoring vision. This
+   * value should be as small as possible without accidentally trusting odometry while still
+   * on/falling off of the bump
+   */
+  public Time bumpOdometryIgnoreTime = Seconds.of(0.25);
+
+  /**
+   * The maximum cumulative tilt (abs(pitch) + abs(roll)) that may be present before the bump
+   * odometry ignorance is triggered.
+   */
+  public Angle maxOdometryTilt = Degrees.of(5.0);
+
+  /**
+   * The maximum z acceleration (abs(z acceleration)) that may be present before the bump odometry
+   * ignorance is triggered.
+   */
+  public LinearAcceleration maxOdometryZAcceleration = MetersPerSecondPerSecond.of(5.0);
 }

src/main/java/frc/robot/subsystems/drive/Drive.java

@@ -18,6 +18,8 @@
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
+import edu.wpi.first.math.filter.Debouncer;
+import edu.wpi.first.math.filter.Debouncer.DebounceType;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
@@ -94,6 +96,17 @@ public class Drive extends SubsystemBase implements DriveTemplate {
 
   private PoseEstimator maPoseEstimator = new PoseEstimator(VecBuilder.fill(0.003, 0.003, 0.0002));
 
+  /**
+   * Whether or not we think we're on or just came off of the bump and should fully trust vision to
+   * prevent crazy odometry.
+   */
+  @AutoLogOutput(key = "Odometry/BumpDetected")
+  private boolean bumpDetected = false;
+
+  private final Debouncer bumpOdometryDebouncer =
+      new Debouncer(
+          JsonConstants.driveConstants.bumpOdometryIgnoreTime.in(Seconds), DebounceType.kFalling);
+
   public Drive(
       GyroIO gyroIO,
       ModuleIO flModuleIO,
@@ -232,6 +245,18 @@ public void periodic() {
 
     // Update gyro alert
     gyroDisconnectedAlert.set(!gyroInputs.connected && Constants.currentMode != Mode.SIM);
+
+    // Check for bump
+    double cumulativeTiltRadians =
+        Math.abs(gyroInputs.rollRadians) + Math.abs(gyroInputs.pitchRadians);
+    double zAccelMagnitude = Math.abs(gyroInputs.zAccelerationMetersPerSecondPerSecond);
+    boolean bumpDetectedNow =
+        cumulativeTiltRadians > JsonConstants.driveConstants.maxOdometryTilt.in(Radians)
+            || zAccelMagnitude
+                > JsonConstants.driveConstants.maxOdometryZAcceleration.in(
+                    MetersPerSecondPerSecond);
+
+    this.bumpDetected = bumpOdometryDebouncer.calculate(bumpDetectedNow);
   }
 
   /**
@@ -396,6 +421,12 @@ public void addVisionMeasurement(
       Pose2d visionRobotPoseMeters,
       double timestampSeconds,
       Matrix<N3, N1> visionMeasurementStdDevs) {
+    if (bumpDetected) {
+      visionMeasurementStdDevs.set(0, 0, 0.0);
+      visionMeasurementStdDevs.set(1, 0, 0.0);
+      visionMeasurementStdDevs.set(2, 0, 0.0);
+    }
+
     if (JsonConstants.featureFlags.useMAPoseEstimator) {
       maPoseEstimator.addVisionData(
           List.of(

src/main/java/frc/robot/subsystems/drive/GyroIO.java

@@ -18,6 +18,10 @@ public static class GyroIOInputs {
     public double yawVelocityRadPerSec = 0.0;
     public double[] odometryYawTimestamps = new double[] {};
     public Rotation2d[] odometryYawPositions = new Rotation2d[] {};
+
+    public double pitchRadians = 0.0;
+    public double rollRadians = 0.0;
+    public double zAccelerationMetersPerSecondPerSecond = 0.0;
   }
 
   public default void updateInputs(GyroIOInputs inputs) {}

src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java

@@ -7,6 +7,9 @@
 
 package frc.robot.subsystems.drive;
 
+import static edu.wpi.first.units.Units.MetersPerSecondPerSecond;
+import static edu.wpi.first.units.Units.Radians;
+
 import com.ctre.phoenix6.BaseStatusSignal;
 import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.configs.Pigeon2Configuration;
@@ -16,6 +19,7 @@
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.LinearAcceleration;
 import frc.robot.constants.JsonConstants;
 import java.util.Queue;
 
@@ -30,7 +34,12 @@ public class GyroIOPigeon2 implements GyroIO {
   private final Queue<Double> yawTimestampQueue;
   private final StatusSignal<AngularVelocity> yawVelocity = pigeon.getAngularVelocityZWorld();
 
-  private final BaseStatusSignal[] signals = new BaseStatusSignal[] {yaw, yawVelocity};
+  private final StatusSignal<Angle> pitch = pigeon.getPitch();
+  private final StatusSignal<Angle> roll = pigeon.getRoll();
+  private final StatusSignal<LinearAcceleration> zAccel = pigeon.getAccelerationZ();
+
+  private final BaseStatusSignal[] signals =
+      new BaseStatusSignal[] {yaw, yawVelocity, pitch, roll, zAccel};
 
   public GyroIOPigeon2() {
     if (JsonConstants.physicalDriveConstants.DrivetrainConstants.Pigeon2Configs != null) {
@@ -65,5 +74,9 @@ public void updateInputs(GyroIOInputs inputs) {
             .toArray(Rotation2d[]::new);
     yawTimestampQueue.clear();
     yawPositionQueue.clear();
+
+    inputs.pitchRadians = pitch.getValue().in(Radians);
+    inputs.rollRadians = roll.getValue().in(Radians);
+    inputs.zAccelerationMetersPerSecondPerSecond = zAccel.getValue().in(MetersPerSecondPerSecond);
   }
 }