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.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index b33c48d..6336386 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -199,6 +199,11 @@
break;
}
+ ready_ = imu_zeroer_.Zeroed();
+
+ // TODO(james): How aggressively can we fault here? If we fault to
+ // aggressively, we might have issues during startup if wpilib_interface takes
+ // too long to start publishing IMU measurements.
if (monotonic_now > last_gyro_time_ + chrono::milliseconds(20)) {
last_gyro_rate_ = 0.0;
}
@@ -463,6 +468,15 @@
status->Send(builder.Finish());
}
+ // If the filters aren't ready/valid, then disable all outputs (currently,
+ // this only happens if the IMU is faulted or has not zeroed).
+ // TODO(james): Add exceptions so that during competitive play the driver
+ // can retain minimal control of the robot.
+ if (!filters_.Ready()) {
+ output_struct.left_voltage = 0.0;
+ output_struct.right_voltage = 0.0;
+ }
+
double left_voltage = 0.0;
double right_voltage = 0.0;
if (output) {