| James Kuszmaul | d3f9eb2 | 2020-01-12 15:02:07 -0800 | [diff] [blame] | 1 | #include "frc971/zeroing/imu_zeroer.h" |
| 2 | |
| 3 | namespace frc971::zeroing { |
| 4 | |
| 5 | ImuZeroer::ImuZeroer() { |
| 6 | gyro_average_.setZero(); |
| 7 | last_gyro_sample_.setZero(); |
| 8 | last_accel_sample_.setZero(); |
| 9 | } |
| 10 | |
| 11 | bool ImuZeroer::Zeroed() const { return zeroed_ || Faulted(); } |
| 12 | |
| 13 | bool ImuZeroer::Faulted() const { return faulted_; } |
| 14 | |
| 15 | Eigen::Vector3d ImuZeroer::ZeroedGyro() const { |
| 16 | return last_gyro_sample_ - gyro_average_; |
| 17 | } |
| 18 | Eigen::Vector3d ImuZeroer::ZeroedAccel() const { return last_accel_sample_; } |
| 19 | Eigen::Vector3d ImuZeroer::GyroOffset() const { return gyro_average_; } |
| 20 | |
| 21 | bool ImuZeroer::GyroZeroReady() const { |
| 22 | return gyro_averager_.full() && gyro_averager_.GetRange() < kGyroMaxVariation; |
| 23 | } |
| 24 | |
| 25 | bool ImuZeroer::AccelZeroReady() const { |
| 26 | return accel_averager_.full() && |
| 27 | accel_averager_.GetRange() < kAccelMaxVariation; |
| 28 | } |
| 29 | |
| 30 | void ImuZeroer::ProcessMeasurement(const IMUValues &values) { |
| 31 | last_gyro_sample_ << values.gyro_x(), values.gyro_y(), values.gyro_z(); |
| 32 | gyro_averager_.AddData(last_gyro_sample_); |
| 33 | last_accel_sample_ << values.accelerometer_x(), values.accelerometer_y(), |
| 34 | values.accelerometer_z(); |
| 35 | accel_averager_.AddData(last_accel_sample_); |
| 36 | if (GyroZeroReady() && AccelZeroReady()) { |
| 37 | if (!zeroed_) { |
| 38 | zeroed_ = true; |
| 39 | gyro_average_ = gyro_averager_.GetAverage(); |
| 40 | } else { |
| 41 | // If we got a new zero and it is substantially different from the |
| 42 | // original zero, fault. |
| 43 | if ((gyro_averager_.GetAverage() - gyro_average_).norm() > |
| 44 | kGyroFaultVariation) { |
| 45 | faulted_ = true; |
| 46 | } |
| 47 | } |
| 48 | gyro_averager_.Reset(); |
| 49 | } |
| 50 | } |
| 51 | |
| 52 | } // namespace frc971::zeroing |