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/BUILD b/frc971/control_loops/drivetrain/BUILD
index 9fef8f9..30abc87 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -691,6 +691,7 @@
"improved_down_estimator.h",
],
deps = [
+ ":drivetrain_config",
":drivetrain_status_fbs",
"//aos/events:event_loop",
"//frc971/control_loops:control_loops_fbs",
@@ -707,6 +708,7 @@
"improved_down_estimator_test.cc",
],
deps = [
+ ":drivetrain_test_lib",
"//aos/testing:googletest",
"//aos/testing:random_seed",
"//frc971/control_loops/drivetrain:improved_down_estimator",
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;
}
}
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.h b/frc971/control_loops/drivetrain/drivetrain_config.h
index 5a29008..a33ff22 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.h
+++ b/frc971/control_loops/drivetrain/drivetrain_config.h
@@ -37,6 +37,7 @@
IMU_X = 0, // Use the x-axis of the IMU.
IMU_Y = 1, // Use the y-axis of the IMU.
IMU_FLIPPED_X = 2, // Use the flipped x-axis of the IMU.
+ IMU_Z = 3, // Use the z-axis of the IMU.
};
template <typename Scalar = double>
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
index 336025a..6a90122 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
@@ -190,16 +190,23 @@
auto builder = imu_sender_.MakeBuilder();
frc971::IMUValues::Builder imu_builder =
builder.MakeBuilder<frc971::IMUValues>();
- imu_builder.add_gyro_x(0.0);
- imu_builder.add_gyro_y(0.0);
- imu_builder.add_gyro_z(
- (drivetrain_plant_.X(3, 0) - drivetrain_plant_.X(1, 0)) /
- (dt_config_.robot_radius * 2.0));
+ const Eigen::Vector3d gyro =
+ dt_config_.imu_transform.inverse() *
+ Eigen::Vector3d(0.0, 0.0,
+ (drivetrain_plant_.X(3, 0) - drivetrain_plant_.X(1, 0)) /
+ (dt_config_.robot_radius * 2.0));
+ imu_builder.add_gyro_x(gyro.x());
+ imu_builder.add_gyro_y(gyro.y());
+ imu_builder.add_gyro_z(gyro.z());
// Acceleration due to gravity, in m/s/s.
constexpr double kG = 9.807;
- imu_builder.add_accelerometer_x(last_acceleration_.x() / kG);
- imu_builder.add_accelerometer_y(last_acceleration_.y() / kG);
- imu_builder.add_accelerometer_z(1.0);
+ const Eigen::Vector3d accel =
+ dt_config_.imu_transform.inverse() *
+ Eigen::Vector3d(last_acceleration_.x() / kG, last_acceleration_.y() / kG,
+ 1.0);
+ imu_builder.add_accelerometer_x(accel.x());
+ imu_builder.add_accelerometer_y(accel.y());
+ imu_builder.add_accelerometer_z(accel.z());
imu_builder.add_monotonic_timestamp_ns(
std::chrono::duration_cast<std::chrono::nanoseconds>(
event_loop_->monotonic_now().time_since_epoch())
diff --git a/frc971/control_loops/drivetrain/improved_down_estimator.h b/frc971/control_loops/drivetrain/improved_down_estimator.h
index 27de867..c1c8e1b 100644
--- a/frc971/control_loops/drivetrain/improved_down_estimator.h
+++ b/frc971/control_loops/drivetrain/improved_down_estimator.h
@@ -6,6 +6,7 @@
#include "aos/events/event_loop.h"
#include "aos/time/time.h"
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/runge_kutta.h"
#include "glog/logging.h"
@@ -276,6 +277,8 @@
// accelerometer calibration).
class DrivetrainUkf : public QuaternionUkf {
public:
+ DrivetrainUkf(const DrivetrainConfig<double> &dt_config)
+ : QuaternionUkf(dt_config.imu_transform) {}
// UKF for http://kodlab.seas.upenn.edu/uploads/Arun/UKFpaper.pdf
// Reference in case the link is dead:
// Kraft, Edgar. "A quaternion-based unscented Kalman filter for orientation
diff --git a/frc971/control_loops/drivetrain/improved_down_estimator_test.cc b/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
index 86ef5e9..2edce5a 100644
--- a/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
+++ b/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
@@ -4,6 +4,7 @@
#include <random>
#include "aos/testing/random_seed.h"
+#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
#include "frc971/control_loops/drivetrain/improved_down_estimator.h"
#include "glog/logging.h"
#include "gtest/gtest.h"
@@ -91,7 +92,8 @@
}
TEST(DownEstimatorTest, UkfConstantRotation) {
- drivetrain::DrivetrainUkf dtukf;
+ drivetrain::DrivetrainUkf dtukf(
+ drivetrain::testing::GetTestDrivetrainConfig());
const Eigen::Vector3d ux = Eigen::Vector3d::UnitX();
EXPECT_EQ(0.0,
(Eigen::Vector3d(0.0, 0.0, 1.0) - dtukf.H(dtukf.X_hat().coeffs()))
@@ -112,7 +114,8 @@
// Tests that the euler angles in the status message are correct.
TEST(DownEstimatorTest, UkfEulerStatus) {
- drivetrain::DrivetrainUkf dtukf;
+ drivetrain::DrivetrainUkf dtukf(
+ drivetrain::testing::GetTestDrivetrainConfig());
const Eigen::Vector3d ux = Eigen::Vector3d::UnitX();
const Eigen::Vector3d uy = Eigen::Vector3d::UnitY();
const Eigen::Vector3d uz = Eigen::Vector3d::UnitZ();
@@ -167,7 +170,8 @@
// that we are slightly rotated, that we eventually adjust our estimate to be
// correct.
TEST(DownEstimatorTest, UkfAccelCorrectsBias) {
- drivetrain::DrivetrainUkf dtukf;
+ drivetrain::DrivetrainUkf dtukf(
+ drivetrain::testing::GetTestDrivetrainConfig());
const Eigen::Vector3d ux = Eigen::Vector3d::UnitX();
Eigen::Matrix<double, 3, 1> measurement;
// Supply the accelerometer with a slightly off reading to ensure that we
@@ -192,7 +196,8 @@
// that we are slightly rotated, that we eventually adjust our estimate to be
// correct.
TEST(DownEstimatorTest, UkfIgnoreBadAccel) {
- drivetrain::DrivetrainUkf dtukf;
+ drivetrain::DrivetrainUkf dtukf(
+ drivetrain::testing::GetTestDrivetrainConfig());
const Eigen::Vector3d uy = Eigen::Vector3d::UnitY();
Eigen::Matrix<double, 3, 1> measurement;
// Set up a scenario where, if we naively took the accelerometer readings, we