Add a driver for the new IMU
Redo how zeroing works for the old one too.
This also forced me to update the ahal SPI library to a slightly pared
down version of what WPILib master has.
Change-Id: I631ff230c053c6256325ab6f4e532ca90c901424
diff --git a/frc971/wpilib/fpga_time_conversion.cc b/frc971/wpilib/fpga_time_conversion.cc
new file mode 100644
index 0000000..6f9a267
--- /dev/null
+++ b/frc971/wpilib/fpga_time_conversion.cc
@@ -0,0 +1,36 @@
+#include "frc971/wpilib/fpga_time_conversion.h"
+
+#include "aos/util/compiler_memory_barrier.h"
+
+namespace frc971 {
+namespace wpilib {
+
+std::optional<std::chrono::nanoseconds> CalculateFpgaOffset() {
+ aos_compiler_memory_barrier();
+ const hal::fpga_clock::time_point fpga_time_before = hal::fpga_clock::now();
+ aos_compiler_memory_barrier();
+ const aos::monotonic_clock::time_point monotonic_now =
+ aos::monotonic_clock::now();
+ aos_compiler_memory_barrier();
+ const hal::fpga_clock::time_point fpga_time_after = hal::fpga_clock::now();
+ aos_compiler_memory_barrier();
+
+ const std::chrono::nanoseconds fpga_sample_length =
+ fpga_time_after - fpga_time_before;
+
+ if (fpga_sample_length < fpga_sample_length.zero()) {
+ return std::nullopt;
+ }
+ if (fpga_sample_length > std::chrono::microseconds(20)) {
+ return std::nullopt;
+ }
+
+ const std::chrono::nanoseconds fpga_average =
+ (fpga_time_after.time_since_epoch() +
+ fpga_time_before.time_since_epoch()) /
+ 2;
+ return monotonic_now.time_since_epoch() - fpga_average;
+}
+
+} // namespace wpilib
+} // namespace frc971