Better handle missed IMU messages in localizer
Increase the max dt to better handle dropped messages.
Also, more explicitly track the reasons why different messages were
dropped. The current main culprits seem to be (a) messages missing
entirely and (b) Pi <-> PICO checksum failures.
Change-Id: I882b29a304fb0a673722d9dc37184a87e0a27a0d
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2022/localizer/localizer.cc b/y2022/localizer/localizer.cc
index b12525b..954bffc 100644
--- a/y2022/localizer/localizer.cc
+++ b/y2022/localizer/localizer.cc
@@ -266,7 +266,7 @@
if (t_ == aos::monotonic_clock::min_time) {
t_ = t;
}
- if (t_ + 2 * kNominalDt < t) {
+ if (t_ + 10 * kNominalDt < t) {
t_ = t;
++clock_resets_;
}
@@ -970,6 +970,34 @@
output_fetcher_.Fetch();
for (const IMUValues *value : *values.readings()) {
zeroer_.InsertAndProcessMeasurement(*value);
+ if (zeroer_.Faulted()) {
+ if (value->checksum_failed()) {
+ imu_fault_tracker_.pico_to_pi_checksum_mismatch++;
+ } else if (value->previous_reading_diag_stat()->checksum_mismatch()) {
+ imu_fault_tracker_.imu_to_pico_checksum_mismatch++;
+ } else {
+ imu_fault_tracker_.other_zeroing_faults++;
+ }
+ } else {
+ if (!first_valid_data_counter_.has_value()) {
+ first_valid_data_counter_ = value->data_counter();
+ }
+ }
+ if (first_valid_data_counter_.has_value()) {
+ total_imu_messages_received_++;
+ // Only update when we have good checksums, since the data counter
+ // could get corrupted.
+ if (!zeroer_.Faulted()) {
+ if (value->data_counter() < last_data_counter_) {
+ data_counter_offset_ += 1 << 16;
+ }
+ imu_fault_tracker_.missed_messages =
+ (1 + value->data_counter() + data_counter_offset_ -
+ first_valid_data_counter_.value()) -
+ total_imu_messages_received_;
+ last_data_counter_ = value->data_counter();
+ }
+ }
const std::optional<Eigen::Vector2d> encoders =
zeroer_.Faulted()
? std::nullopt
@@ -977,8 +1005,16 @@
left_encoder_.Unwrap(value->left_encoder()),
right_encoder_.Unwrap(value->right_encoder())});
{
- const aos::monotonic_clock::time_point pico_timestamp{
- std::chrono::microseconds(value->pico_timestamp_us())};
+ // If we can't trust the imu reading, just naively increment the
+ // pico timestamp.
+ const aos::monotonic_clock::time_point pico_timestamp =
+ zeroer_.Faulted()
+ ? (last_pico_timestamp_.has_value()
+ ? last_pico_timestamp_.value() + kNominalDt
+ : aos::monotonic_clock::epoch())
+ : aos::monotonic_clock::time_point(
+ std::chrono::microseconds(
+ value->pico_timestamp_us()));
// TODO(james): If we get large enough drift off of the pico,
// actually do something about it.
if (!pico_offset_.has_value()) {
@@ -999,9 +1035,11 @@
std::chrono::milliseconds(10) <
event_loop_->context().monotonic_event_time);
const bool zeroed = zeroer_.Zeroed();
+ // For gyros, use the most recent gyro reading if we aren't zeroed,
+ // to avoid missing integration cycles.
model_based_.HandleImu(
sample_timestamp,
- zeroed ? zeroer_.ZeroedGyro().value() : Eigen::Vector3d::Zero(),
+ zeroed ? zeroer_.ZeroedGyro().value() : last_gyro_,
zeroed ? zeroer_.ZeroedAccel().value()
: dt_config_.imu_transform.transpose() *
Eigen::Vector3d::UnitZ(),
@@ -1010,6 +1048,10 @@
: Eigen::Vector2d{output_fetcher_->left_voltage(),
output_fetcher_->right_voltage()});
last_pico_timestamp_ = pico_timestamp;
+
+ if (zeroed) {
+ last_gyro_ = zeroer_.ZeroedGyro().value();
+ }
}
{
auto builder = status_sender_.MakeBuilder();
@@ -1017,12 +1059,15 @@
model_based_.PopulateStatus(builder.fbb());
const flatbuffers::Offset<control_loops::drivetrain::ImuZeroerState>
zeroer_status = zeroer_.PopulateStatus(builder.fbb());
+ const flatbuffers::Offset<ImuFailures> imu_failures =
+ ImuFailures::Pack(*builder.fbb(), &imu_fault_tracker_);
LocalizerStatus::Builder status_builder =
builder.MakeBuilder<LocalizerStatus>();
status_builder.add_model_based(model_based_status);
status_builder.add_zeroed(zeroer_.Zeroed());
status_builder.add_faulted_zero(zeroer_.Faulted());
status_builder.add_zeroing(zeroer_status);
+ status_builder.add_imu_failures(imu_failures);
if (encoders.has_value()) {
status_builder.add_left_encoder(encoders.value()(0));
status_builder.add_right_encoder(encoders.value()(1));
diff --git a/y2022/localizer/localizer.h b/y2022/localizer/localizer.h
index f2cb50e..f8205d7 100644
--- a/y2022/localizer/localizer.h
+++ b/y2022/localizer/localizer.h
@@ -345,6 +345,14 @@
// Note that this can drift over sufficiently long time periods!
std::optional<std::chrono::nanoseconds> pico_offset_;
+ ImuFailuresT imu_fault_tracker_;
+ std::optional<size_t> first_valid_data_counter_;
+ size_t total_imu_messages_received_ = 0;
+ size_t data_counter_offset_ = 0;
+ int last_data_counter_ = 0;
+
+ Eigen::Vector3d last_gyro_ = Eigen::Vector3d::Zero();
+
zeroing::UnwrapSensor left_encoder_;
zeroing::UnwrapSensor right_encoder_;
};
diff --git a/y2022/localizer/localizer_plotter.ts b/y2022/localizer/localizer_plotter.ts
index 239d2cb..bcd8894 100644
--- a/y2022/localizer/localizer_plotter.ts
+++ b/y2022/localizer/localizer_plotter.ts
@@ -268,10 +268,34 @@
const timingPlot =
aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
- timingPlot.plot.getAxisLabels().setTitle('Timing');
+ timingPlot.plot.getAxisLabels().setTitle('Fault Counting');
timingPlot.plot.getAxisLabels().setXLabel(TIME);
timingPlot.addMessageLine(localizer, ['model_based', 'clock_resets'])
.setColor(GREEN)
.setDrawLine(false);
+
+ timingPlot
+ .addMessageLine(
+ localizer, ['imu_failures', 'imu_to_pico_checksum_mismatch'])
+ .setColor(BLUE)
+ .setDrawLine(false);
+
+ timingPlot
+ .addMessageLine(
+ localizer, ['imu_failures', 'pico_to_pi_checksum_mismatch'])
+ .setColor(RED)
+ .setDrawLine(false);
+
+ timingPlot
+ .addMessageLine(
+ localizer, ['imu_failures', 'other_zeroing_faults'])
+ .setColor(CYAN)
+ .setDrawLine(false);
+
+ timingPlot
+ .addMessageLine(
+ localizer, ['imu_failures', 'missed_messages'])
+ .setColor(PINK)
+ .setDrawLine(false);
}
diff --git a/y2022/localizer/localizer_status.fbs b/y2022/localizer/localizer_status.fbs
index ae96b63..05cfc73 100644
--- a/y2022/localizer/localizer_status.fbs
+++ b/y2022/localizer/localizer_status.fbs
@@ -92,6 +92,13 @@
statistics:CumulativeStatistics (id: 18);
}
+table ImuFailures {
+ imu_to_pico_checksum_mismatch:uint (id: 0);
+ pico_to_pi_checksum_mismatch:uint (id: 1);
+ missed_messages:uint (id: 2);
+ other_zeroing_faults:uint (id: 3);
+}
+
table LocalizerStatus {
model_based:ModelBasedStatus (id: 0);
// Whether the IMU is zeroed or not.
@@ -110,6 +117,7 @@
pico_offset_error_ns:int64 (id: 5);
left_encoder:double (id: 6);
right_encoder:double (id: 7);
+ imu_failures:ImuFailures (id: 8);
}
root_type LocalizerStatus;