Make ImuWatcher class for handling pico idiosyncracies
This provides a class to consistently handle the IMUValuesBatch coming
from the imu process on a pi.
Change-Id: Id2cdeb23fdd0a3e1c2bd01219678d957906112fa
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/frc971/imu_reader/BUILD b/frc971/imu_reader/BUILD
index 7d424c4..4825900 100644
--- a/frc971/imu_reader/BUILD
+++ b/frc971/imu_reader/BUILD
@@ -1,3 +1,6 @@
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
+
cc_library(
name = "imu",
srcs = [
@@ -14,8 +17,38 @@
"//aos/util:crc32",
"//frc971/wpilib:imu_batch_fbs",
"//frc971/wpilib:imu_fbs",
- "//y2022:constants",
"@com_github_google_glog//:glog",
"@com_google_absl//absl/types:span",
],
)
+
+flatbuffer_cc_library(
+ name = "imu_failures_fbs",
+ srcs = [
+ "imu_failures.fbs",
+ ],
+ visibility = ["//visibility:public"],
+)
+
+flatbuffer_ts_library(
+ name = "imu_failures_ts_fbs",
+ srcs = [
+ "imu_failures.fbs",
+ ],
+ visibility = ["//visibility:public"],
+)
+
+cc_library(
+ name = "imu_watcher",
+ srcs = ["imu_watcher.cc"],
+ hdrs = ["imu_watcher.h"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":imu_failures_fbs",
+ "//aos/events:event_loop",
+ "//frc971/control_loops/drivetrain:drivetrain_config",
+ "//frc971/wpilib:imu_batch_fbs",
+ "//frc971/zeroing:imu_zeroer",
+ "//frc971/zeroing:wrap",
+ ],
+)
diff --git a/frc971/imu_reader/imu_failures.fbs b/frc971/imu_reader/imu_failures.fbs
new file mode 100644
index 0000000..e6d9d85
--- /dev/null
+++ b/frc971/imu_reader/imu_failures.fbs
@@ -0,0 +1,16 @@
+namespace frc971.controls;
+
+// Counters to track how many times different errors have occurred.
+table ImuFailures {
+ // Count of total number of checksum mismatches between the IMU and the
+ // pico itself.
+ imu_to_pico_checksum_mismatch:uint (id: 0);
+ // Count of total number of checksum mismatches between the pico board
+ // and the raspberry pi.
+ pico_to_pi_checksum_mismatch:uint (id: 1);
+ // Total number of dropped/missed messages.
+ missed_messages:uint (id: 2);
+ // Total number of messages dropped for any other conditions that can fault
+ // the zeroer (e.g., diagnostic failures in the IMU).
+ other_zeroing_faults:uint (id: 3);
+}
diff --git a/frc971/imu_reader/imu_watcher.cc b/frc971/imu_reader/imu_watcher.cc
new file mode 100644
index 0000000..0e0d656
--- /dev/null
+++ b/frc971/imu_reader/imu_watcher.cc
@@ -0,0 +1,117 @@
+#include "frc971/imu_reader/imu_watcher.h"
+
+#include "frc971/wpilib/imu_batch_generated.h"
+
+namespace frc971::controls {
+namespace {
+// Return the amount of distance that the drivetrain can travel before the
+// encoders will wrap. Necessary because the pico only sends over the encoders
+// in 16-bit counters, which will wrap relatively readily.
+double EncoderWrapDistance(double drivetrain_distance_per_encoder_tick) {
+ return drivetrain_distance_per_encoder_tick * (1 << 16);
+}
+} // namespace
+ImuWatcher::ImuWatcher(
+ aos::EventLoop *event_loop,
+ const control_loops::drivetrain::DrivetrainConfig<double> &dt_config,
+ const double drivetrain_distance_per_encoder_tick,
+ std::function<
+ void(aos::monotonic_clock::time_point, aos::monotonic_clock::time_point,
+ std::optional<Eigen::Vector2d>, Eigen::Vector3d, Eigen::Vector3d)>
+ callback)
+ : dt_config_(dt_config),
+ callback_(std::move(callback)),
+ zeroer_(zeroing::ImuZeroer::FaultBehavior::kTemporary),
+ left_encoder_(
+ -EncoderWrapDistance(drivetrain_distance_per_encoder_tick) / 2.0,
+ EncoderWrapDistance(drivetrain_distance_per_encoder_tick)),
+ right_encoder_(
+ -EncoderWrapDistance(drivetrain_distance_per_encoder_tick) / 2.0,
+ EncoderWrapDistance(drivetrain_distance_per_encoder_tick)) {
+ event_loop->MakeWatcher("/localizer", [this, event_loop](
+ const IMUValuesBatch &values) {
+ CHECK(values.has_readings());
+ 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();
+ }
+ }
+ // Set encoders to nullopt if we are faulted at all (faults may include
+ // checksum mismatches).
+ const std::optional<Eigen::Vector2d> encoders =
+ zeroer_.Faulted()
+ ? std::nullopt
+ : std::make_optional(Eigen::Vector2d{
+ left_encoder_.Unwrap(value->left_encoder()),
+ right_encoder_.Unwrap(value->right_encoder())});
+ {
+ // 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()) {
+ pico_offset_ =
+ event_loop->context().monotonic_event_time - pico_timestamp;
+ last_pico_timestamp_ = pico_timestamp;
+ }
+ if (pico_timestamp < last_pico_timestamp_) {
+ pico_offset_.value() += std::chrono::microseconds(1ULL << 32);
+ }
+ const aos::monotonic_clock::time_point sample_timestamp =
+ pico_offset_.value() + pico_timestamp;
+ pico_offset_error_ =
+ event_loop->context().monotonic_event_time - sample_timestamp;
+ const bool zeroed = zeroer_.Zeroed();
+
+ // When not zeroed, we aim to approximate zero acceleration by doing a
+ // zero-order hold on the gyro and setting the accelerometer readings to
+ // gravity.
+ callback_(sample_timestamp,
+ aos::monotonic_clock::time_point(std::chrono::nanoseconds(
+ value->monotonic_timestamp_ns())),
+ encoders, zeroed ? zeroer_.ZeroedGyro().value() : last_gyro_,
+ zeroed ? zeroer_.ZeroedAccel().value()
+ : dt_config_.imu_transform.transpose() *
+ Eigen::Vector3d::UnitZ());
+
+ if (zeroed) {
+ last_gyro_ = zeroer_.ZeroedGyro().value();
+ }
+ last_pico_timestamp_ = pico_timestamp;
+ }
+ }
+ });
+}
+} // namespace frc971::controls
diff --git a/frc971/imu_reader/imu_watcher.h b/frc971/imu_reader/imu_watcher.h
new file mode 100644
index 0000000..8867266
--- /dev/null
+++ b/frc971/imu_reader/imu_watcher.h
@@ -0,0 +1,104 @@
+#ifndef FRC971_IMU_READER_IMU_WATCHER_H_
+#define FRC971_IMU_READER_IMU_WATCHER_H_
+
+#include "aos/events/event_loop.h"
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/imu_reader/imu_failures_generated.h"
+#include "frc971/zeroing/imu_zeroer.h"
+#include "frc971/zeroing/wrap.h"
+
+namespace frc971::controls {
+// This class handles listening to an IMUValuesBatch channel sourced off of our
+// ADIS16505 pico board and calling the user-specified callback with the
+// relevant data. This intermediary is used to unwrap encoder readings, check
+// for checksum mismatches, zero the gyro/accelerometer, and translate
+// timestamps between devices.
+// TODO(james): Get unit tests for this class specifically written (we already
+// have tests for the code that exercises this).
+class ImuWatcher {
+ public:
+ // Expected frequency of messages from the pico-based IMU.
+ static constexpr std::chrono::microseconds kNominalDt{500};
+
+ // The callback specified by the user will take:
+ // sample_time_pico: The pico-based timestamp corresponding to the measurement
+ // time. This will be offset by roughly pico_offset_error from the pi's
+ // monotonic clock.
+ // sample_time_pi: Timestamp from the kernel for when the pi observed the
+ // relevant measurement.
+ // encoders: Current encoder values, [left, right]. nullopt if we have faults.
+ // gyro: Current gyro readings, in the raw IMU axes (i.e., these must be
+ // rotated by dt_config.imu_transform before being used). Suitable
+ // for input to the down estimator.
+ // accel: Current accelerometer readings, in the raw IMU axes (i.e., these
+ // must be rotated by dt_config.imu_transform before being used). Suitable
+ // for input to the down estimator.
+ ImuWatcher(
+ aos::EventLoop *event_loop,
+ const control_loops::drivetrain::DrivetrainConfig<double> &dt_config,
+ double drivetrain_distance_per_encoder_tick,
+ std::function<void(
+ aos::monotonic_clock::time_point, aos::monotonic_clock::time_point,
+ std::optional<Eigen::Vector2d>, Eigen::Vector3d, Eigen::Vector3d)>
+ callback);
+
+ const zeroing::ImuZeroer &zeroer() const { return zeroer_; }
+
+ flatbuffers::Offset<ImuFailures> PopulateImuFailures(
+ flatbuffers::FlatBufferBuilder *fbb) const {
+ return ImuFailures::Pack(*fbb, &imu_fault_tracker_);
+ }
+
+ // t = pico_offset + pico_timestamp.
+ // Note that this can drift over sufficiently long time periods!
+ std::optional<std::chrono::nanoseconds> pico_offset() const {
+ return pico_offset_;
+ }
+ // pico_offset_error = actual_time - (pico_offset + pico_timestamp)
+ // If the pico clock and pi clock are exactly in sync, this will always be
+ // zero.
+ aos::monotonic_clock::duration pico_offset_error() const {
+ return pico_offset_error_;
+ }
+
+ private:
+ const control_loops::drivetrain::DrivetrainConfig<double> dt_config_;
+ std::function<void(
+ aos::monotonic_clock::time_point, aos::monotonic_clock::time_point,
+ std::optional<Eigen::Vector2d>, Eigen::Vector3d, Eigen::Vector3d)>
+ callback_;
+
+ // Last observed pico measurement. Used to track IMU staleness.
+ std::optional<aos::monotonic_clock::time_point> last_pico_timestamp_;
+ // Estimate of the drift between the pi and pico clocks. See
+ // pico_offset_error() for definition.
+ aos::monotonic_clock::duration pico_offset_error_;
+ // Raw offset between the pico and pi clocks. Gets updated to compensate for
+ // wrapping in the pico timestamp.
+ std::optional<std::chrono::nanoseconds> pico_offset_;
+
+ zeroing::ImuZeroer zeroer_;
+
+ ImuFailuresT imu_fault_tracker_;
+ // The first observed data counter. This is used to help us track dropped
+ // messages.
+ std::optional<size_t> first_valid_data_counter_;
+ size_t total_imu_messages_received_ = 0;
+ // added to the current read data counter to allow the data counter to
+ // increase monotonically. Will be a multiple of 2 ** 16.
+ size_t data_counter_offset_ = 0;
+ // PRevious data counter value (data_counter_offset_ not included).
+ int last_data_counter_ = 0;
+
+ // Unwrappers for the left and right encoders (necessary because the pico only
+ // sends out 16-bit encoder counts).
+ zeroing::UnwrapSensor left_encoder_;
+ zeroing::UnwrapSensor right_encoder_;
+
+ // When we lose IMU readings (e.g., due to checksum mismatches), we perform a
+ // zero-order hold on gyro readings; in order to do this, store the most
+ // recent gyro readings.
+ Eigen::Vector3d last_gyro_ = Eigen::Vector3d::Zero();
+};
+} // namespace frc971::controls
+#endif // FRC971_IMU_READER_IMU_WATCHER_H_