commit bcd96fc3f8602b609f0a65b231a3a7af001a46b4
author James Kuszmaul <jabukuszmaul+collab@gmail.com> Mon Oct 12 20:29:32 2020 -0700
committer James Kuszmaul <jabukuszmaul+collab@gmail.com> Tue Oct 20 20:34:36 2020 -0700
tree 6a477562a2813f7468588ee8f58caa4d6c8f3eb6
parent f3950365eceba67b91e75347f76dff08f33784a7

Respect WasReset() indicator in the drivetrain

At least, pay attention to it for the localizer/down estimator.

Change-Id: I9cf8720c7ad9b2bd1f16b8e65acf824b10f3c7ed

frc971/control_loops/drivetrain/localizer.h

diff --git a/frc971/control_loops/drivetrain/localizer.h b/frc971/control_loops/drivetrain/localizer.h
index 6677145..67d681e 100644
--- a/frc971/control_loops/drivetrain/localizer.h
+++ b/frc971/control_loops/drivetrain/localizer.h
@@ -55,6 +55,8 @@
                       ::aos::monotonic_clock::time_point now,
                       double left_encoder, double right_encoder,
                       double gyro_rate, const Eigen::Vector3d &accel) = 0;
+  virtual void Reset(aos::monotonic_clock::time_point t,
+                     const Ekf::State &state) = 0;
   // Reset the absolute position of the estimator.
   virtual void ResetPosition(::aos::monotonic_clock::time_point t, double x,
                              double y, double theta, double theta_uncertainty,
@@ -148,6 +150,11 @@
                                now);
   }
 
+  void Reset(aos::monotonic_clock::time_point t,
+             const HybridEkf<double>::State &state) override {
+    ekf_.ResetInitialState(t, state, ekf_.P());
+  }
+
   void ResetPosition(::aos::monotonic_clock::time_point t, double x, double y,
                      double theta, double /*theta_override*/,
                      bool /*reset_theta*/) override {