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