Improve gyro zeroing
Instead of using a single large data chunk for zeroing, collect multiple
shorter samples that we then gradually accumulate.
Change-Id: I5496a7c47a703764fcd0f5b1498ef080e9b30fe9
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 094a947..1afc32d 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -147,6 +147,9 @@
while (imu_values_fetcher_.FetchNext()) {
imu_zeroer_.ProcessMeasurement(*imu_values_fetcher_);
last_gyro_time_ = monotonic_now;
+ if (!imu_zeroer_.Zeroed()) {
+ continue;
+ }
aos::monotonic_clock::time_point reading_time(std::chrono::nanoseconds(
imu_values_fetcher_->monotonic_timestamp_ns()));
if (last_imu_update_ == aos::monotonic_clock::min_time) {