commit ba59dc98dbb4f4e646c76c4e9a4df520e641ac5e
author James Kuszmaul <jabukuszmaul@gmail.com> Sat Mar 12 10:46:54 2022 -0800
committer Austin Schuh <austin.linux@gmail.com> Sat Mar 12 10:55:12 2022 -0800
tree 6997e588b6f4b888e7ac62e105a208215388bc2c
parent 1b73423dab39b17d0862839280351e010a4f77f1

check optional access in y2022 drivetrain

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

frc971/control_loops/drivetrain/hybrid_ekf.h

diff --git a/frc971/control_loops/drivetrain/hybrid_ekf.h b/frc971/control_loops/drivetrain/hybrid_ekf.h
index 2a972b6..fcbed76 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf.h
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -330,6 +330,12 @@
     }
     return X_hat();
   }
+  std::optional<State> OldestState() {
+    if (observations_.empty()) {
+      return std::nullopt;
+    }
+    return observations_.begin()->X_hat;
+  }
 
   // Returns the most recent input vector.
   Input MostRecentInput() {