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/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 4b36714..7d297e0 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -137,8 +137,8 @@
if (last_imu_update_ == aos::monotonic_clock::min_time) {
last_imu_update_ = reading_time;
}
- down_estimator_.Predict(imu_zeroer_.ZeroedGyro(),
- imu_zeroer_.ZeroedAccel(),
+ down_estimator_.Predict(imu_zeroer_.ZeroedGyro().value(),
+ imu_zeroer_.ZeroedAccel().value(),
reading_time - last_imu_update_);
last_imu_update_ = reading_time;
}
@@ -170,26 +170,31 @@
}
// TODO(austin): Signal the current gear to both loops.
+ bool imu_zeroer_zeroed = imu_zeroer_.Zeroed();
switch (dt_config_.gyro_type) {
case GyroType::IMU_X_GYRO:
if (got_imu_reading) {
- last_gyro_rate_ = imu_zeroer_.ZeroedGyro().x();
+ last_gyro_rate_ =
+ imu_zeroer_zeroed ? imu_zeroer_.ZeroedGyro().value().x() : 0.0;
}
break;
case GyroType::IMU_Y_GYRO:
if (got_imu_reading) {
- last_gyro_rate_ = imu_zeroer_.ZeroedGyro().y();
+ last_gyro_rate_ =
+ imu_zeroer_zeroed ? imu_zeroer_.ZeroedGyro().value().y() : 0.0;
}
break;
case GyroType::IMU_Z_GYRO:
if (got_imu_reading) {
- last_gyro_rate_ = imu_zeroer_.ZeroedGyro().z();
+ last_gyro_rate_ =
+ imu_zeroer_zeroed ? imu_zeroer_.ZeroedGyro().value().z() : 0.0;
}
break;
case GyroType::FLIPPED_IMU_Z_GYRO:
if (got_imu_reading) {
- last_gyro_rate_ = -imu_zeroer_.ZeroedGyro().z();
+ last_gyro_rate_ =
+ imu_zeroer_zeroed ? -imu_zeroer_.ZeroedGyro().value().z() : 0.0;
}
break;
case GyroType::SPARTAN_GYRO:
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.
diff --git a/y2022/localizer/localizer.cc b/y2022/localizer/localizer.cc
index 1aa2458..964bf74 100644
--- a/y2022/localizer/localizer.cc
+++ b/y2022/localizer/localizer.cc
@@ -862,6 +862,7 @@
aos::EventLoop *event_loop,
const control_loops::drivetrain::DrivetrainConfig<double> &dt_config)
: event_loop_(event_loop),
+ dt_config_(dt_config),
model_based_(dt_config),
status_sender_(event_loop_->MakeSender<LocalizerStatus>("/localizer")),
output_sender_(event_loop_->MakeSender<LocalizerOutput>("/localizer")),
@@ -962,7 +963,7 @@
const Eigen::Vector2d encoders{
left_encoder_.Unwrap(value->left_encoder()),
right_encoder_.Unwrap(value->right_encoder())};
- if (zeroer_.Zeroed()) {
+ {
const aos::monotonic_clock::time_point pico_timestamp{
std::chrono::microseconds(value->pico_timestamp_us())};
// TODO(james): If we get large enough drift off of the pico,
@@ -984,8 +985,13 @@
(output_fetcher_.context().monotonic_event_time +
std::chrono::milliseconds(10) <
event_loop_->context().monotonic_event_time);
+ const bool zeroed = zeroer_.Zeroed();
model_based_.HandleImu(
- sample_timestamp, zeroer_.ZeroedGyro(), zeroer_.ZeroedAccel(),
+ sample_timestamp,
+ zeroed ? zeroer_.ZeroedGyro().value() : Eigen::Vector3d::Zero(),
+ zeroed ? zeroer_.ZeroedAccel().value()
+ : dt_config_.imu_transform.transpose() *
+ Eigen::Vector3d::UnitZ(),
encoders,
disabled ? Eigen::Vector2d::Zero()
: Eigen::Vector2d{output_fetcher_->left_voltage(),
diff --git a/y2022/localizer/localizer.h b/y2022/localizer/localizer.h
index e9a61e1..be14b45 100644
--- a/y2022/localizer/localizer.h
+++ b/y2022/localizer/localizer.h
@@ -321,6 +321,7 @@
std::optional<aos::monotonic_clock::duration> ClockOffset(
std::string_view pi);
aos::EventLoop *event_loop_;
+ const control_loops::drivetrain::DrivetrainConfig<double> &dt_config_;
ModelBasedLocalizer model_based_;
aos::Sender<LocalizerStatus> status_sender_;
aos::Sender<LocalizerOutput> output_sender_;