Separate out CPU-heavy portion of IMU zeroing
This lets us run the hard stuff only once per drivetrain iteration,
rather than 10 times per drivetrain iteration.
Change-Id: I262f41cafaf3b34fbd7f5205896dfdcde808db77
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index a19f57b..77c52c4 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -148,7 +148,7 @@
}
while (imu_values_fetcher_.FetchNext()) {
- imu_zeroer_.ProcessMeasurement(*imu_values_fetcher_);
+ imu_zeroer_.InsertMeasurement(*imu_values_fetcher_);
last_gyro_time_ = monotonic_now;
if (!imu_zeroer_.Zeroed()) {
continue;
@@ -165,6 +165,7 @@
bool got_imu_reading = false;
if (imu_values_fetcher_.get() != nullptr) {
+ imu_zeroer_.ProcessMeasurements();
got_imu_reading = true;
switch (dt_config_.imu_type) {
case IMUType::IMU_X:
diff --git a/frc971/zeroing/imu_zeroer.cc b/frc971/zeroing/imu_zeroer.cc
index b13b43d..9e82d13 100644
--- a/frc971/zeroing/imu_zeroer.cc
+++ b/frc971/zeroing/imu_zeroer.cc
@@ -35,15 +35,23 @@
accel_averager_.GetRange() < kAccelMaxVariation;
}
-void ImuZeroer::ProcessMeasurement(const IMUValues &values) {
+void ImuZeroer::InsertAndProcessMeasurement(const IMUValues &values) {
+ InsertMeasurement(values);
+ ProcessMeasurements();
+}
+
+void ImuZeroer::InsertMeasurement(const IMUValues &values) {
last_gyro_sample_ << values.gyro_x(), values.gyro_y(), values.gyro_z();
gyro_averager_.AddData(last_gyro_sample_);
last_accel_sample_ << values.accelerometer_x(), values.accelerometer_y(),
values.accelerometer_z();
accel_averager_.AddData(last_accel_sample_);
+}
+
+void ImuZeroer::ProcessMeasurements() {
if (GyroZeroReady() && AccelZeroReady()) {
++good_iters_;
- if (good_iters_ > kSamplesToAverage / 4) {
+ if (good_iters_ > kSamplesToAverage / 40) {
const Eigen::Vector3d current_gyro_average = gyro_averager_.GetAverage();
constexpr double kAverageUpdateWeight = 0.05;
if (num_zeroes_ > 0) {
diff --git a/frc971/zeroing/imu_zeroer.h b/frc971/zeroing/imu_zeroer.h
index b55c6dd..fd82571 100644
--- a/frc971/zeroing/imu_zeroer.h
+++ b/frc971/zeroing/imu_zeroer.h
@@ -22,7 +22,10 @@
ImuZeroer();
bool Zeroed() const;
bool Faulted() const;
- void ProcessMeasurement(const IMUValues &values);
+ void InsertMeasurement(const IMUValues &values);
+ // PErforms the heavier-duty processing for managing zeroing.
+ void ProcessMeasurements();
+ void InsertAndProcessMeasurement(const IMUValues &values);
Eigen::Vector3d GyroOffset() const;
Eigen::Vector3d ZeroedGyro() const;
Eigen::Vector3d ZeroedAccel() const;
diff --git a/frc971/zeroing/imu_zeroer_test.cc b/frc971/zeroing/imu_zeroer_test.cc
index eee3f57..e97fbe2 100644
--- a/frc971/zeroing/imu_zeroer_test.cc
+++ b/frc971/zeroing/imu_zeroer_test.cc
@@ -32,7 +32,7 @@
ASSERT_EQ(0.0, zeroer.ZeroedAccel().norm());
// A measurement before we are zeroed should just result in the measurement
// being passed through without modification.
- zeroer.ProcessMeasurement(
+ zeroer.InsertAndProcessMeasurement(
MakeMeasurement({0.01, 0.02, 0.03}, {4, 5, 6}).message());
ASSERT_FALSE(zeroer.Zeroed());
ASSERT_FALSE(zeroer.Faulted());
@@ -50,7 +50,7 @@
ImuZeroer zeroer;
ASSERT_FALSE(zeroer.Zeroed());
for (size_t ii = 0; ii < kMinSamplesToZero; ++ii) {
- zeroer.ProcessMeasurement(
+ zeroer.InsertAndProcessMeasurement(
MakeMeasurement({0.01, 0.02, 0.03}, {4, 5, 6}).message());
}
ASSERT_TRUE(zeroer.Zeroed());
@@ -68,7 +68,7 @@
ASSERT_EQ(6.0, zeroer.ZeroedAccel().z());
// If we get another measurement offset by {1, 1, 1} we should read the result
// as {1, 1, 1}.
- zeroer.ProcessMeasurement(
+ zeroer.InsertAndProcessMeasurement(
MakeMeasurement({0.02, 0.03, 0.04}, {0, 0, 0}).message());
ASSERT_FALSE(zeroer.Faulted());
ASSERT_FLOAT_EQ(0.01, zeroer.ZeroedGyro().x());
@@ -82,7 +82,7 @@
ImuZeroer zeroer;
ASSERT_FALSE(zeroer.Zeroed());
for (size_t ii = 0; ii < kMinSamplesToZero; ++ii) {
- zeroer.ProcessMeasurement(
+ zeroer.InsertAndProcessMeasurement(
MakeMeasurement({0.1, 0.2, 0.3}, {4, 5, 6}).message());
ASSERT_FALSE(zeroer.Zeroed());
}
@@ -97,10 +97,9 @@
for (size_t ii = 0; ii < kMinSamplesToZero; ++ii) {
const double offset =
(static_cast<double>(ii) / (kMinSamplesToZero - 1) - 0.5) * 0.001;
- zeroer.ProcessMeasurement(
+ zeroer.InsertAndProcessMeasurement(
MakeMeasurement({0.01 + offset, 0.02 + offset, 0.03 + offset},
- {4 + offset, 5 + offset, 6 + offset})
- .message());
+ {4 + offset, 5 + offset, 6 + offset}).message());
}
ASSERT_TRUE(zeroer.Zeroed());
ASSERT_FALSE(zeroer.Faulted());
@@ -109,7 +108,7 @@
ASSERT_NEAR(0.03, zeroer.GyroOffset().z(), 1e-3);
// If we get another measurement offset by {0.01, 0.01, 0.01} we should read
// the result as {0.01, 0.01, 0.01}.
- zeroer.ProcessMeasurement(
+ zeroer.InsertAndProcessMeasurement(
MakeMeasurement({0.02, 0.03, 0.04}, {0, 0, 0}).message());
ASSERT_FALSE(zeroer.Faulted());
ASSERT_NEAR(0.01, zeroer.ZeroedGyro().x(), 1e-3);
@@ -128,10 +127,9 @@
ASSERT_FALSE(zeroer.Zeroed());
const double offset =
(static_cast<double>(ii) / (kMinSamplesToZero - 1) - 0.5) * 1.0;
- zeroer.ProcessMeasurement(
+ zeroer.InsertAndProcessMeasurement(
MakeMeasurement({0.01 + offset, 0.02 + offset, 0.03 + offset},
- {4 + offset, 5 + offset, 6 + offset})
- .message());
+ {4 + offset, 5 + offset, 6 + offset}).message());
}
ASSERT_FALSE(zeroer.Zeroed());
ASSERT_FALSE(zeroer.Faulted());
@@ -143,14 +141,14 @@
ImuZeroer zeroer;
ASSERT_FALSE(zeroer.Zeroed());
for (size_t ii = 0; ii < kMinSamplesToZero; ++ii) {
- zeroer.ProcessMeasurement(
+ zeroer.InsertAndProcessMeasurement(
MakeMeasurement({0.01, 0.02, 0.03}, {4, 5, 6}).message());
}
ASSERT_TRUE(zeroer.Zeroed());
ASSERT_FALSE(zeroer.Faulted())
<< "We should not fault until we complete a second cycle of zeroing.";
for (size_t ii = 0; ii < kMinSamplesToZero; ++ii) {
- zeroer.ProcessMeasurement(
+ zeroer.InsertAndProcessMeasurement(
MakeMeasurement({0.01, 0.05, 0.03}, {4, 5, 6}).message());
}
ASSERT_TRUE(zeroer.Faulted());
@@ -161,12 +159,12 @@
ImuZeroer zeroer;
ASSERT_FALSE(zeroer.Zeroed());
for (size_t ii = 0; ii < kMinSamplesToZero; ++ii) {
- zeroer.ProcessMeasurement(
+ zeroer.InsertAndProcessMeasurement(
MakeMeasurement({0.01, 0.02, 0.03}, {4, 5, 6}).message());
}
ASSERT_TRUE(zeroer.Zeroed());
for (size_t ii = 0; ii < kMinSamplesToZero; ++ii) {
- zeroer.ProcessMeasurement(
+ zeroer.InsertAndProcessMeasurement(
MakeMeasurement({0.01, 0.020001, 0.03}, {4, 5, 6}).message());
}
ASSERT_FALSE(zeroer.Faulted());