Separate out CPU-heavy portion of IMU zeroing
This lets us run the hard stuff only once per drivetrain iteration,
rather than 10 times per drivetrain iteration.
Change-Id: I262f41cafaf3b34fbd7f5205896dfdcde808db77
diff --git a/frc971/zeroing/imu_zeroer.cc b/frc971/zeroing/imu_zeroer.cc
index b13b43d..9e82d13 100644
--- a/frc971/zeroing/imu_zeroer.cc
+++ b/frc971/zeroing/imu_zeroer.cc
@@ -35,15 +35,23 @@
accel_averager_.GetRange() < kAccelMaxVariation;
}
-void ImuZeroer::ProcessMeasurement(const IMUValues &values) {
+void ImuZeroer::InsertAndProcessMeasurement(const IMUValues &values) {
+ InsertMeasurement(values);
+ ProcessMeasurements();
+}
+
+void ImuZeroer::InsertMeasurement(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_);
+}
+
+void ImuZeroer::ProcessMeasurements() {
if (GyroZeroReady() && AccelZeroReady()) {
++good_iters_;
- if (good_iters_ > kSamplesToAverage / 4) {
+ if (good_iters_ > kSamplesToAverage / 40) {
const Eigen::Vector3d current_gyro_average = gyro_averager_.GetAverage();
constexpr double kAverageUpdateWeight = 0.05;
if (num_zeroes_ > 0) {