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) {
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_;
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index b378484..b3097ec 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -191,6 +191,38 @@
VerifyDownEstimator();
}
+// Tests that the drivetrain disables itself when the IMU errors.
+TEST_F(DrivetrainTest, DisablesOnImuError) {
+ SetEnabled(true);
+ {
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType::MOTION_PROFILE);
+ goal_builder.add_left_goal(-1.0);
+ goal_builder.add_right_goal(1.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ }
+
+ // Sanity check that the drivetrain is indeed commanding voltage while the IMU
+ // is healthy.
+ for (int i = 0; i < 50; ++i) {
+ RunFor(dt());
+ ASSERT_TRUE(drivetrain_output_fetcher_.Fetch());
+ EXPECT_NE(0.0, drivetrain_output_fetcher_->left_voltage());
+ EXPECT_NE(0.0, drivetrain_output_fetcher_->right_voltage());
+ }
+
+ // Fault the IMU and confirm that we disable the outputs.
+ drivetrain_plant_.set_imu_faulted(true);
+
+ for (int i = 0; i < 500; ++i) {
+ RunFor(dt());
+ ASSERT_TRUE(drivetrain_output_fetcher_.Fetch());
+ EXPECT_EQ(0.0, drivetrain_output_fetcher_->left_voltage());
+ EXPECT_EQ(0.0, drivetrain_output_fetcher_->right_voltage());
+ }
+}
+
// Tests that the drivetrain converges on a goal when under the effect of a
// voltage offset/disturbance.
TEST_F(DrivetrainTest, ConvergesWithVoltageError) {
@@ -960,6 +992,8 @@
const double estimated_y = drivetrain_status_fetcher_->y();
const ::Eigen::Vector2d actual = drivetrain_plant_.GetPosition();
// Expect the x position comparison to fail; everything else to succeed.
+ spline_estimate_tolerance_ = 0.11;
+ spline_control_tolerance_ = 0.11;
EXPECT_GT(std::abs(estimated_x - expected_x), spline_control_tolerance_);
EXPECT_NEAR(estimated_y, expected_y, spline_control_tolerance_);
EXPECT_NEAR(actual(0), estimated_x, spline_estimate_tolerance_);
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
index 4ff71d6..fb685f3 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
@@ -193,8 +193,22 @@
return;
}
auto builder = imu_sender_.MakeBuilder();
+
+ frc971::ADIS16470DiagStat::Builder diag_stat_builder =
+ builder.MakeBuilder<frc971::ADIS16470DiagStat>();
+ diag_stat_builder.add_clock_error(false);
+ diag_stat_builder.add_memory_failure(imu_faulted_);
+ 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();
+
frc971::IMUValues::Builder imu_builder =
builder.MakeBuilder<frc971::IMUValues>();
+ imu_builder.add_self_test_diag_stat(diag_stat_offset);
const Eigen::Vector3d gyro =
dt_config_.imu_transform.inverse() *
Eigen::Vector3d(0.0, 0.0,
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.h b/frc971/control_loops/drivetrain/drivetrain_test_lib.h
index e443ba1..2075e66 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.h
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.h
@@ -79,6 +79,10 @@
send_messages_ = send_messages;
}
+ void set_imu_faulted(const bool fault_imu) {
+ imu_faulted_ = fault_imu;
+ }
+
private:
// Sends out the position queue messages.
void SendPositionMessage();
@@ -103,6 +107,8 @@
drivetrain_status_fetcher_;
::aos::Sender<::frc971::IMUValuesBatch> imu_sender_;
+ bool imu_faulted_ = false;
+
DrivetrainConfig<double> dt_config_;
DrivetrainPlant drivetrain_plant_;
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