Write class to handle gyro zeroing
Since we're moving the gyro zeroing into the drivetrain, take the
opportunity to write a new class to wrap it and to handle automatically
zeroing us any time we stay still for 5 seconds.
Change-Id: I9be7c970b6bbe3cf1eddc217c93467dfc21cd4cd
diff --git a/frc971/zeroing/imu_zeroer_test.cc b/frc971/zeroing/imu_zeroer_test.cc
new file mode 100644
index 0000000..9919e2f
--- /dev/null
+++ b/frc971/zeroing/imu_zeroer_test.cc
@@ -0,0 +1,159 @@
+#include "aos/flatbuffers.h"
+#include "gtest/gtest.h"
+#include "frc971/zeroing/imu_zeroer.h"
+
+namespace frc971::zeroing {
+
+aos::FlatbufferDetachedBuffer<IMUValues> MakeMeasurement(
+ const Eigen::Vector3d &gyro, const Eigen::Vector3d &accel) {
+ flatbuffers::FlatBufferBuilder fbb;
+ fbb.ForceDefaults(1);
+ IMUValuesBuilder builder(fbb);
+ builder.add_gyro_x(gyro.x());
+ builder.add_gyro_y(gyro.y());
+ builder.add_gyro_z(gyro.z());
+ builder.add_accelerometer_x(accel.x());
+ builder.add_accelerometer_y(accel.y());
+ builder.add_accelerometer_z(accel.z());
+ fbb.Finish(builder.Finish());
+ return fbb.Release();
+}
+
+// Tests that when we initialize everything is in a sane state.
+TEST(ImuZeroerTest, InitializeUnzeroed) {
+ ImuZeroer zeroer;
+ 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());
+ // A measurement before we are zeroed should just result in the measurement
+ // being passed through without modification.
+ zeroer.ProcessMeasurement(MakeMeasurement({1, 2, 3}, {4, 5, 6}).message());
+ ASSERT_FALSE(zeroer.Zeroed());
+ ASSERT_FALSE(zeroer.Faulted());
+ ASSERT_EQ(0.0, zeroer.GyroOffset().norm());
+ ASSERT_EQ(1.0, zeroer.ZeroedGyro().x());
+ ASSERT_EQ(2.0, zeroer.ZeroedGyro().y());
+ ASSERT_EQ(3.0, zeroer.ZeroedGyro().z());
+ ASSERT_EQ(4.0, zeroer.ZeroedAccel().x());
+ ASSERT_EQ(5.0, zeroer.ZeroedAccel().y());
+ ASSERT_EQ(6.0, zeroer.ZeroedAccel().z());
+}
+
+// Tests that we zero if we receive a bunch of identical measurements.
+TEST(ImuZeroerTest, ZeroOnConstantData) {
+ ImuZeroer zeroer;
+ ASSERT_FALSE(zeroer.Zeroed());
+ for (size_t ii = 0; ii < ImuZeroer::kSamplesToAverage; ++ii) {
+ ASSERT_FALSE(zeroer.Zeroed());
+ zeroer.ProcessMeasurement(MakeMeasurement({1, 2, 3}, {4, 5, 6}).message());
+ }
+ ASSERT_TRUE(zeroer.Zeroed());
+ ASSERT_FALSE(zeroer.Faulted());
+ // Gyro should be zeroed to {1, 2, 3}.
+ ASSERT_EQ(1.0, zeroer.GyroOffset().x());
+ ASSERT_EQ(2.0, zeroer.GyroOffset().y());
+ ASSERT_EQ(3.0, zeroer.GyroOffset().z());
+ ASSERT_EQ(0.0, zeroer.ZeroedGyro().x());
+ ASSERT_EQ(0.0, zeroer.ZeroedGyro().y());
+ ASSERT_EQ(0.0, zeroer.ZeroedGyro().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());
+ // If we get another measurement offset by {1, 1, 1} we should read the result
+ // as {1, 1, 1}.
+ zeroer.ProcessMeasurement(MakeMeasurement({2, 3, 4}, {0, 0, 0}).message());
+ ASSERT_FALSE(zeroer.Faulted());
+ ASSERT_EQ(1.0, zeroer.ZeroedGyro().x());
+ ASSERT_EQ(1.0, zeroer.ZeroedGyro().y());
+ ASSERT_EQ(1.0, zeroer.ZeroedGyro().z());
+}
+
+// Tests that we tolerate small amounts of noise in the incoming data and can
+// still zero.
+TEST(ImuZeroerTest, ZeroOnLowNoiseData) {
+ ImuZeroer zeroer;
+ ASSERT_FALSE(zeroer.Zeroed());
+ for (size_t ii = 0; ii < ImuZeroer::kSamplesToAverage; ++ii) {
+ ASSERT_FALSE(zeroer.Zeroed());
+ const double offset =
+ (static_cast<double>(ii) / (ImuZeroer::kSamplesToAverage - 1) - 0.5) *
+ 0.01;
+ zeroer.ProcessMeasurement(
+ MakeMeasurement({1 + offset, 2 + offset, 3 + offset},
+ {4 + offset, 5 + offset, 6 + offset})
+ .message());
+ }
+ ASSERT_TRUE(zeroer.Zeroed());
+ ASSERT_FALSE(zeroer.Faulted());
+ // Gyro should be zeroed to {1, 2, 3}.
+ ASSERT_NEAR(1.0, zeroer.GyroOffset().x(), 1e-10);
+ ASSERT_NEAR(2.0, zeroer.GyroOffset().y(), 1e-10);
+ ASSERT_NEAR(3.0, zeroer.GyroOffset().z(), 1e-10);
+ // If we get another measurement offset by {1, 1, 1} we should read the result
+ // as {1, 1, 1}.
+ zeroer.ProcessMeasurement(MakeMeasurement({2, 3, 4}, {0, 0, 0}).message());
+ ASSERT_FALSE(zeroer.Faulted());
+ ASSERT_NEAR(1.0, zeroer.ZeroedGyro().x(), 1e-10);
+ ASSERT_NEAR(1.0, zeroer.ZeroedGyro().y(), 1e-10);
+ ASSERT_NEAR(1.0, zeroer.ZeroedGyro().z(), 1e-10);
+ ASSERT_EQ(0.0, zeroer.ZeroedAccel().x());
+ ASSERT_EQ(0.0, zeroer.ZeroedAccel().y());
+ ASSERT_EQ(0.0, zeroer.ZeroedAccel().z());
+}
+
+// Tests that we do not zero if there is too much noise in the input data.
+TEST(ImuZeroerTest, NoZeroOnHighNoiseData) {
+ ImuZeroer zeroer;
+ ASSERT_FALSE(zeroer.Zeroed());
+ for (size_t ii = 0; ii < ImuZeroer::kSamplesToAverage; ++ii) {
+ ASSERT_FALSE(zeroer.Zeroed());
+ const double offset =
+ (static_cast<double>(ii) / (ImuZeroer::kSamplesToAverage - 1) - 0.5) *
+ 1.0;
+ zeroer.ProcessMeasurement(
+ MakeMeasurement({1 + offset, 2 + offset, 3 + offset},
+ {4 + offset, 5 + offset, 6 + offset})
+ .message());
+ }
+ ASSERT_FALSE(zeroer.Zeroed());
+ ASSERT_FALSE(zeroer.Faulted());
+}
+
+// Tests that we fault if we successfully rezero and get a significantly offset
+// zero.
+TEST(ImuZeroerTest, FaultOnNewZero) {
+ ImuZeroer zeroer;
+ ASSERT_FALSE(zeroer.Zeroed());
+ for (size_t ii = 0; ii < ImuZeroer::kSamplesToAverage; ++ii) {
+ ASSERT_FALSE(zeroer.Zeroed());
+ zeroer.ProcessMeasurement(MakeMeasurement({1, 2, 3}, {4, 5, 6}).message());
+ }
+ ASSERT_TRUE(zeroer.Zeroed());
+ for (size_t ii = 0; ii < ImuZeroer::kSamplesToAverage; ++ii) {
+ ASSERT_FALSE(zeroer.Faulted())
+ << "We should not fault until we complete a second cycle of zeroing.";
+ zeroer.ProcessMeasurement(MakeMeasurement({1, 5, 3}, {4, 5, 6}).message());
+ }
+ ASSERT_TRUE(zeroer.Faulted());
+}
+
+// Tests that we do not fault if the zero only changes by a small amount.
+TEST(ImuZeroerTest, NoFaultOnSimilarZero) {
+ ImuZeroer zeroer;
+ ASSERT_FALSE(zeroer.Zeroed());
+ for (size_t ii = 0; ii < ImuZeroer::kSamplesToAverage; ++ii) {
+ ASSERT_FALSE(zeroer.Zeroed());
+ zeroer.ProcessMeasurement(MakeMeasurement({1, 2, 3}, {4, 5, 6}).message());
+ }
+ ASSERT_TRUE(zeroer.Zeroed());
+ for (size_t ii = 0; ii < ImuZeroer::kSamplesToAverage; ++ii) {
+ zeroer.ProcessMeasurement(
+ MakeMeasurement({1, 2.0001, 3}, {4, 5, 6}).message());
+ }
+ ASSERT_FALSE(zeroer.Faulted());
+}
+
+} // namespace frc971::zeroing