commit c76c19bd7320b06f4727a4ee7254a8ff6d3bd5ac
author James Kuszmaul <jabukuszmaul@gmail.com> Wed Mar 16 22:41:44 2022 -0700
committer James Kuszmaul <jabukuszmaul@gmail.com> Wed Mar 16 22:41:44 2022 -0700
tree 01a98d87618c304a1588a3c6189301c831b728d5
parent 4f215491597772516e1c4c8b4988c04206afcc60

Ignore localizer updates during autonomous

localizer pi is giving bad yaw's during auto. Ignore it.

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

y2022/control_loops/drivetrain/localizer.cc

diff --git a/y2022/control_loops/drivetrain/localizer.cc b/y2022/control_loops/drivetrain/localizer.cc
index 0735534..65df654 100644
--- a/y2022/control_loops/drivetrain/localizer.cc
+++ b/y2022/control_loops/drivetrain/localizer.cc
@@ -14,6 +14,8 @@
       localizer_output_fetcher_(
           event_loop_->MakeFetcher<frc971::controls::LocalizerOutput>(
               "/localizer")),
+      joystick_state_fetcher_(
+          event_loop_->MakeFetcher<aos::JoystickState>("/aos")),
       clock_offset_fetcher_(
           event_loop_->MakeFetcher<aos::message_bridge::ServerStatistics>(
               "/aos")) {
@@ -41,6 +43,13 @@
                        double gyro_rate, const Eigen::Vector3d &accel) {
   ekf_.UpdateEncodersAndGyro(left_encoder, right_encoder, gyro_rate,
                              U.cast<float>(), accel.cast<float>(), now);
+  joystick_state_fetcher_.Fetch();
+  if (joystick_state_fetcher_.get() != nullptr &&
+      joystick_state_fetcher_->autonomous()) {
+    // TODO(james): This is an inelegant way to avoid having the localizer mess
+    // up splines. Do better.
+    return;
+  }
   if (localizer_output_fetcher_.Fetch()) {
     clock_offset_fetcher_.Fetch();
     bool message_bridge_connected = true;

y2022/control_loops/drivetrain/localizer.h

diff --git a/y2022/control_loops/drivetrain/localizer.h b/y2022/control_loops/drivetrain/localizer.h
index 4505e9b..0d2673b 100644
--- a/y2022/control_loops/drivetrain/localizer.h
+++ b/y2022/control_loops/drivetrain/localizer.h
@@ -8,6 +8,7 @@
 #include "frc971/control_loops/drivetrain/localizer.h"
 #include "y2022/localizer/localizer_output_generated.h"
 #include "aos/network/message_bridge_server_generated.h"
+#include "frc971/input/joystick_state_generated.h"
 
 namespace y2022 {
 namespace control_loops {
@@ -67,6 +68,7 @@
   HybridEkf ekf_;
 
   aos::Fetcher<frc971::controls::LocalizerOutput> localizer_output_fetcher_;
+  aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
   aos::Fetcher<aos::message_bridge::ServerStatistics> clock_offset_fetcher_;
 
   // Target selector to allow us to satisfy the LocalizerInterface requirements.