commit db85fa3816e3ab7691e9878bf2e9b3390693fa91
author James Kuszmaul <jabukuszmaul+collab@gmail.com> Sat Mar 16 16:55:47 2024 -0700
committer James Kuszmaul <jabukuszmaul+collab@gmail.com> Sat Mar 16 17:03:46 2024 -0700
tree df23a6081db817c954ac9fa141149d5ae12fd3cd
parent 2700e0fdb8b5e00e3e425f8c88acbaa064ba7761

Expose more gyro zeroing parameters

These tuning values are somewhat hardware-specific. Don't actually use
this yet, since it didn't turn out to be necessary for what I was trying
to accomplish.

Change-Id: I4ba19ca0595ae1d1d8988b5326666acca13bb41a
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>

frc971/zeroing/imu_zeroer.cc

diff --git a/frc971/zeroing/imu_zeroer.cc b/frc971/zeroing/imu_zeroer.cc
index c06ed43..3164bed 100644
--- a/frc971/zeroing/imu_zeroer.cc
+++ b/frc971/zeroing/imu_zeroer.cc
@@ -25,8 +25,8 @@
 }
 }  // namespace
 
-ImuZeroer::ImuZeroer(FaultBehavior fault_behavior)
-    : fault_behavior_(fault_behavior) {
+ImuZeroer::ImuZeroer(FaultBehavior fault_behavior, double gyro_max_variation)
+    : fault_behavior_(fault_behavior), gyro_max_variation_(gyro_max_variation) {
   gyro_average_.setZero();
   last_gyro_sample_.setZero();
   last_accel_sample_.setZero();
@@ -53,7 +53,7 @@
 
 bool ImuZeroer::GyroZeroReady() const {
   return gyro_averager_.full() &&
-         gyro_averager_.GetRange() < kGyroMaxVariation &&
+         gyro_averager_.GetRange() < gyro_max_variation_ &&
          (last_gyro_sample_.lpNorm<Eigen::Infinity>() <
           kGyroMaxZeroingMagnitude);
 }

frc971/zeroing/imu_zeroer.h

diff --git a/frc971/zeroing/imu_zeroer.h b/frc971/zeroing/imu_zeroer.h
index 2a5036d..52f6cc0 100644
--- a/frc971/zeroing/imu_zeroer.h
+++ b/frc971/zeroing/imu_zeroer.h
@@ -30,7 +30,13 @@
     kTemporary
   };
 
-  explicit ImuZeroer(FaultBehavior fault_behavior = FaultBehavior::kLatch);
+  // Max variation (difference between the maximum and minimum value) in a
+  // kSamplesToAverage range before we allow using the samples for zeroing.
+  // These values are currently based on looking at results from the ADIS16448.
+  static constexpr double kGyroMaxVariation = 0.02;  // rad / sec
+
+  explicit ImuZeroer(FaultBehavior fault_behavior = FaultBehavior::kLatch,
+                     double gyro_max_variation = kGyroMaxVariation);
   bool Zeroed() const;
   bool Faulted() const;
   bool InsertMeasurement(const IMUValues &values);
@@ -45,10 +51,6 @@
       flatbuffers::FlatBufferBuilder *fbb) const;
 
  private:
-  // Max variation (difference between the maximum and minimum value) in a
-  // kSamplesToAverage range before we allow using the samples for zeroing.
-  // These values are currently based on looking at results from the ADIS16448.
-  static constexpr double kGyroMaxVariation = 0.02;  // rad / sec
   // Maximum magnitude we allow the gyro zero to have--this is used to prevent
   // us from zeroing the gyro if we just happen to be spinning at a very
   // consistent non-zero rate. Currently this is only plausible in simulation.
@@ -74,6 +76,7 @@
   Eigen::Vector3d last_accel_sample_;
 
   const FaultBehavior fault_behavior_;
+  const double gyro_max_variation_;
   bool reading_faulted_ = false;
   bool zeroing_faulted_ = false;