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/zeroing/imu_zeroer.cc b/frc971/zeroing/imu_zeroer.cc
index 9e82d13..70c5a42 100644
--- a/frc971/zeroing/imu_zeroer.cc
+++ b/frc971/zeroing/imu_zeroer.cc
@@ -2,6 +2,25 @@
namespace frc971::zeroing {
+namespace {
+bool DiagStatHasFaults(const ADIS16470DiagStat &diag) {
+ return diag.clock_error() || diag.memory_failure() || diag.sensor_failure() ||
+ diag.standby_mode() || diag.spi_communication_error() ||
+ diag.flash_memory_update_error() || diag.data_path_overrun();
+}
+bool ReadingHasFaults(const IMUValues &values) {
+ if (values.has_previous_reading_diag_stat() &&
+ DiagStatHasFaults(*values.previous_reading_diag_stat())) {
+ return true;
+ }
+ if (values.has_self_test_diag_stat() &&
+ DiagStatHasFaults(*values.self_test_diag_stat())) {
+ return true;
+ }
+ return false;
+}
+} // namespace
+
ImuZeroer::ImuZeroer() {
gyro_average_.setZero();
accel_average_.setZero();
@@ -10,7 +29,7 @@
}
bool ImuZeroer::Zeroed() const {
- return num_zeroes_ > kRequiredZeroPoints || Faulted();
+ return num_zeroes_ > kRequiredZeroPoints && !Faulted();
}
bool ImuZeroer::Faulted() const { return faulted_; }
@@ -41,6 +60,10 @@
}
void ImuZeroer::InsertMeasurement(const IMUValues &values) {
+ if (ReadingHasFaults(values)) {
+ faulted_ = true;
+ return;
+ }
last_gyro_sample_ << values.gyro_x(), values.gyro_y(), values.gyro_z();
gyro_averager_.AddData(last_gyro_sample_);
last_accel_sample_ << values.accelerometer_x(), values.accelerometer_y(),
diff --git a/frc971/zeroing/imu_zeroer_test.cc b/frc971/zeroing/imu_zeroer_test.cc
index e97fbe2..b984b1a 100644
--- a/frc971/zeroing/imu_zeroer_test.cc
+++ b/frc971/zeroing/imu_zeroer_test.cc
@@ -8,9 +8,22 @@
2 * ImuZeroer::kSamplesToAverage * ImuZeroer::kRequiredZeroPoints;
aos::FlatbufferDetachedBuffer<IMUValues> MakeMeasurement(
- const Eigen::Vector3d &gyro, const Eigen::Vector3d &accel) {
+ const Eigen::Vector3d &gyro, const Eigen::Vector3d &accel,
+ bool faulted = false) {
flatbuffers::FlatBufferBuilder fbb;
fbb.ForceDefaults(true);
+
+ ADIS16470DiagStatBuilder diag_stat_builder(fbb);
+ diag_stat_builder.add_clock_error(faulted);
+ diag_stat_builder.add_memory_failure(false);
+ diag_stat_builder.add_sensor_failure(false);
+ diag_stat_builder.add_standby_mode(false);
+ diag_stat_builder.add_spi_communication_error(false);
+ diag_stat_builder.add_flash_memory_update_error(false);
+ diag_stat_builder.add_data_path_overrun(false);
+
+ const auto diag_stat_offset = diag_stat_builder.Finish();
+
IMUValuesBuilder builder(fbb);
builder.add_gyro_x(gyro.x());
builder.add_gyro_y(gyro.y());
@@ -18,6 +31,7 @@
builder.add_accelerometer_x(accel.x());
builder.add_accelerometer_y(accel.y());
builder.add_accelerometer_z(accel.z());
+ builder.add_previous_reading_diag_stat(diag_stat_offset);
fbb.Finish(builder.Finish());
return fbb.Release();
}
@@ -152,6 +166,7 @@
MakeMeasurement({0.01, 0.05, 0.03}, {4, 5, 6}).message());
}
ASSERT_TRUE(zeroer.Faulted());
+ ASSERT_FALSE(zeroer.Zeroed());
}
// Tests that we do not fault if the zero only changes by a small amount.
@@ -170,4 +185,15 @@
ASSERT_FALSE(zeroer.Faulted());
}
+// Tests that we fault on a bad diagnostic.
+TEST(ImuZeroerTest, FaultOnBadDiagnostic) {
+ ImuZeroer zeroer;
+ ASSERT_FALSE(zeroer.Zeroed());
+ ASSERT_FALSE(zeroer.Faulted());
+ zeroer.InsertAndProcessMeasurement(
+ MakeMeasurement({0.01, 0.02, 0.03}, {4, 5, 6}, true).message());
+ ASSERT_FALSE(zeroer.Zeroed());
+ ASSERT_TRUE(zeroer.Faulted());
+}
+
} // namespace frc971::zeroing