commit f6058c56410112685cf21a6923142fc8bdfa4651
author James Kuszmaul <jabukuszmaul+collab@gmail.com> Sat Mar 16 13:53:45 2024 -0700
committer James Kuszmaul <jabukuszmaul+collab@gmail.com> Sat Mar 16 16:56:39 2024 -0700
tree 023c4a6182d52a102e41b60de52e2017edf60373
parent 2b56ff3c8e1e4a31b4c755c55371e7436bc15551

Do hysteresis in both directions on IMU blender

This also changes the algorithm a bit such that we don't strictly need X
saturated readings *in a row* to consider ourselves saturated.

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

frc971/imu_fdcan/dual_imu_blender_lib.cc

diff --git a/frc971/imu_fdcan/dual_imu_blender_lib.cc b/frc971/imu_fdcan/dual_imu_blender_lib.cc
index 2627739..6099844 100644
--- a/frc971/imu_fdcan/dual_imu_blender_lib.cc
+++ b/frc971/imu_fdcan/dual_imu_blender_lib.cc
@@ -14,7 +14,7 @@
 // Coefficient to multiply the saturation values by to give some room on where
 // we switch to tdk.
 static constexpr double kSaturationCoeff = 0.9;
-static constexpr size_t kSaturationCounterThreshold = 20;
+static constexpr int kSaturationCounterThreshold = 20;
 
 using frc971::imu_fdcan::DualImuBlender;
 
@@ -46,6 +46,12 @@
   imu_values->set_pico_timestamp_us(dual_imu->board_timestamp_us());
   imu_values->set_monotonic_timestamp_ns(dual_imu->kernel_timestamp());
   imu_values->set_data_counter(dual_imu->packet_counter());
+  // Notes on saturation strategy:
+  // We use the TDK to detect saturation because we presume that if the Murata
+  // is saturated then it may produce poor or undefined behavior (including
+  // potentially producing values that make it look like it is not saturated).
+  // In practice, the Murata does seem to behave reasonably under saturation (it
+  // just maxes out its outputs at the given value).
 
   if (std::abs(dual_imu->tdk()->gyro_x()) >=
       kSaturationCoeff * kMurataGyroSaturation) {
@@ -65,14 +71,24 @@
     imu_values->set_gyro_y(dual_imu->murata()->gyro_y());
   }
 
+  // TODO(james): Currently we only do hysteresis for the gyro Z axis because
+  // this is the only axis that is particularly critical. We should do something
+  // like this for all axes.
   if (std::abs(dual_imu->tdk()->gyro_z()) >=
       kSaturationCoeff * kMurataGyroSaturation) {
     ++saturated_counter_;
   } else {
-    saturated_counter_ = 0;
+    --saturated_counter_;
+  }
+  if (saturated_counter_ <= -kSaturationCounterThreshold) {
+    is_saturated_ = false;
+    saturated_counter_ = -kSaturationCounterThreshold;
+  } else if (saturated_counter_ >= kSaturationCounterThreshold) {
+    is_saturated_ = true;
+    saturated_counter_ = kSaturationCounterThreshold;
   }
 
-  if (saturated_counter_ > kSaturationCounterThreshold) {
+  if (is_saturated_) {
     dual_imu_blender_status_builder->set_gyro_z(imu::ImuType::TDK);
     imu_values->set_gyro_z(dual_imu->tdk()->gyro_z());
   } else {

frc971/imu_fdcan/dual_imu_blender_lib.h

diff --git a/frc971/imu_fdcan/dual_imu_blender_lib.h b/frc971/imu_fdcan/dual_imu_blender_lib.h
index b3aa5c0..223f3ed 100644
--- a/frc971/imu_fdcan/dual_imu_blender_lib.h
+++ b/frc971/imu_fdcan/dual_imu_blender_lib.h
@@ -20,7 +20,8 @@
  private:
   aos::Sender<IMUValuesBatchStatic> imu_values_batch_sender_;
   aos::Sender<imu::DualImuBlenderStatusStatic> dual_imu_blender_status_sender_;
-  size_t saturated_counter_ = 0;
+  int saturated_counter_ = 0;
+  bool is_saturated_ = false;
 };
 
 }  // namespace frc971::imu_fdcan