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>
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;