Add ability to handle an upside-down Z IMU
Change-Id: I389eebcefec62c55a11661848167d168cf6b1f9a
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index a4401ea..177c0a5 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -192,6 +192,13 @@
last_gyro_time_ = monotonic_now;
}
break;
+ case GyroType::FLIPPED_IMU_Z_GYRO:
+ if (is_latest_imu_values) {
+ LOG_STRUCT(DEBUG, "using", *imu_values.get());
+ last_gyro_rate_ = -imu_values->gyro_z;
+ last_gyro_time_ = monotonic_now;
+ }
+ break;
case GyroType::SPARTAN_GYRO:
if (gyro_reading.FetchLatest()) {
LOG_STRUCT(DEBUG, "using", *gyro_reading.get());
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.h b/frc971/control_loops/drivetrain/drivetrain_config.h
index 6e546a3..92d15b0 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.h
+++ b/frc971/control_loops/drivetrain/drivetrain_config.h
@@ -27,6 +27,7 @@
IMU_Y_GYRO = 2, // Use the y-axis of the gyro on the IMU.
IMU_Z_GYRO = 3, // Use the z-axis of the gyro on the IMU.
FLIPPED_SPARTAN_GYRO = 4, // Use the gyro on the spartan board.
+ FLIPPED_IMU_Z_GYRO = 5, // Use the flipped z-axis of the gyro on the IMU.
};
enum class IMUType : int32_t {