Make drivetrain_test_lib handle IMU transforms
This also updates the 2020 imu transform to better match the 2020
drivetrain. I haven't actually checked the yaw orientation of the IMU
though.
Change-Id: Ib036705269dfc467a60714c456751a6cfca8a67e
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 77c52c4..1c0ac4f 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -41,6 +41,7 @@
gyro_reading_fetcher_(
event_loop->MakeFetcher<::frc971::sensors::GyroReading>(
"/drivetrain")),
+ down_estimator_(dt_config),
localizer_(localizer),
kf_(dt_config_.make_kf_drivetrain_loop()),
dt_openloop_(dt_config_, &kf_),
@@ -177,6 +178,9 @@
case IMUType::IMU_Y:
last_accel_ = -imu_values_fetcher_->accelerometer_y();
break;
+ case IMUType::IMU_Z:
+ last_accel_ = imu_values_fetcher_->accelerometer_z();
+ break;
}
}