Disable drivetrain on IMU faults
If we see any faults in the IMU diagnostics, fault the IMU zeroer and
disable any drivetrain outputs so long as the IMU is not properly
zeroed.
This also causes some slight effect to the localizer in one of the
tests, forcing me to bump up a tolerance.
Change-Id: I3bf6909f9bbbd4c5cffc8fce654d2e9c5494dc32
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index f979eb9..9a23edd 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -96,6 +96,8 @@
// Returns the current uncapped voltage from the kalman filter.
double DrivetrainUUncapped(int index) const { return kf_.U_uncapped(index); }
+ bool Ready() const { return ready_; }
+
private:
// Returns the current controller index for the current gear.
int ControllerIndexFromGears() const;
@@ -132,6 +134,8 @@
double last_accel_ = 0.0;
double last_gyro_rate_ = 0.0;
+ bool ready_ = false;
+
// Last applied voltage.
Eigen::Matrix<double, 2, 1> last_voltage_;
Eigen::Matrix<double, 2, 1> last_last_voltage_;