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/zeroing/imu_zeroer_test.cc

diff --git a/frc971/zeroing/imu_zeroer_test.cc b/frc971/zeroing/imu_zeroer_test.cc
index 0a68541..deff1b6 100644
--- a/frc971/zeroing/imu_zeroer_test.cc
+++ b/frc971/zeroing/imu_zeroer_test.cc
@@ -42,8 +42,8 @@
   ASSERT_FALSE(zeroer.Zeroed());
   ASSERT_FALSE(zeroer.Faulted());
   ASSERT_EQ(0.0, zeroer.GyroOffset().norm());
-  ASSERT_EQ(0.0, zeroer.ZeroedGyro().norm());
-  ASSERT_EQ(0.0, zeroer.ZeroedAccel().norm());
+  ASSERT_EQ(0.0, zeroer.ZeroedGyro().value().norm());
+  ASSERT_EQ(0.0, zeroer.ZeroedAccel().value().norm());
   // A measurement before we are zeroed should just result in the measurement
   // being passed through without modification.
   zeroer.InsertAndProcessMeasurement(
@@ -51,12 +51,12 @@
   ASSERT_FALSE(zeroer.Zeroed());
   ASSERT_FALSE(zeroer.Faulted());
   ASSERT_EQ(0.0, zeroer.GyroOffset().norm());
-  ASSERT_FLOAT_EQ(0.01, zeroer.ZeroedGyro().x());
-  ASSERT_FLOAT_EQ(0.02, zeroer.ZeroedGyro().y());
-  ASSERT_FLOAT_EQ(0.03, zeroer.ZeroedGyro().z());
-  ASSERT_EQ(4.0, zeroer.ZeroedAccel().x());
-  ASSERT_EQ(5.0, zeroer.ZeroedAccel().y());
-  ASSERT_EQ(6.0, zeroer.ZeroedAccel().z());
+  ASSERT_FLOAT_EQ(0.01, zeroer.ZeroedGyro().value().x());
+  ASSERT_FLOAT_EQ(0.02, zeroer.ZeroedGyro().value().y());
+  ASSERT_FLOAT_EQ(0.03, zeroer.ZeroedGyro().value().z());
+  ASSERT_EQ(4.0, zeroer.ZeroedAccel().value().x());
+  ASSERT_EQ(5.0, zeroer.ZeroedAccel().value().y());
+  ASSERT_EQ(6.0, zeroer.ZeroedAccel().value().z());
 }
 
 // Tests that we zero if we receive a bunch of identical measurements.
@@ -73,21 +73,21 @@
   ASSERT_FLOAT_EQ(0.01, zeroer.GyroOffset().x());
   ASSERT_FLOAT_EQ(0.02, zeroer.GyroOffset().y());
   ASSERT_FLOAT_EQ(0.03, zeroer.GyroOffset().z());
-  ASSERT_EQ(0.0, zeroer.ZeroedGyro().x());
-  ASSERT_EQ(0.0, zeroer.ZeroedGyro().y());
-  ASSERT_EQ(0.0, zeroer.ZeroedGyro().z());
+  ASSERT_EQ(0.0, zeroer.ZeroedGyro().value().x());
+  ASSERT_EQ(0.0, zeroer.ZeroedGyro().value().y());
+  ASSERT_EQ(0.0, zeroer.ZeroedGyro().value().z());
   // Accelerometer readings should not be affected.
-  ASSERT_EQ(4.0, zeroer.ZeroedAccel().x());
-  ASSERT_EQ(5.0, zeroer.ZeroedAccel().y());
-  ASSERT_EQ(6.0, zeroer.ZeroedAccel().z());
+  ASSERT_EQ(4.0, zeroer.ZeroedAccel().value().x());
+  ASSERT_EQ(5.0, zeroer.ZeroedAccel().value().y());
+  ASSERT_EQ(6.0, zeroer.ZeroedAccel().value().z());
   // If we get another measurement offset by {1, 1, 1} we should read the result
   // as {1, 1, 1}.
   zeroer.InsertAndProcessMeasurement(
       MakeMeasurement({0.02, 0.03, 0.04}, {0, 0, 0}).message());
   ASSERT_FALSE(zeroer.Faulted());
-  ASSERT_FLOAT_EQ(0.01, zeroer.ZeroedGyro().x());
-  ASSERT_FLOAT_EQ(0.01, zeroer.ZeroedGyro().y());
-  ASSERT_FLOAT_EQ(0.01, zeroer.ZeroedGyro().z());
+  ASSERT_FLOAT_EQ(0.01, zeroer.ZeroedGyro().value().x());
+  ASSERT_FLOAT_EQ(0.01, zeroer.ZeroedGyro().value().y());
+  ASSERT_FLOAT_EQ(0.01, zeroer.ZeroedGyro().value().z());
 }
 
 // Tests that we do not zero if the gyro is producing particularly high
@@ -125,12 +125,12 @@
   zeroer.InsertAndProcessMeasurement(
       MakeMeasurement({0.02, 0.03, 0.04}, {0, 0, 0}).message());
   ASSERT_FALSE(zeroer.Faulted());
-  ASSERT_NEAR(0.01, zeroer.ZeroedGyro().x(), 1e-3);
-  ASSERT_NEAR(0.01, zeroer.ZeroedGyro().y(), 1e-3);
-  ASSERT_NEAR(0.01, zeroer.ZeroedGyro().z(), 1e-3);
-  ASSERT_EQ(0.0, zeroer.ZeroedAccel().x());
-  ASSERT_EQ(0.0, zeroer.ZeroedAccel().y());
-  ASSERT_EQ(0.0, zeroer.ZeroedAccel().z());
+  ASSERT_NEAR(0.01, zeroer.ZeroedGyro().value().x(), 1e-3);
+  ASSERT_NEAR(0.01, zeroer.ZeroedGyro().value().y(), 1e-3);
+  ASSERT_NEAR(0.01, zeroer.ZeroedGyro().value().z(), 1e-3);
+  ASSERT_EQ(0.0, zeroer.ZeroedAccel().value().x());
+  ASSERT_EQ(0.0, zeroer.ZeroedAccel().value().y());
+  ASSERT_EQ(0.0, zeroer.ZeroedAccel().value().z());
 }
 
 // Tests that we do not zero if there is too much noise in the input data.