Reduce size of IMU zeroing buffer
The 10000 sample buffer was causing some issues. Lower the size of the
buffer until I can determine how I really want to manage it.
Change-Id: I9e4a84a8448ed4aeb7e7cc39622970a9aeede674
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index a8014d5..094a947 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -53,6 +53,11 @@
right_high_requested_(dt_config_.default_high_gear) {
::aos::controls::HPolytope<0>::Init();
event_loop->SetRuntimeRealtimePriority(30);
+ event_loop->OnRun([this]() {
+ // On the first fetch, make sure that we are caught all the way up to the
+ // present.
+ imu_values_fetcher_.Fetch();
+ });
}
int DrivetrainLoop::ControllerIndexFromGears() {
diff --git a/frc971/zeroing/imu_zeroer.h b/frc971/zeroing/imu_zeroer.h
index 5f53e0d..2662934 100644
--- a/frc971/zeroing/imu_zeroer.h
+++ b/frc971/zeroing/imu_zeroer.h
@@ -10,10 +10,12 @@
// able to do so.
class ImuZeroer {
public:
- // Average 5 seconds of data (assuming 2kHz sampling rate).
+ // Average 0.5 seconds of data (assuming 2kHz sampling rate).
// TODO(james): Make the gyro zero in a constant amount of time, rather than a
// constant number of samples...
- static constexpr size_t kSamplesToAverage = 10000.0;
+ // TODO(james): Run average and GetRange calculations over every sample on
+ // every timestep, to provide consistent timing.
+ static constexpr size_t kSamplesToAverage = 1000.0;
ImuZeroer();
bool Zeroed() const;
diff --git a/frc971/zeroing/imu_zeroer_test.cc b/frc971/zeroing/imu_zeroer_test.cc
index 9919e2f..9ec52da 100644
--- a/frc971/zeroing/imu_zeroer_test.cc
+++ b/frc971/zeroing/imu_zeroer_test.cc
@@ -89,16 +89,16 @@
ASSERT_TRUE(zeroer.Zeroed());
ASSERT_FALSE(zeroer.Faulted());
// Gyro should be zeroed to {1, 2, 3}.
- ASSERT_NEAR(1.0, zeroer.GyroOffset().x(), 1e-10);
- ASSERT_NEAR(2.0, zeroer.GyroOffset().y(), 1e-10);
- ASSERT_NEAR(3.0, zeroer.GyroOffset().z(), 1e-10);
+ ASSERT_NEAR(1.0, zeroer.GyroOffset().x(), 1e-8);
+ ASSERT_NEAR(2.0, zeroer.GyroOffset().y(), 1e-8);
+ ASSERT_NEAR(3.0, zeroer.GyroOffset().z(), 1e-8);
// If we get another measurement offset by {1, 1, 1} we should read the result
// as {1, 1, 1}.
zeroer.ProcessMeasurement(MakeMeasurement({2, 3, 4}, {0, 0, 0}).message());
ASSERT_FALSE(zeroer.Faulted());
- ASSERT_NEAR(1.0, zeroer.ZeroedGyro().x(), 1e-10);
- ASSERT_NEAR(1.0, zeroer.ZeroedGyro().y(), 1e-10);
- ASSERT_NEAR(1.0, zeroer.ZeroedGyro().z(), 1e-10);
+ ASSERT_NEAR(1.0, zeroer.ZeroedGyro().x(), 1e-8);
+ ASSERT_NEAR(1.0, zeroer.ZeroedGyro().y(), 1e-8);
+ ASSERT_NEAR(1.0, zeroer.ZeroedGyro().z(), 1e-8);
ASSERT_EQ(0.0, zeroer.ZeroedAccel().x());
ASSERT_EQ(0.0, zeroer.ZeroedAccel().y());
ASSERT_EQ(0.0, zeroer.ZeroedAccel().z());