Improve handling of absent encoder values
This is the first year that we have relied on the network to forward
encoder values to the localizer, which makes it so that we can no longer
rely on encoder values being available. Improve our behavior in these
cases to prevent us from destroying the localizer's state too much when
we lose encoders/network.
Change-Id: I664d331a8249873987402a819a38ad75e70fc4ff
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/frc971/control_loops/drivetrain/localization/utils.cc b/frc971/control_loops/drivetrain/localization/utils.cc
index 0986037..14bc2ef 100644
--- a/frc971/control_loops/drivetrain/localization/utils.cc
+++ b/frc971/control_loops/drivetrain/localization/utils.cc
@@ -41,7 +41,9 @@
template <typename T>
std::optional<Eigen::Vector2d> GetPosition(
T &fetcher, aos::monotonic_clock::time_point now) {
- fetcher.Fetch();
+ if (!fetcher.Fetch()) {
+ return std::nullopt;
+ }
const bool stale =
(fetcher.get() == nullptr) ||
(fetcher.context().monotonic_event_time + std::chrono::milliseconds(10) <