frc971/imu_reader/imu_watcher.cc

#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,
    TimestampSource timestamp_source)
    : 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, timestamp_source, 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->has_previous_reading_diag_stat() &&
                   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() &&
            value->has_data_counter()) {
          first_valid_data_counter_ = value->data_counter();
        }
      }
      int messages_dropped_this_cycle = 0;
      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;
          }
          const int total_dropped =
              (1 + value->data_counter() + data_counter_offset_ -
               first_valid_data_counter_.value()) -
              total_imu_messages_received_;
          messages_dropped_this_cycle =
              total_dropped - imu_fault_tracker_.missed_messages;
          imu_fault_tracker_.missed_messages = total_dropped;
          last_data_counter_ = value->data_counter();
        }
      }
      // Set encoders to nullopt if we are faulted at all (faults may include
      // checksum mismatches).
      const bool have_encoders = !zeroer_.Faulted() &&
                                 value->has_left_encoder() &&
                                 value->has_right_encoder();
      const std::optional<Eigen::Vector2d> encoders =
          have_encoders ? std::make_optional(Eigen::Vector2d{
                              left_encoder_.Unwrap(value->left_encoder()),
                              right_encoder_.Unwrap(value->right_encoder())})
                        : std::nullopt;
      {
        const aos::monotonic_clock::time_point pi_read_timestamp =
            aos::monotonic_clock::time_point(
                std::chrono::nanoseconds(value->monotonic_timestamp_ns()));
        // If we can't trust the imu reading, just naively increment the
        // pico timestamp.
        const aos::monotonic_clock::time_point pico_timestamp =
            timestamp_source == TimestampSource::kPi
                ? pi_read_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_ = pi_read_timestamp - pico_timestamp;
          last_pico_timestamp_ = pico_timestamp;
        }
        // The pico will sleep for a minimum of 3 seconds when resetting the
        // IMU. Any absence of messages for less than that time is probably not
        // an actual reset.
        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_ = pi_read_timestamp - sample_timestamp;

        if (last_imu_message_.has_value()) {
          constexpr std::chrono::seconds kMinimumImuResetTime(3);
          const std::chrono::nanoseconds message_time_gap =
              last_imu_message_.value() -
              event_loop->context().monotonic_event_time;
          if (message_time_gap > kMinimumImuResetTime) {
            bool assigned_fault = false;
            // The IMU has probably reset; attempt to determine whether just the
            // IMU was reset or if the IMU and pico reset.
            if (std::chrono::abs(pico_offset_error_) >
                std::chrono::seconds(1)) {
              // If we have this big of a gap, then everything downstream of
              // this is going to complain anyways, so reset the offset.
              pico_offset_.reset();

              imu_fault_tracker_.probable_pico_reset_count++;
              assigned_fault = true;
            }
            // See if we dropped the "right" number of messages for the gap in
            // question.
            if (std::chrono::abs(messages_dropped_this_cycle * kNominalDt -
                                 message_time_gap) >
                std::chrono::milliseconds(100)) {
              imu_fault_tracker_.probable_imu_reset_count++;
              assigned_fault = true;
            }

            if (!assigned_fault) {
              imu_fault_tracker_.unassignable_reset_count++;
            }
          }
        }
        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, pi_read_timestamp, 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;
      }
      last_imu_message_ = event_loop->context().monotonic_event_time;
    }
  });
}
}  // namespace frc971::controls