Better handle missed IMU messages in localizer
Increase the max dt to better handle dropped messages.
Also, more explicitly track the reasons why different messages were
dropped. The current main culprits seem to be (a) messages missing
entirely and (b) Pi <-> PICO checksum failures.
Change-Id: I882b29a304fb0a673722d9dc37184a87e0a27a0d
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2022/localizer/localizer.h b/y2022/localizer/localizer.h
index f2cb50e..f8205d7 100644
--- a/y2022/localizer/localizer.h
+++ b/y2022/localizer/localizer.h
@@ -345,6 +345,14 @@
// Note that this can drift over sufficiently long time periods!
std::optional<std::chrono::nanoseconds> pico_offset_;
+ ImuFailuresT imu_fault_tracker_;
+ std::optional<size_t> first_valid_data_counter_;
+ size_t total_imu_messages_received_ = 0;
+ size_t data_counter_offset_ = 0;
+ int last_data_counter_ = 0;
+
+ Eigen::Vector3d last_gyro_ = Eigen::Vector3d::Zero();
+
zeroing::UnwrapSensor left_encoder_;
zeroing::UnwrapSensor right_encoder_;
};