Refactor common IMU code into frc971
This lets us have different main functions for different years to manage
the hardware complexity.
Change-Id: Ia0fbef081265b6301e1d9e9ecc4f1d496f43a9cc
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/frc971/imu_reader/imu.h b/frc971/imu_reader/imu.h
new file mode 100644
index 0000000..e3285d0
--- /dev/null
+++ b/frc971/imu_reader/imu.h
@@ -0,0 +1,36 @@
+#ifndef FRC971_IMU_READER_IMU_H_
+#define FRC971_IMU_READER_IMU_H_
+
+#include "aos/events/shm_event_loop.h"
+#include "frc971/wpilib/imu_batch_generated.h"
+
+namespace frc971::imu {
+
+// Reads IMU packets from the kernel driver which reads them over spi
+// from the Raspberry Pi Pico on the IMU board.
+class Imu {
+ public:
+ // Constructs an IMU reader object. encoder_scalar is in meters/count.
+ Imu(aos::ShmEventLoop *event_loop, double encoder_scalar);
+ ~Imu();
+
+ private:
+ flatbuffers::Offset<frc971::ADIS16470DiagStat> PackDiagStat(
+ flatbuffers::FlatBufferBuilder *fbb, uint16_t value);
+ flatbuffers::Offset<frc971::IMUValues> ProcessReading(
+ flatbuffers::FlatBufferBuilder *fbb, absl::Span<uint8_t> buf);
+ double ConvertValue32(absl::Span<const uint8_t> data, double lsb_per_output);
+ double ConvertValue16(absl::Span<const uint8_t> data, double lsb_per_output);
+
+ aos::ShmEventLoop *event_loop_;
+ aos::Sender<frc971::IMUValuesBatch> imu_sender_;
+ int imu_fd_;
+
+ uint failed_checksums_ = 0;
+
+ double encoder_scalar_;
+};
+
+} // namespace frc971::imu
+
+#endif // FRC971_IMU_READER_IMU_H_