commit 6d6e13067dcc1627737a968c7168e46f0ecb21b8
author James Kuszmaul <jabukuszmaul@gmail.com> Sat Mar 12 15:22:48 2022 -0800
committer James Kuszmaul <jabukuszmaul@gmail.com> Sun Mar 13 17:19:52 2022 -0700
tree 6a7499cc49ba48c1dfe2ec4d8be343ea1510de6f
parent 5f0d110cf8df4e04fb3699a0a50c9fd4715b6ec6

Don't latch IMU faults in 2022 localizer

We get occasional checksum failures. Don't brick the localizer because
of that.

Change-Id: I67fa55264ac56127b7a8f150db35bf7747af7468
Signed-off-by: James Kuszmaul <jabukuszmaul@gmail.com>

frc971/control_loops/drivetrain/drivetrain.cc

diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 4b36714..7d297e0 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -137,8 +137,8 @@
       if (last_imu_update_ == aos::monotonic_clock::min_time) {
         last_imu_update_ = reading_time;
       }
-      down_estimator_.Predict(imu_zeroer_.ZeroedGyro(),
-                              imu_zeroer_.ZeroedAccel(),
+      down_estimator_.Predict(imu_zeroer_.ZeroedGyro().value(),
+                              imu_zeroer_.ZeroedAccel().value(),
                               reading_time - last_imu_update_);
       last_imu_update_ = reading_time;
     }
@@ -170,26 +170,31 @@
   }
 
   // TODO(austin): Signal the current gear to both loops.
+  bool imu_zeroer_zeroed = imu_zeroer_.Zeroed();
 
   switch (dt_config_.gyro_type) {
     case GyroType::IMU_X_GYRO:
       if (got_imu_reading) {
-        last_gyro_rate_ = imu_zeroer_.ZeroedGyro().x();
+        last_gyro_rate_ =
+            imu_zeroer_zeroed ? imu_zeroer_.ZeroedGyro().value().x() : 0.0;
       }
       break;
     case GyroType::IMU_Y_GYRO:
       if (got_imu_reading) {
-        last_gyro_rate_ = imu_zeroer_.ZeroedGyro().y();
+        last_gyro_rate_ =
+            imu_zeroer_zeroed ? imu_zeroer_.ZeroedGyro().value().y() : 0.0;
       }
       break;
     case GyroType::IMU_Z_GYRO:
       if (got_imu_reading) {
-        last_gyro_rate_ = imu_zeroer_.ZeroedGyro().z();
+        last_gyro_rate_ =
+            imu_zeroer_zeroed ? imu_zeroer_.ZeroedGyro().value().z() : 0.0;
       }
       break;
     case GyroType::FLIPPED_IMU_Z_GYRO:
       if (got_imu_reading) {
-        last_gyro_rate_ = -imu_zeroer_.ZeroedGyro().z();
+        last_gyro_rate_ =
+            imu_zeroer_zeroed ? -imu_zeroer_.ZeroedGyro().value().z() : 0.0;
       }
       break;
     case GyroType::SPARTAN_GYRO: