Improve gyro zeroing
Instead of using a single large data chunk for zeroing, collect multiple
shorter samples that we then gradually accumulate.
Change-Id: I5496a7c47a703764fcd0f5b1498ef080e9b30fe9
diff --git a/frc971/zeroing/imu_zeroer_test.cc b/frc971/zeroing/imu_zeroer_test.cc
index 9ec52da..7a74413 100644
--- a/frc971/zeroing/imu_zeroer_test.cc
+++ b/frc971/zeroing/imu_zeroer_test.cc
@@ -4,6 +4,9 @@
namespace frc971::zeroing {
+static constexpr int kMinSamplesToZero =
+ 2 * ImuZeroer::kSamplesToAverage * ImuZeroer::kRequiredZeroPoints;
+
aos::FlatbufferDetachedBuffer<IMUValues> MakeMeasurement(
const Eigen::Vector3d &gyro, const Eigen::Vector3d &accel) {
flatbuffers::FlatBufferBuilder fbb;
@@ -29,13 +32,14 @@
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());
+ zeroer.ProcessMeasurement(
+ MakeMeasurement({0.01, 0.02, 0.03}, {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_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());
@@ -45,16 +49,16 @@
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());
+ for (size_t ii = 0; ii < kMinSamplesToZero; ++ii) {
+ zeroer.ProcessMeasurement(
+ MakeMeasurement({0.01, 0.02, 0.03}, {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_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());
@@ -64,11 +68,25 @@
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());
+ zeroer.ProcessMeasurement(
+ MakeMeasurement({0.02, 0.03, 0.04}, {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());
+ ASSERT_FLOAT_EQ(0.01, zeroer.ZeroedGyro().x());
+ ASSERT_FLOAT_EQ(0.01, zeroer.ZeroedGyro().y());
+ ASSERT_FLOAT_EQ(0.01, zeroer.ZeroedGyro().z());
+}
+
+// Tests that we do not zero if the gyro is producing particularly high
+// magnitude results.
+TEST(ImuZeroerTest, NoZeroOnHighMagnitudeGyro) {
+ ImuZeroer zeroer;
+ ASSERT_FALSE(zeroer.Zeroed());
+ for (size_t ii = 0; ii < kMinSamplesToZero; ++ii) {
+ zeroer.ProcessMeasurement(
+ MakeMeasurement({0.1, 0.2, 0.3}, {4, 5, 6}).message());
+ ASSERT_FALSE(zeroer.Zeroed());
+ }
+ ASSERT_FALSE(zeroer.Faulted());
}
// Tests that we tolerate small amounts of noise in the incoming data and can
@@ -76,29 +94,27 @@
TEST(ImuZeroerTest, ZeroOnLowNoiseData) {
ImuZeroer zeroer;
ASSERT_FALSE(zeroer.Zeroed());
- for (size_t ii = 0; ii < ImuZeroer::kSamplesToAverage; ++ii) {
- ASSERT_FALSE(zeroer.Zeroed());
+ for (size_t ii = 0; ii < kMinSamplesToZero; ++ii) {
const double offset =
- (static_cast<double>(ii) / (ImuZeroer::kSamplesToAverage - 1) - 0.5) *
- 0.01;
+ (static_cast<double>(ii) / (kMinSamplesToZero - 1) - 0.5) * 0.001;
zeroer.ProcessMeasurement(
- MakeMeasurement({1 + offset, 2 + offset, 3 + offset},
+ MakeMeasurement({0.01 + offset, 0.02 + offset, 0.03 + 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-8);
- ASSERT_NEAR(2.0, zeroer.GyroOffset().y(), 1e-8);
- ASSERT_NEAR(3.0, zeroer.GyroOffset().z(), 1e-8);
- // 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_NEAR(0.01, zeroer.GyroOffset().x(), 1e-3);
+ ASSERT_NEAR(0.02, zeroer.GyroOffset().y(), 1e-3);
+ ASSERT_NEAR(0.03, zeroer.GyroOffset().z(), 1e-3);
+ // If we get another measurement offset by {0.01, 0.01, 0.01} we should read
+ // the result as {0.01, 0.01, 0.01}.
+ zeroer.ProcessMeasurement(
+ MakeMeasurement({0.02, 0.03, 0.04}, {0, 0, 0}).message());
ASSERT_FALSE(zeroer.Faulted());
- ASSERT_NEAR(1.0, zeroer.ZeroedGyro().x(), 1e-8);
- ASSERT_NEAR(1.0, zeroer.ZeroedGyro().y(), 1e-8);
- ASSERT_NEAR(1.0, zeroer.ZeroedGyro().z(), 1e-8);
+ 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());
@@ -108,13 +124,12 @@
TEST(ImuZeroerTest, NoZeroOnHighNoiseData) {
ImuZeroer zeroer;
ASSERT_FALSE(zeroer.Zeroed());
- for (size_t ii = 0; ii < ImuZeroer::kSamplesToAverage; ++ii) {
+ for (size_t ii = 0; ii < kMinSamplesToZero; ++ii) {
ASSERT_FALSE(zeroer.Zeroed());
const double offset =
- (static_cast<double>(ii) / (ImuZeroer::kSamplesToAverage - 1) - 0.5) *
- 1.0;
+ (static_cast<double>(ii) / (kMinSamplesToZero - 1) - 0.5) * 1.0;
zeroer.ProcessMeasurement(
- MakeMeasurement({1 + offset, 2 + offset, 3 + offset},
+ MakeMeasurement({0.01 + offset, 0.02 + offset, 0.03 + offset},
{4 + offset, 5 + offset, 6 + offset})
.message());
}
@@ -127,15 +142,16 @@
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());
+ for (size_t ii = 0; ii < kMinSamplesToZero; ++ii) {
+ zeroer.ProcessMeasurement(
+ MakeMeasurement({0.01, 0.02, 0.03}, {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_FALSE(zeroer.Faulted())
+ << "We should not fault until we complete a second cycle of zeroing.";
+ for (size_t ii = 0; ii < kMinSamplesToZero; ++ii) {
+ zeroer.ProcessMeasurement(
+ MakeMeasurement({0.01, 0.05, 0.03}, {4, 5, 6}).message());
}
ASSERT_TRUE(zeroer.Faulted());
}
@@ -144,14 +160,14 @@
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());
+ for (size_t ii = 0; ii < kMinSamplesToZero; ++ii) {
+ zeroer.ProcessMeasurement(
+ MakeMeasurement({0.01, 0.02, 0.03}, {4, 5, 6}).message());
}
ASSERT_TRUE(zeroer.Zeroed());
- for (size_t ii = 0; ii < ImuZeroer::kSamplesToAverage; ++ii) {
+ for (size_t ii = 0; ii < kMinSamplesToZero; ++ii) {
zeroer.ProcessMeasurement(
- MakeMeasurement({1, 2.0001, 3}, {4, 5, 6}).message());
+ MakeMeasurement({0.01, 0.020001, 0.03}, {4, 5, 6}).message());
}
ASSERT_FALSE(zeroer.Faulted());
}