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/y2022/localizer/BUILD b/y2022/localizer/BUILD
index ec717fc..2a0a9fe 100644
--- a/y2022/localizer/BUILD
+++ b/y2022/localizer/BUILD
@@ -168,35 +168,15 @@
],
)
-cc_library(
- name = "imu",
- srcs = [
- "imu.cc",
- ],
- hdrs = [
- "imu.h",
- ],
- target_compatible_with = ["@platforms//os:linux"],
- deps = [
- "//aos/events:epoll",
- "//aos/events:shm_event_loop",
- "//aos/util:crc32",
- "//frc971/wpilib:imu_batch_fbs",
- "//frc971/wpilib:imu_fbs",
- "//y2022:constants",
- "@com_github_google_glog//:glog",
- "@com_google_absl//absl/types:span",
- ],
-)
-
cc_binary(
name = "imu_main",
srcs = ["imu_main.cc"],
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
deps = [
- ":imu",
"//aos:init",
"//aos/events:shm_event_loop",
+ "//frc971/imu_reader:imu",
+ "//y2022:constants",
],
)
diff --git a/y2022/localizer/imu.cc b/y2022/localizer/imu.cc
deleted file mode 100644
index 7b5022f..0000000
--- a/y2022/localizer/imu.cc
+++ /dev/null
@@ -1,167 +0,0 @@
-#include "y2022/localizer/imu.h"
-
-#include "aos/util/crc32.h"
-#include "glog/logging.h"
-#include "y2022/constants.h"
-
-namespace y2022::localizer {
-
-namespace {
-
-constexpr size_t kReadSize = 50;
-constexpr double kGyroScale = 1 / 655360.0 / 360.0 * (2 * M_PI);
-constexpr double kAccelScale = 1 / 26756268.0 / 9.80665;
-constexpr double kTempScale = 0.1;
-
-} // namespace
-
-Imu::Imu(aos::ShmEventLoop *event_loop)
- : event_loop_(event_loop),
- imu_sender_(
- event_loop_->MakeSender<frc971::IMUValuesBatch>("/localizer")) {
- imu_fd_ = open("/dev/adis16505", O_RDONLY | O_NONBLOCK);
- PCHECK(imu_fd_ != -1) << ": Failed to open SPI device for IMU.";
- aos::internal::EPoll *epoll = event_loop_->epoll();
- epoll->OnReadable(imu_fd_, [this]() {
- uint8_t buf[kReadSize];
- ssize_t read_len = read(imu_fd_, buf, kReadSize);
- // TODO: Do we care about gracefully handling EAGAIN or anything else?
- // This should only get called when there is data.
- PCHECK(read_len != -1);
- CHECK_EQ(read_len, static_cast<ssize_t>(kReadSize))
- << ": Read incorrect number of bytes.";
-
- auto sender = imu_sender_.MakeBuilder();
-
- const flatbuffers::Offset<frc971::IMUValues> values_offset =
- ProcessReading(sender.fbb(), absl::Span(buf, kReadSize));
- const flatbuffers::Offset<
- flatbuffers::Vector<flatbuffers::Offset<frc971::IMUValues>>>
- readings_offset = sender.fbb()->CreateVector(&values_offset, 1);
- frc971::IMUValuesBatch::Builder batch_builder =
- sender.MakeBuilder<frc971::IMUValuesBatch>();
- batch_builder.add_readings(readings_offset);
- imu_sender_.CheckOk(sender.Send(batch_builder.Finish()));
- });
-}
-
-flatbuffers::Offset<frc971::IMUValues> Imu::ProcessReading(
- flatbuffers::FlatBufferBuilder *fbb, const absl::Span<uint8_t> message) {
- absl::Span<const uint8_t> buf = message;
-
- uint64_t driver_timestamp;
- memcpy(&driver_timestamp, buf.data(), sizeof(driver_timestamp));
- buf = buf.subspan(8);
-
- uint16_t diag_stat;
- memcpy(&diag_stat, buf.data(), sizeof(diag_stat));
- buf = buf.subspan(2);
-
- double x_gyro = ConvertValue32(buf, kGyroScale);
- buf = buf.subspan(4);
- double y_gyro = ConvertValue32(buf, kGyroScale);
- buf = buf.subspan(4);
- double z_gyro = ConvertValue32(buf, kGyroScale);
- buf = buf.subspan(4);
- double x_accel = ConvertValue32(buf, kAccelScale);
- buf = buf.subspan(4);
- double y_accel = ConvertValue32(buf, kAccelScale);
- buf = buf.subspan(4);
- double z_accel = ConvertValue32(buf, kAccelScale);
- buf = buf.subspan(4);
- double temp = ConvertValue16(buf, kTempScale);
- buf = buf.subspan(2);
- uint16_t data_counter;
- memcpy(&data_counter, buf.data(), sizeof(data_counter));
- buf = buf.subspan(2);
- uint32_t pico_timestamp;
- memcpy(&pico_timestamp, buf.data(), sizeof(pico_timestamp));
- buf = buf.subspan(4);
- int16_t encoder1_count;
- memcpy(&encoder1_count, buf.data(), sizeof(encoder1_count));
- buf = buf.subspan(2);
- int16_t encoder2_count;
- memcpy(&encoder2_count, buf.data(), sizeof(encoder2_count));
- buf = buf.subspan(2);
- uint32_t checksum;
- memcpy(&checksum, buf.data(), sizeof(checksum));
- buf = buf.subspan(4);
-
- CHECK(buf.empty()) << "Have leftover bytes: " << buf.size();
-
- u_int32_t calculated_checksum = aos::ComputeCrc32(message.subspan(8, 38));
-
- if (checksum != calculated_checksum) {
- this->failed_checksums_++;
- }
-
- const auto diag_stat_offset = PackDiagStat(fbb, diag_stat);
-
- frc971::IMUValues::Builder imu_builder(*fbb);
-
- if (checksum == calculated_checksum) {
- constexpr uint16_t kChecksumMismatch = 1 << 0;
- bool imu_checksum_matched = !(diag_stat & kChecksumMismatch);
-
- // data from the IMU packet
- if (imu_checksum_matched) {
- imu_builder.add_gyro_x(x_gyro);
- imu_builder.add_gyro_y(y_gyro);
- imu_builder.add_gyro_z(z_gyro);
-
- imu_builder.add_accelerometer_x(x_accel);
- imu_builder.add_accelerometer_y(y_accel);
- imu_builder.add_accelerometer_z(z_accel);
-
- imu_builder.add_temperature(temp);
-
- imu_builder.add_data_counter(data_counter);
- }
-
- // extra data from the pico
- imu_builder.add_pico_timestamp_us(pico_timestamp);
- imu_builder.add_left_encoder(
- -constants::Values::DrivetrainEncoderToMeters(encoder2_count));
- imu_builder.add_right_encoder(
- constants::Values::DrivetrainEncoderToMeters(encoder1_count));
- imu_builder.add_previous_reading_diag_stat(diag_stat_offset);
- }
-
- // extra data from us
- imu_builder.add_monotonic_timestamp_ns(driver_timestamp);
- imu_builder.add_failed_checksums(failed_checksums_);
- imu_builder.add_checksum_failed(checksum != calculated_checksum);
-
- return imu_builder.Finish();
-}
-
-flatbuffers::Offset<frc971::ADIS16470DiagStat> Imu::PackDiagStat(
- flatbuffers::FlatBufferBuilder *fbb, uint16_t value) {
- frc971::ADIS16470DiagStat::Builder diag_stat_builder(*fbb);
- diag_stat_builder.add_clock_error(value & (1 << 7));
- diag_stat_builder.add_memory_failure(value & (1 << 6));
- diag_stat_builder.add_sensor_failure(value & (1 << 5));
- diag_stat_builder.add_standby_mode(value & (1 << 4));
- diag_stat_builder.add_spi_communication_error(value & (1 << 3));
- diag_stat_builder.add_flash_memory_update_error(value & (1 << 2));
- diag_stat_builder.add_data_path_overrun(value & (1 << 1));
- diag_stat_builder.add_checksum_mismatch(value & (1 << 0));
- return diag_stat_builder.Finish();
-}
-
-double Imu::ConvertValue32(absl::Span<const uint8_t> data,
- double lsb_per_output) {
- int32_t value;
- memcpy(&value, data.data(), sizeof(value));
- return static_cast<double>(value) * lsb_per_output;
-}
-
-double Imu::ConvertValue16(absl::Span<const uint8_t> data,
- double lsb_per_output) {
- int16_t value;
- memcpy(&value, data.data(), sizeof(value));
- return static_cast<double>(value) * lsb_per_output;
-}
-
-Imu::~Imu() { PCHECK(0 == close(imu_fd_)); }
-} // namespace y2022::localizer
diff --git a/y2022/localizer/imu.h b/y2022/localizer/imu.h
deleted file mode 100644
index cd45710..0000000
--- a/y2022/localizer/imu.h
+++ /dev/null
@@ -1,31 +0,0 @@
-#ifndef Y2022_LOCALIZER_IMU_H_
-#define Y2022_LOCALIZER_IMU_H_
-#include "aos/events/shm_event_loop.h"
-#include "frc971/wpilib/imu_batch_generated.h"
-#include "y2022/constants.h"
-
-namespace y2022::localizer {
-
-// Reads IMU packets from the kernel driver which reads them over spi
-// from the Raspberry Pi Pico on the IMU board.
-class Imu {
- public:
- Imu(aos::ShmEventLoop *event_loop);
- ~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;
-};
-} // namespace y2022::localizer
-#endif // Y2022_LOCALIZER_IMU_H_
diff --git a/y2022/localizer/imu_main.cc b/y2022/localizer/imu_main.cc
index 53811ea..03dfc58 100644
--- a/y2022/localizer/imu_main.cc
+++ b/y2022/localizer/imu_main.cc
@@ -1,7 +1,8 @@
#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
#include "aos/realtime.h"
-#include "y2022/localizer/imu.h"
+#include "frc971/imu_reader/imu.h"
+#include "y2022/constants.h"
DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
@@ -15,7 +16,8 @@
<< ": Failed to set read permissions on IMU device.";
aos::ShmEventLoop event_loop(&config.message());
- y2022::localizer::Imu imu(&event_loop);
+ frc971::imu::Imu imu(&event_loop,
+ y2022::constants::Values::DrivetrainEncoderToMeters(1));
event_loop.SetRuntimeRealtimePriority(30);