Don't latch IMU faults in 2022 localizer
We get occasional checksum failures. Don't brick the localizer because
of that.
Change-Id: I67fa55264ac56127b7a8f150db35bf7747af7468
Signed-off-by: James Kuszmaul <jabukuszmaul@gmail.com>
diff --git a/frc971/zeroing/imu_zeroer.cc b/frc971/zeroing/imu_zeroer.cc
index a9b1045..6bc2d0e 100644
--- a/frc971/zeroing/imu_zeroer.cc
+++ b/frc971/zeroing/imu_zeroer.cc
@@ -6,7 +6,8 @@
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();
+ diag.flash_memory_update_error() || diag.data_path_overrun() ||
+ diag.checksum_mismatch();
}
bool ReadingHasFaults(const IMUValues &values) {
if (values.has_previous_reading_diag_stat() &&
@@ -17,11 +18,15 @@
DiagStatHasFaults(*values.self_test_diag_stat())) {
return true;
}
+ if (values.checksum_failed()) {
+ return true;
+ }
return false;
}
} // namespace
-ImuZeroer::ImuZeroer() {
+ImuZeroer::ImuZeroer(FaultBehavior fault_behavior)
+ : fault_behavior_(fault_behavior) {
gyro_average_.setZero();
accel_average_.setZero();
last_gyro_sample_.setZero();
@@ -32,14 +37,20 @@
return num_zeroes_ > kRequiredZeroPoints && !Faulted();
}
-bool ImuZeroer::Faulted() const { return faulted_; }
+bool ImuZeroer::Faulted() const { return reading_faulted_ || zeroing_faulted_; }
-Eigen::Vector3d ImuZeroer::ZeroedGyro() const {
- return last_gyro_sample_ - gyro_average_;
+std::optional<Eigen::Vector3d> ImuZeroer::ZeroedGyro() const {
+ return Faulted() ? std::nullopt
+ : std::make_optional<Eigen::Vector3d>(last_gyro_sample_ -
+ gyro_average_);
}
-Eigen::Vector3d ImuZeroer::ZeroedAccel() const {
- return last_accel_sample_ - accel_average_;
+
+std::optional<Eigen::Vector3d> ImuZeroer::ZeroedAccel() const {
+ return Faulted() ? std::nullopt
+ : std::make_optional<Eigen::Vector3d>(last_accel_sample_ -
+ accel_average_);
}
+
Eigen::Vector3d ImuZeroer::GyroOffset() const { return gyro_average_; }
bool ImuZeroer::GyroZeroReady() const {
@@ -55,20 +66,25 @@
}
void ImuZeroer::InsertAndProcessMeasurement(const IMUValues &values) {
- InsertMeasurement(values);
- ProcessMeasurements();
+ if (InsertMeasurement(values)) {
+ ProcessMeasurements();
+ }
}
-void ImuZeroer::InsertMeasurement(const IMUValues &values) {
+bool ImuZeroer::InsertMeasurement(const IMUValues &values) {
if (ReadingHasFaults(values)) {
- faulted_ = true;
- return;
+ reading_faulted_ = true;
+ return false;
+ }
+ if (fault_behavior_ == FaultBehavior::kTemporary) {
+ reading_faulted_ = false;
}
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(),
values.accelerometer_z();
accel_averager_.AddData(last_accel_sample_);
+ return true;
}
void ImuZeroer::ProcessMeasurements() {
@@ -88,7 +104,9 @@
// original zero, fault.
if ((current_gyro_average - gyro_average_).norm() >
kGyroFaultVariation) {
- faulted_ = true;
+ zeroing_faulted_ = true;
+ } else if (fault_behavior_ == FaultBehavior::kTemporary) {
+ zeroing_faulted_ = false;
}
}
++num_zeroes_;
diff --git a/frc971/zeroing/imu_zeroer.h b/frc971/zeroing/imu_zeroer.h
index f3af9c5..f9ae4d2 100644
--- a/frc971/zeroing/imu_zeroer.h
+++ b/frc971/zeroing/imu_zeroer.h
@@ -1,6 +1,8 @@
#ifndef FRC971_ZEROING_IMU_ZEROER_H_
#define FRC971_ZEROING_IMU_ZEROER_H_
+#include <optional>
+
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/wpilib/imu_generated.h"
#include "frc971/zeroing/averager.h"
@@ -19,16 +21,25 @@
static constexpr size_t kSamplesToAverage = 200;
static constexpr size_t kRequiredZeroPoints = 10;
- ImuZeroer();
+ enum class FaultBehavior {
+ // When we encounter a fault, latch and stay in an errored state
+ // indefinitely.
+ kLatch,
+ // When we encounter a fault, don't process the reading and return nullopt
+ // for all measurements.
+ kTemporary
+ };
+
+ explicit ImuZeroer(FaultBehavior fault_behavior = FaultBehavior::kLatch);
bool Zeroed() const;
bool Faulted() const;
- void InsertMeasurement(const IMUValues &values);
+ bool InsertMeasurement(const IMUValues &values);
// PErforms the heavier-duty processing for managing zeroing.
void ProcessMeasurements();
void InsertAndProcessMeasurement(const IMUValues &values);
Eigen::Vector3d GyroOffset() const;
- Eigen::Vector3d ZeroedGyro() const;
- Eigen::Vector3d ZeroedAccel() const;
+ std::optional<Eigen::Vector3d> ZeroedGyro() const;
+ std::optional<Eigen::Vector3d> ZeroedAccel() const;
flatbuffers::Offset<control_loops::drivetrain::ImuZeroerState> PopulateStatus(
flatbuffers::FlatBufferBuilder *fbb) const;
@@ -62,8 +73,11 @@
Eigen::Vector3d accel_average_;
Eigen::Vector3d last_gyro_sample_;
Eigen::Vector3d last_accel_sample_;
- // Whether the zeroing has faulted at any point thus far.
- bool faulted_ = false;
+
+ const FaultBehavior fault_behavior_;
+ bool reading_faulted_ = false;
+ bool zeroing_faulted_ = false;
+
size_t good_iters_ = 0;
size_t num_zeroes_ = 0;
};
diff --git a/frc971/zeroing/imu_zeroer_test.cc b/frc971/zeroing/imu_zeroer_test.cc
index 0a68541..deff1b6 100644
--- a/frc971/zeroing/imu_zeroer_test.cc
+++ b/frc971/zeroing/imu_zeroer_test.cc
@@ -42,8 +42,8 @@
ASSERT_FALSE(zeroer.Zeroed());
ASSERT_FALSE(zeroer.Faulted());
ASSERT_EQ(0.0, zeroer.GyroOffset().norm());
- ASSERT_EQ(0.0, zeroer.ZeroedGyro().norm());
- ASSERT_EQ(0.0, zeroer.ZeroedAccel().norm());
+ ASSERT_EQ(0.0, zeroer.ZeroedGyro().value().norm());
+ ASSERT_EQ(0.0, zeroer.ZeroedAccel().value().norm());
// A measurement before we are zeroed should just result in the measurement
// being passed through without modification.
zeroer.InsertAndProcessMeasurement(
@@ -51,12 +51,12 @@
ASSERT_FALSE(zeroer.Zeroed());
ASSERT_FALSE(zeroer.Faulted());
ASSERT_EQ(0.0, zeroer.GyroOffset().norm());
- ASSERT_FLOAT_EQ(0.01, zeroer.ZeroedGyro().x());
- ASSERT_FLOAT_EQ(0.02, zeroer.ZeroedGyro().y());
- ASSERT_FLOAT_EQ(0.03, zeroer.ZeroedGyro().z());
- ASSERT_EQ(4.0, zeroer.ZeroedAccel().x());
- ASSERT_EQ(5.0, zeroer.ZeroedAccel().y());
- ASSERT_EQ(6.0, zeroer.ZeroedAccel().z());
+ ASSERT_FLOAT_EQ(0.01, zeroer.ZeroedGyro().value().x());
+ ASSERT_FLOAT_EQ(0.02, zeroer.ZeroedGyro().value().y());
+ ASSERT_FLOAT_EQ(0.03, zeroer.ZeroedGyro().value().z());
+ ASSERT_EQ(4.0, zeroer.ZeroedAccel().value().x());
+ ASSERT_EQ(5.0, zeroer.ZeroedAccel().value().y());
+ ASSERT_EQ(6.0, zeroer.ZeroedAccel().value().z());
}
// Tests that we zero if we receive a bunch of identical measurements.
@@ -73,21 +73,21 @@
ASSERT_FLOAT_EQ(0.01, zeroer.GyroOffset().x());
ASSERT_FLOAT_EQ(0.02, zeroer.GyroOffset().y());
ASSERT_FLOAT_EQ(0.03, zeroer.GyroOffset().z());
- ASSERT_EQ(0.0, zeroer.ZeroedGyro().x());
- ASSERT_EQ(0.0, zeroer.ZeroedGyro().y());
- ASSERT_EQ(0.0, zeroer.ZeroedGyro().z());
+ ASSERT_EQ(0.0, zeroer.ZeroedGyro().value().x());
+ ASSERT_EQ(0.0, zeroer.ZeroedGyro().value().y());
+ ASSERT_EQ(0.0, zeroer.ZeroedGyro().value().z());
// Accelerometer readings should not be affected.
- ASSERT_EQ(4.0, zeroer.ZeroedAccel().x());
- ASSERT_EQ(5.0, zeroer.ZeroedAccel().y());
- ASSERT_EQ(6.0, zeroer.ZeroedAccel().z());
+ ASSERT_EQ(4.0, zeroer.ZeroedAccel().value().x());
+ ASSERT_EQ(5.0, zeroer.ZeroedAccel().value().y());
+ ASSERT_EQ(6.0, zeroer.ZeroedAccel().value().z());
// If we get another measurement offset by {1, 1, 1} we should read the result
// as {1, 1, 1}.
zeroer.InsertAndProcessMeasurement(
MakeMeasurement({0.02, 0.03, 0.04}, {0, 0, 0}).message());
ASSERT_FALSE(zeroer.Faulted());
- ASSERT_FLOAT_EQ(0.01, zeroer.ZeroedGyro().x());
- ASSERT_FLOAT_EQ(0.01, zeroer.ZeroedGyro().y());
- ASSERT_FLOAT_EQ(0.01, zeroer.ZeroedGyro().z());
+ ASSERT_FLOAT_EQ(0.01, zeroer.ZeroedGyro().value().x());
+ ASSERT_FLOAT_EQ(0.01, zeroer.ZeroedGyro().value().y());
+ ASSERT_FLOAT_EQ(0.01, zeroer.ZeroedGyro().value().z());
}
// Tests that we do not zero if the gyro is producing particularly high
@@ -125,12 +125,12 @@
zeroer.InsertAndProcessMeasurement(
MakeMeasurement({0.02, 0.03, 0.04}, {0, 0, 0}).message());
ASSERT_FALSE(zeroer.Faulted());
- ASSERT_NEAR(0.01, zeroer.ZeroedGyro().x(), 1e-3);
- ASSERT_NEAR(0.01, zeroer.ZeroedGyro().y(), 1e-3);
- ASSERT_NEAR(0.01, zeroer.ZeroedGyro().z(), 1e-3);
- ASSERT_EQ(0.0, zeroer.ZeroedAccel().x());
- ASSERT_EQ(0.0, zeroer.ZeroedAccel().y());
- ASSERT_EQ(0.0, zeroer.ZeroedAccel().z());
+ ASSERT_NEAR(0.01, zeroer.ZeroedGyro().value().x(), 1e-3);
+ ASSERT_NEAR(0.01, zeroer.ZeroedGyro().value().y(), 1e-3);
+ ASSERT_NEAR(0.01, zeroer.ZeroedGyro().value().z(), 1e-3);
+ ASSERT_EQ(0.0, zeroer.ZeroedAccel().value().x());
+ ASSERT_EQ(0.0, zeroer.ZeroedAccel().value().y());
+ ASSERT_EQ(0.0, zeroer.ZeroedAccel().value().z());
}
// Tests that we do not zero if there is too much noise in the input data.