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.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