Add a driver for the new IMU
Redo how zeroing works for the old one too.
This also forced me to update the ahal SPI library to a slightly pared
down version of what WPILib master has.
Change-Id: I631ff230c053c6256325ab6f4e532ca90c901424
diff --git a/frc971/wpilib/ADIS16448.cc b/frc971/wpilib/ADIS16448.cc
index 57987e6..76c5894 100644
--- a/frc971/wpilib/ADIS16448.cc
+++ b/frc971/wpilib/ADIS16448.cc
@@ -8,10 +8,8 @@
#include <chrono>
#include "aos/init.h"
-#include "aos/robot_state/robot_state_generated.h"
#include "aos/time/time.h"
#include "frc971/wpilib/imu_generated.h"
-#include "frc971/zeroing/averager.h"
namespace frc971 {
namespace wpilib {
@@ -120,7 +118,6 @@
ADIS16448::ADIS16448(::aos::ShmEventLoop *event_loop, frc::SPI::Port port,
frc::DigitalInput *dio1)
: event_loop_(event_loop),
- robot_state_fetcher_(event_loop_->MakeFetcher<::aos::RobotState>("/aos")),
imu_values_sender_(
event_loop_->MakeSender<::frc971::IMUValues>("/drivetrain")),
spi_(new frc::SPI(port)),
@@ -184,13 +181,6 @@
void ADIS16448::DoRun() {
InitializeUntilSuccessful();
- // Rounded to approximate the 204.8 Hz.
- constexpr size_t kImuSendRate = 205;
-
- zeroing::Averager<double, 6 * kImuSendRate> average_gyro_x;
- zeroing::Averager<double, 6 * kImuSendRate> average_gyro_y;
- zeroing::Averager<double, 6 * kImuSendRate> average_gyro_z;
-
bool got_an_interrupt = false;
while (event_loop_->is_running()) {
{
@@ -205,7 +195,6 @@
}
}
got_an_interrupt = true;
- const monotonic_clock::time_point read_time = monotonic_clock::now();
uint8_t to_send[2 * 14], to_receive[2 * 14];
memset(&to_send[0], 0, sizeof(to_send));
@@ -249,11 +238,11 @@
IMUValues::Builder imu_builder = builder.MakeBuilder<IMUValues>();
- imu_builder.add_fpga_timestamp(::aos::time::DurationInSeconds(
- dio1_->ReadRisingTimestamp().time_since_epoch()));
+ const auto fpga_time = dio1_->ReadRisingTimestamp();
+ imu_builder.add_fpga_timestamp(
+ ::aos::time::DurationInSeconds(fpga_time.time_since_epoch()));
imu_builder.add_monotonic_timestamp_ns(
- chrono::duration_cast<chrono::nanoseconds>(read_time.time_since_epoch())
- .count());
+ time_converter_.FpgaToMonotonic(fpga_time).time_since_epoch().count());
float gyro_x =
ConvertValue(&to_receive[4], kGyroLsbDegreeSecond * M_PI / 180.0);
@@ -262,32 +251,6 @@
float gyro_z =
ConvertValue(&to_receive[8], kGyroLsbDegreeSecond * M_PI / 180.0);
- // The first few seconds of samples are averaged and subtracted from
- // subsequent samples for zeroing purposes.
- if (!gyros_are_zeroed_) {
- average_gyro_x.AddData(gyro_x);
- average_gyro_y.AddData(gyro_y);
- average_gyro_z.AddData(gyro_z);
-
- if (average_gyro_x.full() && average_gyro_y.full() &&
- average_gyro_z.full()) {
- robot_state_fetcher_.Fetch();
- if (robot_state_fetcher_.get() &&
- robot_state_fetcher_->outputs_enabled()) {
- gyro_x_zeroed_offset_ = -average_gyro_x.GetAverage();
- gyro_y_zeroed_offset_ = -average_gyro_y.GetAverage();
- gyro_z_zeroed_offset_ = -average_gyro_z.GetAverage();
- AOS_LOG(INFO, "total gyro zero offset X:%f, Y:%f, Z:%f\n",
- gyro_x_zeroed_offset_, gyro_y_zeroed_offset_,
- gyro_z_zeroed_offset_);
- gyros_are_zeroed_ = true;
- }
- }
- }
- gyro_x += gyro_x_zeroed_offset_;
- gyro_y += gyro_y_zeroed_offset_;
- gyro_z += gyro_z_zeroed_offset_;
-
imu_builder.add_gyro_x(gyro_x);
imu_builder.add_gyro_y(gyro_y);
imu_builder.add_gyro_z(gyro_z);
diff --git a/frc971/wpilib/ADIS16448.h b/frc971/wpilib/ADIS16448.h
index 8f6da89..04d1712 100644
--- a/frc971/wpilib/ADIS16448.h
+++ b/frc971/wpilib/ADIS16448.h
@@ -13,7 +13,7 @@
#include "aos/events/shm_event_loop.h"
#include "aos/logging/logging.h"
-#include "aos/robot_state/robot_state_generated.h"
+#include "frc971/wpilib/fpga_time_conversion.h"
#include "frc971/wpilib/imu_generated.h"
#include "frc971/wpilib/spi_rx_clearer.h"
@@ -49,13 +49,8 @@
spi_idle_callback_ = std::move(spi_idle_callback);
}
- double gyro_x_zeroed_offset() const { return gyro_x_zeroed_offset_; }
- double gyro_y_zeroed_offset() const { return gyro_y_zeroed_offset_; }
- double gyro_z_zeroed_offset() const { return gyro_z_zeroed_offset_; }
-
private:
- // Initializes the sensor and then loops until Quit() is called taking
- // readings.
+ // Initializes the sensor and then takes readings until Quit() is called.
void DoRun();
// Try to initialize repeatedly as long as we're supposed to be running.
@@ -90,7 +85,6 @@
bool Initialize();
::aos::EventLoop *event_loop_;
- ::aos::Fetcher<::aos::RobotState> robot_state_fetcher_;
::aos::Sender<::frc971::IMUValues> imu_values_sender_;
// TODO(Brian): This object has no business owning these ones.
@@ -101,13 +95,9 @@
std::function<void()> spi_idle_callback_ = []() {};
- // The averaged values of the gyro over 6 seconds after power up.
- bool gyros_are_zeroed_ = false;
- double gyro_x_zeroed_offset_ = 0.0;
- double gyro_y_zeroed_offset_ = 0.0;
- double gyro_z_zeroed_offset_ = 0.0;
-
SpiRxClearer rx_clearer_;
+
+ FpgaTimeConverter time_converter_;
};
} // namespace wpilib
diff --git a/frc971/wpilib/ADIS16470.cc b/frc971/wpilib/ADIS16470.cc
new file mode 100644
index 0000000..839b82b
--- /dev/null
+++ b/frc971/wpilib/ADIS16470.cc
@@ -0,0 +1,464 @@
+#include "frc971/wpilib/ADIS16470.h"
+
+#include <inttypes.h>
+
+#include "glog/logging.h"
+
+#include "aos/time/time.h"
+#include "hal/HAL.h"
+
+namespace frc971 {
+namespace wpilib {
+namespace {
+namespace registers {
+
+// Flash memory write count
+constexpr uint8_t FLASH_CNT = 0x00;
+// Diagnostic and operational status
+constexpr uint8_t DIAG_STAT = 0x02;
+// X-axis gyroscope output, lower word
+constexpr uint8_t X_GYRO_LOW = 0x04;
+// X-axis gyroscope output, upper word
+constexpr uint8_t X_GYRO_OUT = 0x06;
+// Y-axis gyroscope output, lower word
+constexpr uint8_t Y_GYRO_LOW = 0x08;
+// Y-axis gyroscope output, upper word
+constexpr uint8_t Y_GYRO_OUT = 0x0A;
+// Z-axis gyroscope output, lower word
+constexpr uint8_t Z_GYRO_LOW = 0x0C;
+// Z-axis gyroscope output, upper word
+constexpr uint8_t Z_GYRO_OUT = 0x0E;
+// X-axis accelerometer output, lower word
+constexpr uint8_t X_ACCL_LOW = 0x10;
+// X-axis accelerometer output, upper word
+constexpr uint8_t X_ACCL_OUT = 0x12;
+// Y-axis accelerometer output, lower word
+constexpr uint8_t Y_ACCL_OUT = 0x16;
+// Y-axis accelerometer output, upper word
+constexpr uint8_t Z_ACCL_LOW = 0x18;
+// Z-axis accelerometer output, lower word
+constexpr uint8_t Z_ACCL_OUT = 0x1A;
+// Z-axis accelerometer output, upper word
+constexpr uint8_t TEMP_OUT = 0x1C;
+// Temperature output (internal, not calibrated)
+constexpr uint8_t TIME_STAMP = 0x1E;
+// PPS mode time stamp
+constexpr uint8_t X_DELTANG_LOW = 0x24;
+// X-axis delta angle output, lower word
+constexpr uint8_t X_DELTANG_OUT = 0x26;
+// X-axis delta angle output, upper word
+constexpr uint8_t Y_DELTANG_LOW = 0x28;
+// Y-axis delta angle output, lower word
+constexpr uint8_t Y_DELTANG_OUT = 0x2A;
+// Y-axis delta angle output, upper word
+constexpr uint8_t Z_DELTANG_LOW = 0x2C;
+// Z-axis delta angle output, lower word
+constexpr uint8_t Z_DELTANG_OUT = 0x2E;
+// Z-axis delta angle output, upper word
+constexpr uint8_t X_DELTVEL_LOW = 0x30;
+// X-axis delta velocity output, lower word
+constexpr uint8_t X_DELTVEL_OUT = 0x32;
+// X-axis delta velocity output, upper word
+constexpr uint8_t Y_DELTVEL_LOW = 0x34;
+// Y-axis delta velocity output, lower word
+constexpr uint8_t Y_DELTVEL_OUT = 0x36;
+// Y-axis delta velocity output, upper word
+constexpr uint8_t Z_DELTVEL_LOW = 0x38;
+// Z-axis delta velocity output, lower word
+constexpr uint8_t Z_DELTVEL_OUT = 0x3A;
+// Z-axis delta velocity output, upper word
+constexpr uint8_t XG_BIAS_LOW = 0x40;
+// X-axis gyroscope bias offset correction, lower word
+constexpr uint8_t XG_BIAS_HIGH = 0x42;
+// X-axis gyroscope bias offset correction, upper word
+constexpr uint8_t YG_BIAS_LOW = 0x44;
+// Y-axis gyroscope bias offset correction, lower word
+constexpr uint8_t YG_BIAS_HIGH = 0x46;
+// Y-axis gyroscope bias offset correction, upper word
+constexpr uint8_t ZG_BIAS_LOW = 0x48;
+// Z-axis gyroscope bias offset correction, lower word
+constexpr uint8_t ZG_BIAS_HIGH = 0x4A;
+// Z-axis gyroscope bias offset correction, upper word
+constexpr uint8_t XA_BIAS_LOW = 0x4C;
+// X-axis accelerometer bias offset correction, lower word
+constexpr uint8_t XA_BIAS_HIGH = 0x4E;
+// X-axis accelerometer bias offset correction, upper word
+constexpr uint8_t YA_BIAS_LOW = 0x50;
+// Y-axis accelerometer bias offset correction, lower word
+constexpr uint8_t YA_BIAS_HIGH = 0x52;
+// Y-axis accelerometer bias offset correction, upper word
+constexpr uint8_t ZA_BIAS_LOW = 0x54;
+// Z-axis accelerometer bias offset correction, lower word
+constexpr uint8_t ZA_BIAS_HIGH = 0x56;
+// Z-axis accelerometer bias offset correction, upper word
+constexpr uint8_t FILT_CTRL = 0x5C;
+// Filter control
+constexpr uint8_t MSC_CTRL = 0x60;
+// Miscellaneous control
+constexpr uint8_t UP_SCALE = 0x62;
+// Clock scale factor, PPS mode
+constexpr uint8_t DEC_RATE = 0x64;
+// Decimation rate control (output data rate)
+constexpr uint8_t NULL_CNFG = 0x66;
+// Auto-null configuration control
+constexpr uint8_t GLOB_CMD = 0x68;
+// Global commands
+constexpr uint8_t FIRM_REV = 0x6C;
+// Firmware revision
+constexpr uint8_t FIRM_DM = 0x6E;
+// Firmware revision date, month and day
+constexpr uint8_t FIRM_Y = 0x70;
+// Firmware revision date, year
+constexpr uint8_t PROD_ID = 0x72;
+// Product identification
+constexpr uint8_t SERIAL_NUM = 0x74;
+// Serial number (relative to assembly lot)
+constexpr uint8_t USER_SCR1 = 0x76;
+// User scratch register 1
+constexpr uint8_t USER_SCR2 = 0x78;
+// User scratch register 2
+constexpr uint8_t USER_SCR3 = 0x7A;
+// User scratch register 3
+constexpr uint8_t FLSHCNT_LOW = 0x7C;
+// Flash update count, lower word
+constexpr uint8_t FLSHCNT_HIGH = 0x7E;
+// Flash update count, upper word
+constexpr uint8_t Y_ACCL_LOW = 0x14;
+
+} // namespace registers
+
+// The complete automatic packet we will send. This needs to include the dummy 0
+// bytes making up full 16-bit frames.
+// Note that in addition to the 24-byte limit from the FPGA, this is also
+// limited to 12 16-bit register reads by the IMU itself given that we're
+// reading at the full 2kHz rate.
+// We rotate the registers here by 1, such that the first thing we read is the
+// last thing triggered by the previous reading. We put DIAG_STAT in this
+// position because we don't care if it's one cycle stale.
+constexpr uint8_t kAutospiPacket[] = {
+ // X
+ registers::X_GYRO_OUT, 0,
+ registers::X_ACCL_OUT, 0, registers::X_ACCL_LOW, 0,
+ // Y
+ registers::Y_GYRO_OUT, 0,
+ registers::Y_ACCL_OUT, 0, registers::Y_ACCL_LOW, 0,
+ // Z
+ registers::Z_GYRO_OUT, 0,
+ registers::Z_ACCL_OUT, 0, registers::Z_ACCL_LOW, 0,
+ // Other
+ registers::TEMP_OUT, 0, registers::DIAG_STAT, 0,
+};
+// clang-format on
+
+static_assert((sizeof(kAutospiPacket) % 2) == 0,
+ "Need a whole number of register reads");
+
+static constexpr size_t kAutospiDataSize = sizeof(kAutospiPacket) + 1 /* timestamp */;
+
+// radian/second/LSB for the gyros (for just the 16-bit value).
+constexpr double kGyroLsbRadianSecond =
+ 1.0 / 10.0 * (2.0 * M_PI / 360.0) /* degrees -> radians */;
+// G/LSB for the accelerometers (for the full 32-bit value).
+constexpr double kAccelerometerLsbG = 1.0 / 54'428'800.0;
+// C/LSB for the temperature.
+constexpr double kTemperatureLsbDegree = 0.1;
+
+// This is what the datasheet says PROD_ID should be.
+constexpr uint16_t kExpectedProductId = 0x4056;
+// This is the PROD_ID we observe.
+constexpr uint16_t kObservedProductId = 0x4256;
+
+} // namespace
+
+ADIS16470::ADIS16470(aos::EventLoop *event_loop, frc::SPI *spi,
+ frc::DigitalInput *data_ready, frc::DigitalOutput *reset)
+ : event_loop_(event_loop),
+ imu_values_sender_(
+ event_loop_->MakeSender<::frc971::IMUValues>("/drivetrain")),
+ initialize_timer_(
+ event_loop_->AddTimer([this]() { DoInitializeStep(); })),
+ spi_(spi),
+ data_ready_(data_ready),
+ reset_(reset) {
+ // Rather than put the entire data packet into the header, just put a size
+ // there and verify it matches here.
+ CHECK_EQ(kAutospiDataSize, read_data_.size());
+
+ // We're not doing burst mode, so this is the IMU's rated speed.
+ spi_->SetClockRate(2'000'000);
+ spi_->SetChipSelectActiveLow();
+ spi_->SetClockActiveLow();
+ spi_->SetSampleDataOnTrailingEdge();
+ spi_->SetMSBFirst();
+
+ // NI's SPI driver defaults to SCHED_OTHER. Find it's PID with ps, and change
+ // it to a RT priority of 33.
+ PCHECK(
+ system("ps -ef | grep '\\[spi0\\]' | awk '{print $1}' | xargs chrt -f -p "
+ "33") == 0);
+
+ event_loop_->OnRun([this]() { BeginInitialization(); });
+}
+
+void ADIS16470::DoReads() {
+ if (state_ != State::kRunning) {
+ // Not sure how to interpret data received now, so ignore it.
+ return;
+ }
+
+ int amount_to_read =
+ spi_->ReadAutoReceivedData(to_read_.data(), 0, 0 /* don't block */);
+ while (true) {
+ if (amount_to_read == 0) break;
+ CHECK(!to_read_.empty());
+ const int amount_read_now = std::min<int>(amount_to_read, to_read_.size());
+ CHECK_GT(amount_read_now, 0) << "amount_to_read: " << amount_to_read
+ << ", to_read_.size(): " << to_read_.size();
+ spi_->ReadAutoReceivedData(to_read_.data(), amount_read_now,
+ 0 /* don't block */);
+ to_read_ = to_read_.subspan(amount_read_now);
+ amount_to_read -= amount_read_now;
+
+ if (to_read_.empty()) {
+ ProcessReading();
+
+ // Reset for the next reading.
+ to_read_ = absl::MakeSpan(read_data_);
+ } else {
+ CHECK_EQ(amount_to_read, 0);
+ break;
+ }
+ }
+}
+
+void ADIS16470::DoInitializeStep() {
+ switch (state_) {
+ case State::kUninitialized: {
+ to_read_ = absl::MakeSpan(read_data_);
+
+ // First, set the SPI to normal mode so it stops trying to talk
+ // automatically.
+ spi_->StopAuto();
+
+ reset_->Set(false);
+ // Datasheet says it needs a 1 us pulse, so make sure we do something in
+ // between asserting and deasserting.
+ std::this_thread::sleep_for(::std::chrono::milliseconds(1));
+ reset_->Set(true);
+
+ state_ = State::kWaitForReset;
+ // Datasheet says it takes 193 ms to come out of reset, so give it some
+ // margin on top of that.
+ initialize_timer_->Setup(event_loop_->monotonic_now() +
+ std::chrono::milliseconds(250));
+ }
+ break;
+
+ case State::kWaitForReset: {
+ flatbuffers::Offset<ADIS16470DiagStat> start_diag_stat;
+ flatbuffers::Offset<ADIS16470DiagStat> self_test_diag_stat;
+ bool success = false;
+ auto builder = imu_values_sender_.MakeBuilder();
+
+ // Configure the IMU the way we want it.
+ const uint16_t product_id = ReadRegister(registers::PROD_ID, 0);
+ if (product_id == kExpectedProductId ||
+ product_id == kObservedProductId) {
+ const uint16_t start_diag_stat_value =
+ ReadRegister(registers::DIAG_STAT, 0);
+ start_diag_stat = PackDiagStat(builder.fbb(), start_diag_stat_value);
+ if (!DiagStatHasError(
+ *GetTemporaryPointer(*builder.fbb(), start_diag_stat))) {
+ WriteRegister(registers::FILT_CTRL, 0 /* no filtering */);
+ WriteRegister(
+ registers::MSC_CTRL,
+ (1 << 7) /* enable gyro linear g compensation */ |
+ (1 << 6) /* enable point of percussion alignment */ |
+ (0 << 2) /* internal clock mode */ |
+ (0 << 1) /* sync polarity, doesn't matter */ |
+ (1 << 0) /* data ready is active high */);
+ WriteRegister(registers::DEC_RATE,
+ 0 /* no internal decimation (averaging) */);
+
+ // Start a sensor self test.
+ WriteRegister(registers::GLOB_CMD, 1 << 2);
+ // Datasheet says it takes 14ms, so give it some margin.
+ std::this_thread::sleep_for(std::chrono::milliseconds(25));
+ // Read DIAG_STAT again, and queue up a read of the first part of the
+ // autospi data packet.
+ const uint16_t self_test_diag_stat_value =
+ ReadRegister(registers::DIAG_STAT, kAutospiPacket[0]);
+ self_test_diag_stat =
+ PackDiagStat(builder.fbb(), self_test_diag_stat_value);
+ if (!DiagStatHasError(
+ *GetTemporaryPointer(*builder.fbb(), self_test_diag_stat))) {
+ // Initialize automatic mode, but don't start it yet.
+ spi_->InitAuto(kAutospiDataSize * 100);
+ spi_->SetAutoTransmitData(kAutospiPacket,
+ 0 /* no extra 0s at the end */);
+ // No idea what units the "stall period" is in. This value is just
+ // bigger than the 16us min from the datasheet. It does not appear
+ // to scale with SPICLK frequency. Empirically, this value comes out
+ // to 16.7us.
+ spi_->ConfigureAutoStall(
+ HAL_SPI_kOnboardCS0,
+ 0 /* the minimum CS delay is enough for this IMU */, 670,
+ 1 /* toggle CS every 2 8-bit bytes */);
+
+ // Read any data queued up by the FPGA.
+ while (true){
+ uint32_t buffer;
+ if (spi_->ReadAutoReceivedData(&buffer, 1, 0 /* don't block */) ==
+ 0) {
+ break;
+ }
+ }
+
+ // Finally, enable automatic mode so it starts reading data.
+ spi_->StartAutoTrigger(*data_ready_, true, false);
+ success = true;
+ }
+ }
+ }
+
+ IMUValues::Builder imu_builder = builder.MakeBuilder<IMUValues>();
+ imu_builder.add_product_id(product_id);
+ if (!start_diag_stat.IsNull()) {
+ imu_builder.add_start_diag_stat(start_diag_stat);
+ }
+ if (!self_test_diag_stat.IsNull()) {
+ imu_builder.add_self_test_diag_stat(self_test_diag_stat);
+ }
+ builder.Send(imu_builder.Finish());
+ if (success) {
+ state_ = State::kRunning;
+ } else {
+ BeginInitialization();
+ }
+ }
+ break;
+
+ case State::kRunning:
+ LOG(FATAL) << "Not a reset state";
+ }
+}
+
+void ADIS16470::ProcessReading() {
+ // If we ever see this, we'll need to decide how to handle it. Probably reset
+ // everything and try again.
+ CHECK_EQ(0, spi_->GetAutoDroppedCount());
+
+ auto builder = imu_values_sender_.MakeBuilder();
+
+ absl::Span<const uint32_t> to_process = read_data_;
+ hal::fpga_clock::time_point fpga_time;
+ {
+ int32_t status = 0;
+ const uint64_t fpga_expanded = HAL_ExpandFPGATime(to_process[0], &status);
+ CHECK_EQ(0, status);
+ fpga_time =
+ hal::fpga_clock::time_point(hal::fpga_clock::duration(fpga_expanded));
+ }
+ to_process = to_process.subspan(1);
+
+ const uint16_t diag_stat_value = (static_cast<uint16_t>(to_process[0]) << 8) |
+ static_cast<uint16_t>(to_process[1]);
+ const auto diag_stat = PackDiagStat(builder.fbb(), diag_stat_value);
+ to_process = to_process.subspan(2);
+
+ IMUValues::Builder imu_builder = builder.MakeBuilder<IMUValues>();
+ imu_builder.add_fpga_timestamp(
+ aos::time::DurationInSeconds(fpga_time.time_since_epoch()));
+ imu_builder.add_monotonic_timestamp_ns(
+ time_converter_.FpgaToMonotonic(fpga_time).time_since_epoch().count());
+ imu_builder.add_previous_reading_diag_stat(diag_stat);
+
+ imu_builder.add_gyro_x(ConvertValue16(to_process, kGyroLsbRadianSecond));
+ to_process = to_process.subspan(2);
+ imu_builder.add_accelerometer_x(
+ ConvertValue32(to_process, kAccelerometerLsbG));
+ to_process = to_process.subspan(4);
+ imu_builder.add_gyro_y(ConvertValue16(to_process, kGyroLsbRadianSecond));
+ to_process = to_process.subspan(2);
+ imu_builder.add_accelerometer_y(
+ ConvertValue32(to_process, kAccelerometerLsbG));
+ to_process = to_process.subspan(4);
+ imu_builder.add_gyro_z(ConvertValue16(to_process, kGyroLsbRadianSecond));
+ to_process = to_process.subspan(2);
+ imu_builder.add_accelerometer_z(
+ ConvertValue32(to_process, kAccelerometerLsbG));
+ to_process = to_process.subspan(4);
+
+ imu_builder.add_temperature(
+ ConvertValue16(to_process, kTemperatureLsbDegree));
+ to_process = to_process.subspan(2);
+
+ CHECK(to_process.empty()) << "Have leftover bytes: " << to_process.size();
+
+ builder.Send(imu_builder.Finish());
+}
+
+double ADIS16470::ConvertValue32(absl::Span<const uint32_t> data,
+ double lsb_per_output) {
+ const uint32_t unsigned_value = (static_cast<uint32_t>(data[0]) << 24) |
+ (static_cast<uint32_t>(data[1]) << 16) |
+ (static_cast<uint32_t>(data[2]) << 8) |
+ static_cast<uint32_t>(data[3]);
+ int32_t signed_value;
+ memcpy(&signed_value, &unsigned_value, sizeof(unsigned_value));
+ return static_cast<double>(signed_value) * lsb_per_output;
+}
+
+double ADIS16470::ConvertValue16(absl::Span<const uint32_t> data,
+ double lsb_per_output) {
+ const uint16_t unsigned_value =
+ (static_cast<uint16_t>(data[0]) << 8) | static_cast<uint16_t>(data[1]);
+ int16_t signed_value;
+ memcpy(&signed_value, &unsigned_value, sizeof(unsigned_value));
+ return static_cast<double>(signed_value) * lsb_per_output;
+}
+
+flatbuffers::Offset<ADIS16470DiagStat> ADIS16470::PackDiagStat(
+ flatbuffers::FlatBufferBuilder *fbb, uint16_t value) {
+ ADIS16470DiagStat::Builder diag_stat_builder(*fbb);
+ diag_stat_builder.add_clock_error(value & (1 << 7));
+ diag_stat_builder.add_memory_failure(value & (1 << 6));
+ diag_stat_builder.add_sensor_failure(value & (1 << 5));
+ diag_stat_builder.add_standby_mode(value & (1 << 4));
+ diag_stat_builder.add_spi_communication_error(value & (1 << 3));
+ diag_stat_builder.add_flash_memory_update_error(value & (1 << 2));
+ diag_stat_builder.add_data_path_overrun(value & (1 << 1));
+ return diag_stat_builder.Finish();
+}
+
+bool ADIS16470::DiagStatHasError(const ADIS16470DiagStat &diag_stat) {
+ return diag_stat.clock_error() || diag_stat.memory_failure() ||
+ diag_stat.sensor_failure() || diag_stat.standby_mode() ||
+ diag_stat.spi_communication_error() ||
+ diag_stat.flash_memory_update_error() || diag_stat.data_path_overrun();
+}
+
+uint16_t ADIS16470::ReadRegister(uint8_t register_address,
+ uint8_t next_register_address) {
+ uint8_t send_buffer[2] = {static_cast<uint8_t>(register_address & 0x7f), 0};
+ uint8_t dummy[2];
+ spi_->Transaction(send_buffer, dummy, sizeof(send_buffer));
+ uint8_t receive_buffer[2];
+ uint8_t next_send_buffer[2] = {
+ static_cast<uint8_t>(next_register_address & 0x7f), 0};
+ spi_->Transaction(next_send_buffer, receive_buffer, sizeof(receive_buffer));
+ return (static_cast<uint16_t>(receive_buffer[0]) << 8) |
+ static_cast<uint16_t>(receive_buffer[1]);
+}
+
+void ADIS16470::WriteRegister(uint8_t register_address, uint16_t value) {
+ uint8_t buffer1[2] = {static_cast<uint8_t>(register_address | 0x80),
+ static_cast<uint8_t>(value & 0xff)};
+ uint8_t buffer2[2] = {static_cast<uint8_t>(register_address | 0x81),
+ static_cast<uint8_t>(value >> 8)};
+ spi_->Write(buffer1, sizeof(buffer1));
+ spi_->Write(buffer2, sizeof(buffer2));
+}
+
+} // namespace wpilib
+} // namespace frc971
diff --git a/frc971/wpilib/ADIS16470.h b/frc971/wpilib/ADIS16470.h
new file mode 100644
index 0000000..4c9917f
--- /dev/null
+++ b/frc971/wpilib/ADIS16470.h
@@ -0,0 +1,97 @@
+#ifndef FRC971_WPILIB_ADIS16470_H_
+#define FRC971_WPILIB_ADIS16470_H_
+
+#include "absl/types/span.h"
+
+#include "aos/events/event_loop.h"
+#include "frc971/wpilib/ahal/DigitalInput.h"
+#include "frc971/wpilib/ahal/DigitalOutput.h"
+#include "frc971/wpilib/ahal/DigitalSource.h"
+#include "frc971/wpilib/ahal/SPI.h"
+#include "frc971/wpilib/fpga_time_conversion.h"
+#include "frc971/wpilib/imu_generated.h"
+
+namespace frc971 {
+namespace wpilib {
+
+// Handles interfacing with an Analog Devices ADIS16470 over SPI and sending the
+// resulting values out on a channel.
+//
+// This relies on the AutoRead functionality in the FPGA to read values when
+// data is ready. It then allows the FPGA to buffer those until right before the
+// relevant control loops run, at which point they are all sent out on the
+// relevant channel.
+class ADIS16470 {
+ public:
+ // event_loop's thread will be hijacked before processing any events.
+ // spi is how to talk to the sensor over SPI.
+ // data_ready is the Data Ready (DR) pin (J6).
+ // reset is the Reset (RST) pin (F3).
+ ADIS16470(aos::EventLoop *event_loop, frc::SPI *spi,
+ frc::DigitalInput *data_ready, frc::DigitalOutput *reset);
+
+ ADIS16470(const ADIS16470 &) = delete;
+ ADIS16470 &operator=(const ADIS16470 &) = delete;
+
+ // Reads all the queued-up data and sends out any complete readings.
+ void DoReads();
+
+ private:
+ enum class State {
+ kUninitialized,
+ kWaitForReset,
+ kRunning,
+ };
+
+ // Performs one (non-blocking) initialization step.
+ void DoInitializeStep();
+
+ // Processes a complete reading in read_data_.
+ void ProcessReading();
+
+ // Converts a 32-bit value at data to a scaled output value where a value of 1
+ // corresponds to lsb_per_output.
+ static double ConvertValue32(absl::Span<const uint32_t> data,
+ double lsb_per_output);
+ static double ConvertValue16(absl::Span<const uint32_t> data,
+ double lsb_per_output);
+
+ static flatbuffers::Offset<ADIS16470DiagStat> PackDiagStat(
+ flatbuffers::FlatBufferBuilder *fbb, uint16_t value);
+
+ static bool DiagStatHasError(const ADIS16470DiagStat &diag_stat);
+
+ // These may only be called during configuration, when spi_ is not in
+ // automatic mode.
+ uint16_t ReadRegister(uint8_t register_address,
+ uint8_t next_register_address);
+ void WriteRegister(uint8_t register_address, uint16_t value);
+
+ void BeginInitialization() {
+ state_ = State::kUninitialized;
+ initialize_timer_->Setup(event_loop_->monotonic_now() +
+ std::chrono::milliseconds(25));
+ }
+
+ aos::EventLoop *const event_loop_;
+ aos::Sender<::frc971::IMUValues> imu_values_sender_;
+ aos::TimerHandler *const initialize_timer_;
+
+ frc::SPI *const spi_;
+ frc::DigitalInput *const data_ready_;
+ frc::DigitalOutput *const reset_;
+
+ State state_ = State::kUninitialized;
+
+ // Data we've read from the FPGA.
+ std::array<uint32_t, 23> read_data_;
+ // Data that we need to read from the FPGA to get a complete reading.
+ absl::Span<uint32_t> to_read_;
+
+ FpgaTimeConverter time_converter_;
+};
+
+} // namespace wpilib
+} // namespace frc971
+
+#endif // FRC971_WPILIB_ADIS16470_H_
diff --git a/frc971/wpilib/BUILD b/frc971/wpilib/BUILD
index 48fda81..18a1775 100644
--- a/frc971/wpilib/BUILD
+++ b/frc971/wpilib/BUILD
@@ -89,6 +89,8 @@
hdrs = [
"gyro_interface.h",
],
+ # This library uses some deprecated parts of the SPI API.
+ copts = ["-Wno-deprecated-declarations"],
restricted_to = ["//tools:roborio"],
deps = [
"//aos/logging",
@@ -265,6 +267,26 @@
)
cc_library(
+ name = "ADIS16470",
+ srcs = [
+ "ADIS16470.cc",
+ ],
+ hdrs = [
+ "ADIS16470.h",
+ ],
+ restricted_to = ["//tools:roborio"],
+ deps = [
+ ":fpga_time_conversion",
+ ":imu_fbs",
+ "//aos/events:event_loop",
+ "//aos/time",
+ "//third_party:wpilib",
+ "@com_github_google_glog//:glog",
+ "@com_google_absl//absl/types:span",
+ ],
+)
+
+cc_library(
name = "ADIS16448",
srcs = [
"ADIS16448.cc",
@@ -272,15 +294,17 @@
hdrs = [
"ADIS16448.h",
],
+ # This library uses some deprecated parts of the SPI API.
+ copts = ["-Wno-deprecated-declarations"],
restricted_to = ["//tools:roborio"],
deps = [
+ ":fpga_time_conversion",
":imu_fbs",
":spi_rx_clearer",
"//aos:init",
"//aos/events:event_loop",
"//aos/events:shm_event_loop",
"//aos/logging",
- "//aos/robot_state:robot_state_fbs",
"//aos/time",
"//frc971/zeroing:averager",
"//third_party:wpilib",
@@ -327,6 +351,7 @@
":dma",
":dma_edge_counting",
":encoder_and_potentiometer",
+ ":fpga_time_conversion",
":wpilib_interface",
"//aos:init",
"//aos/events:event_loop",
@@ -356,3 +381,20 @@
"//third_party:wpilib",
],
)
+
+cc_library(
+ name = "fpga_time_conversion",
+ srcs = [
+ "fpga_time_conversion.cc",
+ ],
+ hdrs = [
+ "fpga_time_conversion.h",
+ ],
+ restricted_to = ["//tools:roborio"],
+ deps = [
+ "//aos/time",
+ "//aos/util:compiler_memory_barrier",
+ "//third_party:wpilib",
+ "@com_github_google_glog//:glog",
+ ],
+)
diff --git a/frc971/wpilib/ahal/SPI.cc b/frc971/wpilib/ahal/SPI.cc
index 00e7dc9..2e8a2fc 100644
--- a/frc971/wpilib/ahal/SPI.cc
+++ b/frc971/wpilib/ahal/SPI.cc
@@ -1,90 +1,93 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2008-2017. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
-#include "hal/SPI.h"
#include "frc971/wpilib/ahal/SPI.h"
#include <cstring>
+#include <utility>
-#include "hal/HAL.h"
-#include "wpi/SmallVector.h"
+#include <hal/SPI.h>
+#include <wpi/SmallVector.h>
+#include <wpi/mutex.h>
-using namespace frc;
+#include "frc971/wpilib/ahal/DigitalSource.h"
+#include "frc971/wpilib/ahal/WPIErrors.h"
-#define HAL_FATAL_WITH_STATUS(status)
+namespace frc {
-SPI::SPI(Port SPIport) {
-#ifdef WPILIB2017
- m_port = SPIport;
-#else
- m_port = static_cast<HAL_SPIPort>(SPIport);
-#endif
+SPI::SPI(Port port) : m_port(static_cast<HAL_SPIPort>(port)) {
int32_t status = 0;
HAL_InitializeSPI(m_port, &status);
- HAL_FATAL_WITH_STATUS(status);
-
- static int instances = 0;
- instances++;
- HAL_Report(HALUsageReporting::kResourceType_SPI, instances);
+ wpi_setHALError(status);
}
SPI::~SPI() { HAL_CloseSPI(m_port); }
-void SPI::SetClockRate(double hz) { HAL_SetSPISpeed(m_port, hz); }
+void SPI::SetClockRate(int hz) { HAL_SetSPISpeed(m_port, hz); }
void SPI::SetMSBFirst() {
m_msbFirst = true;
- HAL_SetSPIOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clk_idle_high);
+ HAL_SetSPIOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clockIdleHigh);
}
void SPI::SetLSBFirst() {
m_msbFirst = false;
- HAL_SetSPIOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clk_idle_high);
+ HAL_SetSPIOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clockIdleHigh);
+}
+
+void SPI::SetSampleDataOnLeadingEdge() {
+ m_sampleOnTrailing = false;
+ HAL_SetSPIOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clockIdleHigh);
+}
+
+void SPI::SetSampleDataOnTrailingEdge() {
+ m_sampleOnTrailing = true;
+ HAL_SetSPIOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clockIdleHigh);
}
void SPI::SetSampleDataOnFalling() {
m_sampleOnTrailing = true;
- HAL_SetSPIOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clk_idle_high);
+ HAL_SetSPIOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clockIdleHigh);
}
void SPI::SetSampleDataOnRising() {
m_sampleOnTrailing = false;
- HAL_SetSPIOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clk_idle_high);
+ HAL_SetSPIOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clockIdleHigh);
}
void SPI::SetClockActiveLow() {
- m_clk_idle_high = true;
- HAL_SetSPIOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clk_idle_high);
+ m_clockIdleHigh = true;
+ HAL_SetSPIOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clockIdleHigh);
}
void SPI::SetClockActiveHigh() {
- m_clk_idle_high = false;
- HAL_SetSPIOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clk_idle_high);
+ m_clockIdleHigh = false;
+ HAL_SetSPIOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clockIdleHigh);
}
void SPI::SetChipSelectActiveHigh() {
int32_t status = 0;
HAL_SetSPIChipSelectActiveHigh(m_port, &status);
- HAL_FATAL_WITH_STATUS(status);
+ wpi_setHALError(status);
}
void SPI::SetChipSelectActiveLow() {
int32_t status = 0;
HAL_SetSPIChipSelectActiveLow(m_port, &status);
- HAL_FATAL_WITH_STATUS(status);
+ wpi_setHALError(status);
}
-int SPI::Write(uint8_t *data, int size) {
+int SPI::Write(uint8_t* data, int size) {
int retVal = 0;
retVal = HAL_WriteSPI(m_port, data, size);
return retVal;
}
-int SPI::Read(bool initiate, uint8_t *dataReceived, int size) {
+int SPI::Read(bool initiate, uint8_t* dataReceived, int size) {
int retVal = 0;
if (initiate) {
wpi::SmallVector<uint8_t, 32> dataToSend;
@@ -96,8 +99,79 @@
return retVal;
}
-int SPI::Transaction(uint8_t *dataToSend, uint8_t *dataReceived, int size) {
+int SPI::Transaction(uint8_t* dataToSend, uint8_t* dataReceived, int size) {
int retVal = 0;
retVal = HAL_TransactionSPI(m_port, dataToSend, dataReceived, size);
return retVal;
}
+
+void SPI::InitAuto(int bufferSize) {
+ int32_t status = 0;
+ HAL_InitSPIAuto(m_port, bufferSize, &status);
+ wpi_setHALError(status);
+}
+
+void SPI::FreeAuto() {
+ int32_t status = 0;
+ HAL_FreeSPIAuto(m_port, &status);
+ wpi_setHALError(status);
+}
+
+void SPI::SetAutoTransmitData(wpi::ArrayRef<uint8_t> dataToSend, int zeroSize) {
+ int32_t status = 0;
+ HAL_SetSPIAutoTransmitData(m_port, dataToSend.data(), dataToSend.size(),
+ zeroSize, &status);
+ wpi_setHALError(status);
+}
+
+void SPI::StartAutoRate(double period) {
+ int32_t status = 0;
+ HAL_StartSPIAutoRate(m_port, period, &status);
+ wpi_setHALError(status);
+}
+
+void SPI::StartAutoTrigger(DigitalSource& source, bool rising, bool falling) {
+ int32_t status = 0;
+ HAL_StartSPIAutoTrigger(
+ m_port, source.GetPortHandleForRouting(),
+ (HAL_AnalogTriggerType)source.GetAnalogTriggerTypeForRouting(), rising,
+ falling, &status);
+ wpi_setHALError(status);
+}
+
+void SPI::StopAuto() {
+ int32_t status = 0;
+ HAL_StopSPIAuto(m_port, &status);
+ wpi_setHALError(status);
+}
+
+void SPI::ForceAutoRead() {
+ int32_t status = 0;
+ HAL_ForceSPIAutoRead(m_port, &status);
+ wpi_setHALError(status);
+}
+
+int SPI::ReadAutoReceivedData(uint32_t *buffer, int numToRead, double timeout) {
+ int32_t status = 0;
+ int32_t val = HAL_ReadSPIAutoReceivedData(m_port, buffer, numToRead,
+ timeout, &status);
+ wpi_setHALError(status);
+ return val;
+}
+
+int SPI::GetAutoDroppedCount() {
+ int32_t status = 0;
+ int32_t val = HAL_GetSPIAutoDroppedCount(m_port, &status);
+ wpi_setHALError(status);
+ return val;
+}
+
+void SPI::ConfigureAutoStall(HAL_SPIPort /*port*/, int csToSclkTicks,
+ int stallTicks, int pow2BytesPerRead) {
+ int32_t status = 0;
+ HAL_ConfigureSPIAutoStall(m_port, csToSclkTicks, stallTicks, pow2BytesPerRead,
+ &status);
+ wpi_setHALError(status);
+}
+
+} // namespace frc
diff --git a/frc971/wpilib/ahal/SPI.h b/frc971/wpilib/ahal/SPI.h
index 898f426..9656f55 100644
--- a/frc971/wpilib/ahal/SPI.h
+++ b/frc971/wpilib/ahal/SPI.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2008-2017. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,12 +7,17 @@
#pragma once
-#include "hal/SPI.h"
+#include <stdint.h>
+
+#include <memory>
+
+#include <hal/SPITypes.h>
+#include <wpi/ArrayRef.h>
+#include <wpi/deprecated.h>
namespace frc {
-class DigitalOutput;
-class DigitalInput;
+class DigitalSource;
/**
* SPI bus interface class.
@@ -21,91 +26,230 @@
* It probably should not be used directly.
*
*/
-class SPI {
+class SPI final {
public:
- enum Port : int32_t {
- kOnboardCS0 = 0,
- kOnboardCS1,
- kOnboardCS2,
- kOnboardCS3,
- kMXP
- };
- explicit SPI(Port SPIport);
- virtual ~SPI();
+ enum Port { kOnboardCS0 = 0, kOnboardCS1, kOnboardCS2, kOnboardCS3, kMXP };
- SPI(const SPI &) = delete;
- SPI &operator=(const SPI &) = delete;
+ /**
+ * Constructor
+ *
+ * @param port the physical SPI port
+ */
+ explicit SPI(Port port);
- // Configure the rate of the generated clock signal.
- //
- // The claimed default value is 500,000Hz, and the claimed maximum value is
- // 4,000,000Hz.
- //
- // This appears to have a very inflexible clocking setup. You can get 0.781MHz
- // or 1.563MHz, but nothing in between. At least it rounds down the requested
- // value like it should... 0.781MHz also appears to be the minimum.
- void SetClockRate(double hz);
+ ~SPI();
- // Configure the order that bits are sent and received on the wire
- // to be most significant bit first.
+ SPI(SPI&&) = default;
+ SPI& operator=(SPI&&) = default;
+
+ /**
+ * Configure the rate of the generated clock signal.
+ *
+ * The default value is 500,000Hz.
+ * The maximum value is 4,000,000Hz.
+ *
+ * @param hz The clock rate in Hertz.
+ */
+ void SetClockRate(int hz);
+
+ /**
+ * Configure the order that bits are sent and received on the wire
+ * to be most significant bit first.
+ */
void SetMSBFirst();
- // Configure the order that bits are sent and received on the wire
- // to be least significant bit first.
+
+ /**
+ * Configure the order that bits are sent and received on the wire
+ * to be least significant bit first.
+ */
void SetLSBFirst();
- // Configure that the data is stable on the falling edge and the data
- // changes on the rising edge.
+ /**
+ * Configure that the data is stable on the leading edge and the data
+ * changes on the trailing edge.
+ */
+ void SetSampleDataOnLeadingEdge();
+
+ /**
+ * Configure that the data is stable on the trailing edge and the data
+ * changes on the leading edge.
+ */
+ void SetSampleDataOnTrailingEdge();
+
+ /**
+ * Configure that the data is stable on the falling edge and the data
+ * changes on the rising edge.
+ */
+ WPI_DEPRECATED("Use SetSampleDataOnTrailingEdge in most cases.")
void SetSampleDataOnFalling();
- // Configure that the data is stable on the rising edge and the data
- // changes on the falling edge.
+
+ /**
+ * Configure that the data is stable on the rising edge and the data
+ * changes on the falling edge.
+ */
+ WPI_DEPRECATED("Use SetSampleDataOnLeadingEdge in most cases")
void SetSampleDataOnRising();
- // Configure the clock output line to be active low.
- // This is sometimes called clock polarity high or clock idle high.
+ /**
+ * Configure the clock output line to be active low.
+ * This is sometimes called clock polarity high or clock idle high.
+ */
void SetClockActiveLow();
- // Configure the clock output line to be active high.
- // This is sometimes called clock polarity low or clock idle low.
+
+ /**
+ * Configure the clock output line to be active high.
+ * This is sometimes called clock polarity low or clock idle low.
+ */
void SetClockActiveHigh();
- // Configure the chip select line to be active high.
+ /**
+ * Configure the chip select line to be active high.
+ */
void SetChipSelectActiveHigh();
- // Configure the chip select line to be active low.
+
+ /**
+ * Configure the chip select line to be active low.
+ */
void SetChipSelectActiveLow();
- // Write data to the slave device. Blocks until there is space in the
- // output FIFO.
- //
- // If not running in output only mode, also saves the data received
- // on the MISO input during the transfer into the receive FIFO.
- int Write(uint8_t *data, int size);
- // Read a word from the receive FIFO.
- //
- // Waits for the current transfer to complete if the receive FIFO is empty.
- //
- // If the receive FIFO is empty, there is no active transfer, and initiate
- // is false, errors.
- //
- // @param initiate If true, this function pushes "0" into the transmit buffer
- // and initiates a transfer. If false, this function assumes
- // that data is already in the receive FIFO from a previous
- // write.
- int Read(bool initiate, uint8_t *dataReceived, int size);
- // Perform a simultaneous read/write transaction with the device
- //
- // @param dataToSend The data to be written out to the device
- // @param dataReceived Buffer to receive data from the device
- // @param size The length of the transaction, in bytes
- int Transaction(uint8_t *dataToSend, uint8_t *dataReceived, int size);
+ /**
+ * Write data to the slave device. Blocks until there is space in the
+ * output FIFO.
+ *
+ * If not running in output only mode, also saves the data received
+ * on the MISO input during the transfer into the receive FIFO.
+ */
+ virtual int Write(uint8_t* data, int size);
+
+ /**
+ * Read a word from the receive FIFO.
+ *
+ * Waits for the current transfer to complete if the receive FIFO is empty.
+ *
+ * If the receive FIFO is empty, there is no active transfer, and initiate
+ * is false, errors.
+ *
+ * @param initiate If true, this function pushes "0" into the transmit buffer
+ * and initiates a transfer. If false, this function assumes
+ * that data is already in the receive FIFO from a previous
+ * write.
+ */
+ virtual int Read(bool initiate, uint8_t* dataReceived, int size);
+
+ /**
+ * Perform a simultaneous read/write transaction with the device
+ *
+ * @param dataToSend The data to be written out to the device
+ * @param dataReceived Buffer to receive data from the device
+ * @param size The length of the transaction, in bytes
+ */
+ virtual int Transaction(uint8_t* dataToSend, uint8_t* dataReceived, int size);
+
+ /**
+ * Initialize automatic SPI transfer engine.
+ *
+ * Only a single engine is available, and use of it blocks use of all other
+ * chip select usage on the same physical SPI port while it is running.
+ *
+ * @param bufferSize buffer size in bytes
+ */
+ void InitAuto(int bufferSize);
+
+ /**
+ * Frees the automatic SPI transfer engine.
+ */
+ void FreeAuto();
+
+ /**
+ * Set the data to be transmitted by the engine.
+ *
+ * Up to 16 bytes are configurable, and may be followed by up to 127 zero
+ * bytes.
+ *
+ * @param dataToSend data to send (maximum 16 bytes)
+ * @param zeroSize number of zeros to send after the data
+ */
+ void SetAutoTransmitData(wpi::ArrayRef<uint8_t> dataToSend, int zeroSize);
+
+ /**
+ * Start running the automatic SPI transfer engine at a periodic rate.
+ *
+ * InitAuto() and SetAutoTransmitData() must be called before calling this
+ * function.
+ *
+ * @param period period between transfers, in seconds (us resolution)
+ */
+ void StartAutoRate(double period);
+
+ /**
+ * Start running the automatic SPI transfer engine when a trigger occurs.
+ *
+ * InitAuto() and SetAutoTransmitData() must be called before calling this
+ * function.
+ *
+ * @param source digital source for the trigger (may be an analog trigger)
+ * @param rising trigger on the rising edge
+ * @param falling trigger on the falling edge
+ */
+ void StartAutoTrigger(DigitalSource& source, bool rising, bool falling);
+
+ /**
+ * Stop running the automatic SPI transfer engine.
+ */
+ void StopAuto();
+
+ /**
+ * Force the engine to make a single transfer.
+ */
+ void ForceAutoRead();
+
+ /**
+ * Read data that has been transferred by the automatic SPI transfer engine.
+ *
+ * Transfers may be made a byte at a time, so it's necessary for the caller
+ * to handle cases where an entire transfer has not been completed.
+ *
+ * Each received data sequence consists of a timestamp followed by the
+ * received data bytes, one byte per word (in the least significant byte).
+ * The length of each received data sequence is the same as the combined
+ * size of the data and zeroSize set in SetAutoTransmitData().
+ *
+ * Blocks until numToRead words have been read or timeout expires.
+ * May be called with numToRead=0 to retrieve how many words are available.
+ *
+ * @param buffer buffer where read words are stored
+ * @param numToRead number of words to read
+ * @param timeout timeout in seconds (ms resolution)
+ * @return Number of words remaining to be read
+ */
+ int ReadAutoReceivedData(uint32_t* buffer, int numToRead, double timeout);
+
+ /**
+ * Get the number of bytes dropped by the automatic SPI transfer engine due
+ * to the receive buffer being full.
+ *
+ * @return Number of bytes dropped
+ */
+ int GetAutoDroppedCount();
+
+ /**
+ * Configure the Auto SPI Stall time between reads.
+ *
+ * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for
+ * MXP.
+ * @param csToSclkTicks the number of ticks to wait before asserting the cs
+ * pin
+ * @param stallTicks the number of ticks to stall for
+ * @param pow2BytesPerRead the number of bytes to read before stalling
+ */
+ void ConfigureAutoStall(HAL_SPIPort port, int csToSclkTicks, int stallTicks,
+ int pow2BytesPerRead);
protected:
-#ifdef WPILIB2017
- int m_port;
-#else
- HAL_SPIPort m_port;
-#endif
- bool m_msbFirst = false; // default little-endian
- bool m_sampleOnTrailing = false; // default data updated on falling edge
- bool m_clk_idle_high = false; // default clock active high
+ hal::SPIPort m_port;
+ bool m_msbFirst = false; // Default little-endian
+ bool m_sampleOnTrailing = false; // Default data updated on falling edge
+ bool m_clockIdleHigh = false; // Default clock active high
private:
void Init();
diff --git a/frc971/wpilib/fpga_time_conversion.cc b/frc971/wpilib/fpga_time_conversion.cc
new file mode 100644
index 0000000..6f9a267
--- /dev/null
+++ b/frc971/wpilib/fpga_time_conversion.cc
@@ -0,0 +1,36 @@
+#include "frc971/wpilib/fpga_time_conversion.h"
+
+#include "aos/util/compiler_memory_barrier.h"
+
+namespace frc971 {
+namespace wpilib {
+
+std::optional<std::chrono::nanoseconds> CalculateFpgaOffset() {
+ aos_compiler_memory_barrier();
+ const hal::fpga_clock::time_point fpga_time_before = hal::fpga_clock::now();
+ aos_compiler_memory_barrier();
+ const aos::monotonic_clock::time_point monotonic_now =
+ aos::monotonic_clock::now();
+ aos_compiler_memory_barrier();
+ const hal::fpga_clock::time_point fpga_time_after = hal::fpga_clock::now();
+ aos_compiler_memory_barrier();
+
+ const std::chrono::nanoseconds fpga_sample_length =
+ fpga_time_after - fpga_time_before;
+
+ if (fpga_sample_length < fpga_sample_length.zero()) {
+ return std::nullopt;
+ }
+ if (fpga_sample_length > std::chrono::microseconds(20)) {
+ return std::nullopt;
+ }
+
+ const std::chrono::nanoseconds fpga_average =
+ (fpga_time_after.time_since_epoch() +
+ fpga_time_before.time_since_epoch()) /
+ 2;
+ return monotonic_now.time_since_epoch() - fpga_average;
+}
+
+} // namespace wpilib
+} // namespace frc971
diff --git a/frc971/wpilib/fpga_time_conversion.h b/frc971/wpilib/fpga_time_conversion.h
new file mode 100644
index 0000000..9862918
--- /dev/null
+++ b/frc971/wpilib/fpga_time_conversion.h
@@ -0,0 +1,56 @@
+#ifndef FRC971_WPILIB_FPGA_TIME_CONVERSION_H_
+#define FRC971_WPILIB_FPGA_TIME_CONVERSION_H_
+
+#include <optional>
+#include <chrono>
+
+#include "aos/time/time.h"
+#include "glog/logging.h"
+#include "hal/cpp/fpga_clock.h"
+
+namespace frc971 {
+namespace wpilib {
+
+// Returns the offset from the monotonic clock to the FPGA time. This is defined
+// as `fpga_time - monotonic_time`.
+// Returns nullopt if sampling the time once failed.
+std::optional<std::chrono::nanoseconds> CalculateFpgaOffset();
+
+class FpgaTimeConverter {
+ public:
+ aos::monotonic_clock::time_point FpgaToMonotonic(
+ hal::fpga_clock::time_point fpga_time) {
+ UpdateOffset();
+ return aos::monotonic_clock::epoch() +
+ (fpga_time.time_since_epoch() + offset_);
+ }
+
+ hal::fpga_clock::time_point MonotonicToFpga(
+ aos::monotonic_clock::time_point monotonic_time) {
+ UpdateOffset();
+ return hal::fpga_clock::epoch() +
+ std::chrono::duration_cast<hal::fpga_clock::duration>(
+ monotonic_time.time_since_epoch() - offset_);
+ }
+
+ private:
+ void UpdateOffset() {
+ for (int i = 0; i < 10; ++i) {
+ const auto new_offset = CalculateFpgaOffset();
+ if (new_offset) {
+ offset_ = *new_offset;
+ return;
+ } else if (offset_ != offset_.min()) {
+ return;
+ }
+ }
+ LOG(FATAL) << "Failed to calculate FPGA offset";
+ }
+
+ std::chrono::nanoseconds offset_ = std::chrono::nanoseconds::min();
+};
+
+} // namespace wpilib
+} // namespace frc971
+
+#endif // FRC971_WPILIB_FPGA_TIME_CONVERSION_H_
diff --git a/frc971/wpilib/imu.fbs b/frc971/wpilib/imu.fbs
index f48f31f..65b181b 100644
--- a/frc971/wpilib/imu.fbs
+++ b/frc971/wpilib/imu.fbs
@@ -1,7 +1,55 @@
namespace frc971;
+// The values in the DIAG_STAT register for the ADIS16470.
+table ADIS16470DiagStat {
+ // True indicates that the internal data sampling clock (fSM, see Figure 15
+ // and Figure 16) does not synchronize with the external clock, which only
+ // applies when using scale sync mode (Register MSC_CTRL, Bits[4:2] = 010, see
+ // Table 101). When this occurs, adjust the frequency of the clock signal on
+ // the SYNC pin to operate within the appropriate range.
+ clock_error:bool;
+
+ // True indicates a failure in the flash memory test (Register GLOB_CMD, Bit
+ // 4, see Table 109), which involves a comparison between a cyclic redundancy
+ // check (CRC) computation of the present flash memory and a CRC computation
+ // from the same memory locations at the time of initial programming (during
+ // production process). If this occurs, repeat the same test. If this error
+ // persists, replace the ADIS16470 device.
+ memory_failure:bool;
+
+ // True indicates failure of at least one sensor, at the conclusion of the
+ // self test (Register GLOB_CMD, Bit 2, see Table 109). If this occurs, repeat
+ // the same test. If this error persists, replace the ADIS16470. Motion,
+ // during the execution of this test, can cause a false failure.
+ sensor_failure:bool;
+
+ // True indicates that the voltage across VDD and GND is <2.8 V, which causes
+ // data processing to stop. When VDD ≥ 2.8 V for 250 ms, the ADIS16470
+ // reinitializes itself and starts producing data again.
+ standby_mode:bool;
+
+ // True indicates that the total number of SCLK cycles is not equal to an
+ // integer multiple of 16. When this occurs, repeat the previous communication
+ // sequence. Persistence in this error may indicate a weakness in the SPI
+ // service that the ADIS16470 is receiving from the system it is supporting.
+ spi_communication_error:bool;
+
+ // True indicates that the most recent flash memory update (Register GLOB_CMD,
+ // Bit 3, see Table 109) failed. If this occurs, ensure that VDD ≥ 3 V and
+ // repeat the update attempt. If this error persists, replace the ADIS16470.
+ flash_memory_update_error:bool;
+
+ // True indicates that one of the data paths have experienced an overrun
+ // condition. If this occurs, initiate a reset, using the RST pin (see Table
+ // 5, Pin F3) or Register GLOB_CMD, Bit 7 (see Table 109). See the Serial Port
+ // Operation section for more details on conditions that may cause this bit to
+ // be set to 1.
+ data_path_overrun:bool;
+}
+
// Values returned from an IMU.
-// Published on ".frc971.imu_values"
+// All of these are raw from the hardware, without any form of zeroing or
+// temperature compensation applied.
table IMUValues {
// Gyro readings in radians/second.
// Positive is clockwise looking at the connector.
@@ -35,8 +83,22 @@
// FPGA timestamp when the values were captured.
fpga_timestamp:double;
- // CLOCK_MONOTONIC time in nanoseconds when the values were captured.
+ // CLOCK_MONOTONIC time in nanoseconds when the values were captured,
+ // converted from fpga_timestamp.
monotonic_timestamp_ns:long;
+
+ // For an ADIS16470, the DIAG_STAT value immediately after reset.
+ start_diag_stat:ADIS16470DiagStat;
+ // For an ADIS16470, the DIAG_STAT value after the initial sensor self test we
+ // trigger is finished.
+ self_test_diag_stat:ADIS16470DiagStat;
+ // For an ADIS16470, the DIAG_STAT value associated with the previous set of
+ // readings. This will never change during normal operation, so being 1 cycle
+ // state is OK.
+ previous_reading_diag_stat:ADIS16470DiagStat;
+
+ // The value read from the PROD_ID register.
+ product_id:uint16;
}
root_type IMUValues;
diff --git a/frc971/wpilib/sensor_reader.cc b/frc971/wpilib/sensor_reader.cc
index 5bcb84a..0d970e8 100644
--- a/frc971/wpilib/sensor_reader.cc
+++ b/frc971/wpilib/sensor_reader.cc
@@ -9,6 +9,7 @@
#include "frc971/wpilib/ahal/DigitalInput.h"
#include "frc971/wpilib/ahal/DriverStation.h"
#include "frc971/wpilib/ahal/Utility.h"
+#include "frc971/wpilib/fpga_time_conversion.h"
#include "frc971/wpilib/wpilib_interface.h"
#include "hal/PWM.h"
@@ -60,35 +61,20 @@
monotonic_clock::time_point SensorReader::GetPWMStartTime() {
int32_t status = 0;
- const hal::fpga_clock::time_point new_fpga_time = hal::fpga_clock::time_point(
- hal::fpga_clock::duration(HAL_GetPWMCycleStartTime(&status)));
+ const auto new_fpga_time =
+ hal::fpga_clock::duration(HAL_GetPWMCycleStartTime(&status));
- aos_compiler_memory_barrier();
- const hal::fpga_clock::time_point fpga_time_before = hal::fpga_clock::now();
- aos_compiler_memory_barrier();
- const monotonic_clock::time_point monotonic_now = monotonic_clock::now();
- aos_compiler_memory_barrier();
- const hal::fpga_clock::time_point fpga_time_after = hal::fpga_clock::now();
- aos_compiler_memory_barrier();
-
- const chrono::nanoseconds fpga_sample_length =
- fpga_time_after - fpga_time_before;
- const chrono::nanoseconds fpga_offset =
- hal::fpga_clock::time_point((fpga_time_after.time_since_epoch() +
- fpga_time_before.time_since_epoch()) /
- 2) -
- new_fpga_time;
-
- // Make sure that there wasn't a context switch while we were sampling the
- // clocks. If there was, we are better off rejecting the sample than using
- // it.
- if (ds_->IsSysActive() && fpga_sample_length <= chrono::microseconds(20) &&
- fpga_sample_length >= chrono::microseconds(0)) {
- // Compute when the edge was.
- return monotonic_now - fpga_offset;
- } else {
+ if (!ds_->IsSysActive()) {
return monotonic_clock::min_time;
}
+
+ const auto fpga_offset = CalculateFpgaOffset();
+ // If we failed to sample the offset, just ignore this reading.
+ if (!fpga_offset) {
+ return monotonic_clock::min_time;
+ }
+
+ return monotonic_clock::epoch() + (new_fpga_time + *fpga_offset);
}
void SensorReader::DoStart() {
diff --git a/y2019/BUILD b/y2019/BUILD
index d9cd4fd..328c93f 100644
--- a/y2019/BUILD
+++ b/y2019/BUILD
@@ -52,6 +52,8 @@
srcs = [
"wpilib_interface.cc",
],
+ # This library uses some deprecated parts of the SPI API.
+ copts = ["-Wno-deprecated-declarations"],
restricted_to = ["//tools:roborio"],
deps = [
":camera_log_fbs",