Improve gyro zeroing
Instead of using a single large data chunk for zeroing, collect multiple
shorter samples that we then gradually accumulate.
Change-Id: I5496a7c47a703764fcd0f5b1498ef080e9b30fe9
diff --git a/frc971/zeroing/imu_zeroer.h b/frc971/zeroing/imu_zeroer.h
index 2662934..b55c6dd 100644
--- a/frc971/zeroing/imu_zeroer.h
+++ b/frc971/zeroing/imu_zeroer.h
@@ -1,6 +1,7 @@
#ifndef FRC971_ZEROING_IMU_ZEROER_H_
#define FRC971_ZEROING_IMU_ZEROER_H_
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/wpilib/imu_generated.h"
#include "frc971/zeroing/averager.h"
@@ -15,7 +16,8 @@
// constant number of samples...
// TODO(james): Run average and GetRange calculations over every sample on
// every timestep, to provide consistent timing.
- static constexpr size_t kSamplesToAverage = 1000.0;
+ static constexpr size_t kSamplesToAverage = 1000;
+ static constexpr size_t kRequiredZeroPoints = 10;
ImuZeroer();
bool Zeroed() const;
@@ -24,14 +26,22 @@
Eigen::Vector3d GyroOffset() const;
Eigen::Vector3d ZeroedGyro() const;
Eigen::Vector3d ZeroedAccel() const;
+
+ flatbuffers::Offset<control_loops::drivetrain::ImuZeroerState> PopulateStatus(
+ flatbuffers::FlatBufferBuilder *fbb) const;
+
private:
// Max variation (difference between the maximum and minimum value) in a
// kSamplesToAverage range before we allow using the samples for zeroing.
// These values are currently based on looking at results from the ADIS16448.
- static constexpr double kGyroMaxVariation = 0.02; // rad / sec
+ static constexpr double kGyroMaxVariation = 0.02; // rad / sec
+ // Maximum magnitude we allow the gyro zero to have--this is used to prevent
+ // us from zeroing the gyro if we just happen to be spinning at a very
+ // consistent non-zero rate. Currently this is only plausible in simulation.
+ static constexpr double kGyroMaxZeroingMagnitude = 0.1; // rad / sec
// Max variation in the range before we consider the accelerometer readings to
// be steady.
- static constexpr double kAccelMaxVariation = 0.02; // g's
+ static constexpr double kAccelMaxVariation = 0.05; // g's
// If we ever are able to rezero and get a zero that is more than
// kGyroFaultVariation away from the original zeroing, fault.
static constexpr double kGyroFaultVariation = 0.005; // rad / sec
@@ -46,12 +56,13 @@
Averager<double, kSamplesToAverage, 3> accel_averager_;
// The average zero position of the gyro.
Eigen::Vector3d gyro_average_;
+ Eigen::Vector3d accel_average_;
Eigen::Vector3d last_gyro_sample_;
Eigen::Vector3d last_accel_sample_;
- // Whether we have currently zeroed yet.
- bool zeroed_ = false;
// Whether the zeroing has faulted at any point thus far.
bool faulted_ = false;
+ size_t good_iters_ = 0;
+ size_t num_zeroes_ = 0;
};
} // namespace frc971::zeroing