commit 0a98140205ec30048471eb30ce138e78577c7c3b
author James Kuszmaul <jabukuszmaul@gmail.com> Sat Oct 09 21:00:34 2021 -0700
committer James Kuszmaul <jabukuszmaul+collab@gmail.com> Fri Oct 15 20:36:48 2021 -0700
tree 9ffc95674ee8f4f521229937ae23edfc15acd68f
parent 37d2c3043cf4ff743c1ba959ebdb4913f3ad9efc

Use ADIS16448 in y2020

This updates the IMU transform to be correct for the yaw axis, but may
have the X/Y axes/signs wrong if we care about accelerometer or
roll/pitch data.

Change-Id: I73d86c8be18f447bb35fc13666ccfdda29fef257
Signed-off-by: James Kuszmaul <jabukuszmaul@gmail.com>

frc971/control_loops/drivetrain/drivetrain.cc

diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 2cc9f35..928d418 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -142,21 +142,23 @@
     imu_zeroer_.ProcessMeasurements();
     got_imu_reading = true;
     CHECK(imu_values_fetcher_->has_readings());
-    const IMUValues *value = imu_values_fetcher_->readings()->Get(
-        imu_values_fetcher_->readings()->size() - 1);
-    switch (dt_config_.imu_type) {
-      case IMUType::IMU_X:
-        last_accel_ = -value->accelerometer_x();
-        break;
-      case IMUType::IMU_FLIPPED_X:
-        last_accel_ = value->accelerometer_x();
-        break;
-      case IMUType::IMU_Y:
-        last_accel_ = -value->accelerometer_y();
-        break;
-      case IMUType::IMU_Z:
-        last_accel_ = value->accelerometer_z();
-        break;
+    if (imu_values_fetcher_->readings()->size() > 0) {
+      const IMUValues *value = imu_values_fetcher_->readings()->Get(
+          imu_values_fetcher_->readings()->size() - 1);
+      switch (dt_config_.imu_type) {
+        case IMUType::IMU_X:
+          last_accel_ = -value->accelerometer_x();
+          break;
+        case IMUType::IMU_FLIPPED_X:
+          last_accel_ = value->accelerometer_x();
+          break;
+        case IMUType::IMU_Y:
+          last_accel_ = -value->accelerometer_y();
+          break;
+        case IMUType::IMU_Z:
+          last_accel_ = value->accelerometer_z();
+          break;
+      }
     }
   }