commit b7f45bb93a4eae2ac6f266a57be465d2fd7a4290
author James Kuszmaul <jabukuszmaul@gmail.com> Wed Feb 26 20:27:48 2020 -0800
committer James Kuszmaul <jabukuszmaul@gmail.com> Wed Feb 26 20:47:06 2020 -0800
tree d05ee27841cf2c8fa3d732e0239935776565be79
parent 818b974a95c1b4684933b277394f7a5d2fd3b91c

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

frc971/control_loops/drivetrain/drivetrain.cc

diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index a19f57b..77c52c4 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -148,7 +148,7 @@
   }
 
   while (imu_values_fetcher_.FetchNext()) {
-    imu_zeroer_.ProcessMeasurement(*imu_values_fetcher_);
+    imu_zeroer_.InsertMeasurement(*imu_values_fetcher_);
     last_gyro_time_ = monotonic_now;
     if (!imu_zeroer_.Zeroed()) {
       continue;
@@ -165,6 +165,7 @@
 
   bool got_imu_reading = false;
   if (imu_values_fetcher_.get() != nullptr) {
+    imu_zeroer_.ProcessMeasurements();
     got_imu_reading = true;
     switch (dt_config_.imu_type) {
       case IMUType::IMU_X: