commit 8c76a9b602f182a103ef01b3fd98392031d7e7a5
author Austin Schuh <austin.linux@gmail.com> Wed Mar 16 21:33:48 2022 -0700
committer Austin Schuh <austin.linux@gmail.com> Wed Mar 16 21:34:23 2022 -0700
tree 4556b408338ba664d47e0487ce8448cb66e544ab
parent 9d5c3eb25073d966febd37160af51c311904962f

Actually zero the spartan IMU

Change-Id: Iad56b95274f07db7b2bb68946f2d426df18734e7
Signed-off-by: Austin Schuh <austin.linux@gmail.com>

frc971/control_loops/drivetrain/drivetrain.cc

diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 7d297e0..15d84ea 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -30,8 +30,8 @@
 
 namespace {
 // Maximum variation to allow in the gyro when zeroing.
-constexpr double kMaxYawGyroZeroingRange = 0.05;
-}
+constexpr double kMaxYawGyroZeroingRange = 0.08;
+}  // namespace
 
 DrivetrainFilters::DrivetrainFilters(const DrivetrainConfig<double> &dt_config,
                                      ::aos::EventLoop *event_loop,
@@ -219,8 +219,10 @@
     case GyroType::FLIPPED_SPARTAN_GYRO:
       if (!yaw_gyro_zero_.has_value()) {
         yaw_gyro_zeroer_.AddData(last_gyro_rate_);
-        if (yaw_gyro_zeroer_.GetRange() < kMaxYawGyroZeroingRange) {
+        if (yaw_gyro_zeroer_.full() && yaw_gyro_zeroer_.GetRange() < kMaxYawGyroZeroingRange) {
           yaw_gyro_zero_ = yaw_gyro_zeroer_.GetAverage()(0);
+          VLOG(1) << "Zeroed to " << *yaw_gyro_zero_ << " Range "
+                  << yaw_gyro_zeroer_.GetRange();
         }
       }
       ready_ = yaw_gyro_zero_.has_value();