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/sensor_reader.cc b/frc971/wpilib/sensor_reader.cc
index 5bcb84a..0d970e8 100644
--- a/frc971/wpilib/sensor_reader.cc
+++ b/frc971/wpilib/sensor_reader.cc
@@ -9,6 +9,7 @@
#include "frc971/wpilib/ahal/DigitalInput.h"
#include "frc971/wpilib/ahal/DriverStation.h"
#include "frc971/wpilib/ahal/Utility.h"
+#include "frc971/wpilib/fpga_time_conversion.h"
#include "frc971/wpilib/wpilib_interface.h"
#include "hal/PWM.h"
@@ -60,35 +61,20 @@
monotonic_clock::time_point SensorReader::GetPWMStartTime() {
int32_t status = 0;
- const hal::fpga_clock::time_point new_fpga_time = hal::fpga_clock::time_point(
- hal::fpga_clock::duration(HAL_GetPWMCycleStartTime(&status)));
+ const auto new_fpga_time =
+ hal::fpga_clock::duration(HAL_GetPWMCycleStartTime(&status));
- aos_compiler_memory_barrier();
- const hal::fpga_clock::time_point fpga_time_before = hal::fpga_clock::now();
- aos_compiler_memory_barrier();
- const monotonic_clock::time_point monotonic_now = 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 chrono::nanoseconds fpga_sample_length =
- fpga_time_after - fpga_time_before;
- const chrono::nanoseconds fpga_offset =
- hal::fpga_clock::time_point((fpga_time_after.time_since_epoch() +
- fpga_time_before.time_since_epoch()) /
- 2) -
- new_fpga_time;
-
- // Make sure that there wasn't a context switch while we were sampling the
- // clocks. If there was, we are better off rejecting the sample than using
- // it.
- if (ds_->IsSysActive() && fpga_sample_length <= chrono::microseconds(20) &&
- fpga_sample_length >= chrono::microseconds(0)) {
- // Compute when the edge was.
- return monotonic_now - fpga_offset;
- } else {
+ if (!ds_->IsSysActive()) {
return monotonic_clock::min_time;
}
+
+ const auto fpga_offset = CalculateFpgaOffset();
+ // If we failed to sample the offset, just ignore this reading.
+ if (!fpga_offset) {
+ return monotonic_clock::min_time;
+ }
+
+ return monotonic_clock::epoch() + (new_fpga_time + *fpga_offset);
}
void SensorReader::DoStart() {