commit c53de4a946b9e06f53f9352f4a49d05997cf825e
author James Kuszmaul <jabukuszmaul@gmail.com> Sat Mar 12 21:32:06 2022 -0800
committer James Kuszmaul <jabukuszmaul@gmail.com> Sat Mar 12 21:33:00 2022 -0800
tree c5428a68127de9c67466600cbe3c57215f227060
parent 01329b4841e1c78403ce9206d256a3657e179fbc

Add zeroing for single-axis gyro in drivetrain

This ensures that if the pico gets a bad zero we still handle it
properly on the roboRIO.

Change-Id: Ia920ab5698974d4c672a219f01303c404eb4fcb5
Signed-off-by: James Kuszmaul <jabukuszmaul@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 66a3dcd..4b36714 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -28,6 +28,11 @@
 namespace control_loops {
 namespace drivetrain {
 
+namespace {
+// Maximum variation to allow in the gyro when zeroing.
+constexpr double kMaxYawGyroZeroingRange = 0.05;
+}
+
 DrivetrainFilters::DrivetrainFilters(const DrivetrainConfig<double> &dt_config,
                                      ::aos::EventLoop *event_loop,
                                      LocalizerInterface *localizer)
@@ -204,9 +209,27 @@
       break;
   }
 
-  ready_ = dt_config_.gyro_type == GyroType::SPARTAN_GYRO ||
-           dt_config_.gyro_type == GyroType::FLIPPED_SPARTAN_GYRO ||
-           imu_zeroer_.Zeroed();
+  switch (dt_config_.gyro_type) {
+    case GyroType::SPARTAN_GYRO:
+    case GyroType::FLIPPED_SPARTAN_GYRO:
+      if (!yaw_gyro_zero_.has_value()) {
+        yaw_gyro_zeroer_.AddData(last_gyro_rate_);
+        if (yaw_gyro_zeroer_.GetRange() < kMaxYawGyroZeroingRange) {
+          yaw_gyro_zero_ = yaw_gyro_zeroer_.GetAverage()(0);
+        }
+      }
+      ready_ = yaw_gyro_zero_.has_value();
+      if (ready_) {
+        last_gyro_rate_ = last_gyro_rate_ - yaw_gyro_zero_.value();
+      }
+      break;
+    case GyroType::IMU_X_GYRO:
+    case GyroType::IMU_Y_GYRO:
+    case GyroType::IMU_Z_GYRO:
+    case GyroType::FLIPPED_IMU_Z_GYRO:
+      ready_ = imu_zeroer_.Zeroed();
+      break;
+  }
 
   // TODO(james): How aggressively can we fault here? If we fault to
   // aggressively, we might have issues during startup if wpilib_interface takes

frc971/control_loops/drivetrain/drivetrain.h

diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index 52ae605..a06b965 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -138,6 +138,9 @@
   // Last applied voltage.
   Eigen::Matrix<double, 2, 1> last_voltage_;
   Eigen::Matrix<double, 2, 1> last_last_voltage_;
+
+  std::optional<double> yaw_gyro_zero_;
+  zeroing::Averager<double, 200> yaw_gyro_zeroer_;
 };
 
 class DrivetrainLoop