commit 6d6e13067dcc1627737a968c7168e46f0ecb21b8
author James Kuszmaul <jabukuszmaul@gmail.com> Sat Mar 12 15:22:48 2022 -0800
committer James Kuszmaul <jabukuszmaul@gmail.com> Sun Mar 13 17:19:52 2022 -0700
tree 6a7499cc49ba48c1dfe2ec4d8be343ea1510de6f
parent 5f0d110cf8df4e04fb3699a0a50c9fd4715b6ec6

Don't latch IMU faults in 2022 localizer

We get occasional checksum failures. Don't brick the localizer because
of that.

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

y2022/localizer/localizer.cc

diff --git a/y2022/localizer/localizer.cc b/y2022/localizer/localizer.cc
index 1aa2458..964bf74 100644
--- a/y2022/localizer/localizer.cc
+++ b/y2022/localizer/localizer.cc
@@ -862,6 +862,7 @@
     aos::EventLoop *event_loop,
     const control_loops::drivetrain::DrivetrainConfig<double> &dt_config)
     : event_loop_(event_loop),
+      dt_config_(dt_config),
       model_based_(dt_config),
       status_sender_(event_loop_->MakeSender<LocalizerStatus>("/localizer")),
       output_sender_(event_loop_->MakeSender<LocalizerOutput>("/localizer")),
@@ -962,7 +963,7 @@
           const Eigen::Vector2d encoders{
               left_encoder_.Unwrap(value->left_encoder()),
               right_encoder_.Unwrap(value->right_encoder())};
-          if (zeroer_.Zeroed()) {
+          {
             const aos::monotonic_clock::time_point pico_timestamp{
                 std::chrono::microseconds(value->pico_timestamp_us())};
             // TODO(james): If we get large enough drift off of the pico,
@@ -984,8 +985,13 @@
                 (output_fetcher_.context().monotonic_event_time +
                      std::chrono::milliseconds(10) <
                  event_loop_->context().monotonic_event_time);
+            const bool zeroed = zeroer_.Zeroed();
             model_based_.HandleImu(
-                sample_timestamp, zeroer_.ZeroedGyro(), zeroer_.ZeroedAccel(),
+                sample_timestamp,
+                zeroed ? zeroer_.ZeroedGyro().value() : Eigen::Vector3d::Zero(),
+                zeroed ? zeroer_.ZeroedAccel().value()
+                       : dt_config_.imu_transform.transpose() *
+                             Eigen::Vector3d::UnitZ(),
                 encoders,
                 disabled ? Eigen::Vector2d::Zero()
                          : Eigen::Vector2d{output_fetcher_->left_voltage(),

y2022/localizer/localizer.h

diff --git a/y2022/localizer/localizer.h b/y2022/localizer/localizer.h
index e9a61e1..be14b45 100644
--- a/y2022/localizer/localizer.h
+++ b/y2022/localizer/localizer.h
@@ -321,6 +321,7 @@
   std::optional<aos::monotonic_clock::duration> ClockOffset(
       std::string_view pi);
   aos::EventLoop *event_loop_;
+  const control_loops::drivetrain::DrivetrainConfig<double> &dt_config_;
   ModelBasedLocalizer model_based_;
   aos::Sender<LocalizerStatus> status_sender_;
   aos::Sender<LocalizerOutput> output_sender_;