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/wpilib/gyro_sender.cc b/frc971/wpilib/gyro_sender.cc
index 4f88f9e..0b590f9 100644
--- a/frc971/wpilib/gyro_sender.cc
+++ b/frc971/wpilib/gyro_sender.cc
@@ -136,7 +136,7 @@
if (joystick_state_fetcher_.get() &&
joystick_state_fetcher_->outputs_enabled() &&
zeroing_data_.full()) {
- zero_offset_ = -zeroing_data_.GetAverage();
+ zero_offset_ = -zeroing_data_.GetAverage()(0, 0);
AOS_LOG(INFO, "total zero offset %f\n", zero_offset_);
zeroed_ = true;
}
diff --git a/frc971/zeroing/BUILD b/frc971/zeroing/BUILD
index fe95bdc..ffd1114 100644
--- a/frc971/zeroing/BUILD
+++ b/frc971/zeroing/BUILD
@@ -7,6 +7,10 @@
hdrs = [
"averager.h",
],
+ deps = [
+ "@com_github_google_glog//:glog",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
)
cc_test(
@@ -17,6 +21,36 @@
deps = [
":averager",
"//aos/testing:googletest",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
+
+cc_library(
+ name = "imu_zeroer",
+ srcs = [
+ "imu_zeroer.cc",
+ ],
+ hdrs = [
+ "imu_zeroer.h",
+ ],
+ deps = [
+ ":averager",
+ "//frc971/wpilib:imu_fbs",
+ "@com_github_google_glog//:glog",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
+
+cc_test(
+ name = "imu_zeroer_test",
+ srcs = [
+ "imu_zeroer_test.cc",
+ ],
+ deps = [
+ ":imu_zeroer",
+ "//aos:flatbuffers",
+ "//aos/testing:googletest",
+ "@org_tuxfamily_eigen//:eigen",
],
)
diff --git a/frc971/zeroing/averager.h b/frc971/zeroing/averager.h
index 879c246..b6ff09d 100644
--- a/frc971/zeroing/averager.h
+++ b/frc971/zeroing/averager.h
@@ -5,45 +5,79 @@
#include <array>
#include <stdint.h>
+#include "Eigen/Dense"
+#include "glog/logging.h"
+
namespace frc971 {
namespace zeroing {
// Averages a set of given numbers. Numbers are given one at a time. Once full
// the average may be requested.
-template <typename data_type, size_t data_size>
+// TODO(james): Look at deduplicating this with some of the work in the
+// MoveDetector.
+template <typename Scalar, size_t num_samples, int rows_per_sample = 1>
class Averager {
public:
+ typedef Eigen::Matrix<Scalar, rows_per_sample, 1> Vector;
// Adds one data point to the set of data points to be averaged.
- // If more than "data_size" samples are added, they will start overwriting
+ // If more than "num_samples" samples are added, they will start overwriting
// the oldest ones.
- void AddData(data_type data) {
+ void AddData(Scalar data) {
+ CHECK_EQ(1, rows_per_sample);
+ AddData(Vector(data));
+ }
+ void AddData(const Vector &data) {
data_[data_point_index_] = data;
- num_data_points_ = ::std::min(data_size, num_data_points_ + 1);
- data_point_index_ = (data_point_index_ + 1) % data_size;
+ num_data_points_ = std::min(num_samples, num_data_points_ + 1);
+ data_point_index_ = (data_point_index_ + 1) % num_samples;
}
// Returns the average of the data points.
- data_type GetAverage() const {
- // TODO(phil): What do we want to do without any elements?
+ Vector GetAverage() const {
if (num_data_points_ == 0) {
- return 0;
+ return Vector::Zero();
}
- data_type average = 0;
- for (data_type data : data_) {
+ Vector average;
+ average.setZero();
+ for (const Vector &data : data_) {
average += data;
}
return average / num_data_points_;
}
- // Returns true when we've gathered data_size data points.
- bool full() const { return num_data_points_ >= data_size; };
+ // Return the difference between the min and max values in the data buffer.
+ Scalar GetRange() const {
+ if (num_data_points_ == 0) {
+ return 0.0;
+ }
+ Vector min_value;
+ min_value.setConstant(std::numeric_limits<Scalar>::max());
+ Vector max_value;
+ max_value.setConstant(std::numeric_limits<Scalar>::lowest());
+ // The array will always fill up starting at zero, so we can iterate from
+ // zero safely.
+ for (size_t ii = 0; ii < num_data_points_; ++ii) {
+ const Vector &value = data_[ii];
+ min_value = min_value.cwiseMin(value);
+ max_value = max_value.cwiseMax(value);
+ }
+ return (max_value - min_value).maxCoeff();
+ }
- size_t size() const { return data_size; }
+ void Reset() {
+ num_data_points_ = 0;
+ data_point_index_ = 0;
+ }
+
+ // Returns true when we've gathered num_samples data points.
+ bool full() const { return num_data_points_ >= num_samples; };
+
+ size_t size() const { return num_samples; }
private:
// Data points to be averaged.
- ::std::array<data_type, data_size> data_;
+ std::array<Vector, num_samples> data_;
// Which data point in "data_" will be filled in next.
size_t data_point_index_ = 0;
// Number of data points added via AddData().
diff --git a/frc971/zeroing/averager_test.cc b/frc971/zeroing/averager_test.cc
index 930a3cb..9f8f727 100644
--- a/frc971/zeroing/averager_test.cc
+++ b/frc971/zeroing/averager_test.cc
@@ -17,7 +17,7 @@
averager.AddData(static_cast<int>(i));
}
ASSERT_TRUE(averager.full());
- ASSERT_EQ(2, averager.GetAverage());
+ ASSERT_EQ(2, averager.GetAverage()(0, 0));
}
// Makes sure that we can compute the average of a bunch of floats.
@@ -28,7 +28,32 @@
averager.AddData(static_cast<float>(i) / 3.0);
}
ASSERT_TRUE(averager.full());
- ASSERT_NEAR(16.5, averager.GetAverage(), 0.001);
+ ASSERT_NEAR(16.5, averager.GetAverage()(0, 0), 0.001);
+}
+
+TEST_F(AveragerTest, CalculateRange) {
+ Averager<float, 5, 2> averager;
+ ASSERT_EQ(0, averager.GetRange());
+ averager.AddData({100, 10});
+ averager.AddData({105, 15});
+ averager.AddData({90, 9});
+ ASSERT_EQ(15, averager.GetRange());
+ for (size_t ii = 0; ii < averager.size(); ++ii) {
+ averager.AddData({10, 20});
+ }
+ ASSERT_EQ(0, averager.GetRange());
+}
+
+TEST_F(AveragerTest, ResetAverager) {
+ Averager<float, 5> averager;
+ for (size_t ii = 0; ii < averager.size(); ++ii) {
+ averager.AddData(10);
+ }
+ ASSERT_TRUE(averager.full());
+ ASSERT_EQ(10.0, averager.GetAverage()(0, 0));
+ averager.Reset();
+ ASSERT_FALSE(averager.full());
+ ASSERT_EQ(0.0, averager.GetAverage()(0, 0));
}
} // namespace zeroing
diff --git a/frc971/zeroing/imu_zeroer.cc b/frc971/zeroing/imu_zeroer.cc
new file mode 100644
index 0000000..25bf6f6
--- /dev/null
+++ b/frc971/zeroing/imu_zeroer.cc
@@ -0,0 +1,52 @@
+#include "frc971/zeroing/imu_zeroer.h"
+
+namespace frc971::zeroing {
+
+ImuZeroer::ImuZeroer() {
+ gyro_average_.setZero();
+ last_gyro_sample_.setZero();
+ last_accel_sample_.setZero();
+}
+
+bool ImuZeroer::Zeroed() const { return zeroed_ || Faulted(); }
+
+bool ImuZeroer::Faulted() const { return faulted_; }
+
+Eigen::Vector3d ImuZeroer::ZeroedGyro() const {
+ return last_gyro_sample_ - gyro_average_;
+}
+Eigen::Vector3d ImuZeroer::ZeroedAccel() const { return last_accel_sample_; }
+Eigen::Vector3d ImuZeroer::GyroOffset() const { return gyro_average_; }
+
+bool ImuZeroer::GyroZeroReady() const {
+ return gyro_averager_.full() && gyro_averager_.GetRange() < kGyroMaxVariation;
+}
+
+bool ImuZeroer::AccelZeroReady() const {
+ return accel_averager_.full() &&
+ accel_averager_.GetRange() < kAccelMaxVariation;
+}
+
+void ImuZeroer::ProcessMeasurement(const IMUValues &values) {
+ last_gyro_sample_ << values.gyro_x(), values.gyro_y(), values.gyro_z();
+ gyro_averager_.AddData(last_gyro_sample_);
+ last_accel_sample_ << values.accelerometer_x(), values.accelerometer_y(),
+ values.accelerometer_z();
+ accel_averager_.AddData(last_accel_sample_);
+ if (GyroZeroReady() && AccelZeroReady()) {
+ if (!zeroed_) {
+ zeroed_ = true;
+ gyro_average_ = gyro_averager_.GetAverage();
+ } else {
+ // If we got a new zero and it is substantially different from the
+ // original zero, fault.
+ if ((gyro_averager_.GetAverage() - gyro_average_).norm() >
+ kGyroFaultVariation) {
+ faulted_ = true;
+ }
+ }
+ gyro_averager_.Reset();
+ }
+}
+
+} // namespace frc971::zeroing
diff --git a/frc971/zeroing/imu_zeroer.h b/frc971/zeroing/imu_zeroer.h
new file mode 100644
index 0000000..e95c231
--- /dev/null
+++ b/frc971/zeroing/imu_zeroer.h
@@ -0,0 +1,54 @@
+#ifndef FRC971_ZEROING_IMU_ZEROER_H_
+#define FRC971_ZEROING_IMU_ZEROER_H_
+
+#include "frc971/wpilib/imu_generated.h"
+#include "frc971/zeroing/averager.h"
+
+namespace frc971::zeroing {
+
+// This class handles processing IMU measurements and zeroing them once it is
+// able to do so.
+class ImuZeroer {
+ public:
+ // Average 5 seconds of data (assuming 2kHz sampling rate).
+ static constexpr size_t kSamplesToAverage = 10000.0;
+
+ ImuZeroer();
+ bool Zeroed() const;
+ bool Faulted() const;
+ void ProcessMeasurement(const IMUValues &values);
+ Eigen::Vector3d GyroOffset() const;
+ Eigen::Vector3d ZeroedGyro() const;
+ Eigen::Vector3d ZeroedAccel() const;
+ private:
+ // Max variation (difference between the maximum and minimum value) in a
+ // kSamplesToAverage range before we allow using the samples for zeroing.
+ // These values are currently based on looking at results from the ADIS16448.
+ static constexpr double kGyroMaxVariation = 0.02; // rad / sec
+ // Max variation in the range before we consider the accelerometer readings to
+ // be steady.
+ static constexpr double kAccelMaxVariation = 0.02; // g's
+ // If we ever are able to rezero and get a zero that is more than
+ // kGyroFaultVariation away from the original zeroing, fault.
+ static constexpr double kGyroFaultVariation = 0.005; // rad / sec
+
+ bool GyroZeroReady() const;
+ bool AccelZeroReady() const;
+
+ Averager<double, kSamplesToAverage, 3> gyro_averager_;
+ // Averager for the accelerometer readings--we don't currently actually
+ // average the readings, but we do check that the accelerometer readings have
+ // stayed roughly constant during the calibration period.
+ Averager<double, kSamplesToAverage, 3> accel_averager_;
+ // The average zero position of the gyro.
+ Eigen::Vector3d gyro_average_;
+ Eigen::Vector3d last_gyro_sample_;
+ Eigen::Vector3d last_accel_sample_;
+ // Whether we have currently zeroed yet.
+ bool zeroed_ = false;
+ // Whether the zeroing has faulted at any point thus far.
+ bool faulted_ = false;
+};
+
+} // namespace frc971::zeroing
+#endif // FRC971_ZEROING_IMU_ZEROER_H_
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