diff --git a/styleguide/spotbugs-exclude.xml b/styleguide/spotbugs-exclude.xml
index b354a0e212..2393c86468 100644
--- a/styleguide/spotbugs-exclude.xml
+++ b/styleguide/spotbugs-exclude.xml
@@ -80,6 +80,14 @@
+
+
+
+
+
+
+
+
diff --git a/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp
new file mode 100644
index 0000000000..64b39d246e
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp
@@ -0,0 +1,857 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2020 Analog Devices Inc. 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. */
+/* */
+/* Modified by Juan Chong - frcsupport@analog.com */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/ADIS16448_IMU.h"
+
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+
+#include "frc/Errors.h"
+
+/* Helpful conversion functions */
+static inline uint16_t BuffToUShort(const uint32_t* buf) {
+ return (static_cast(buf[0]) << 8) | buf[1];
+}
+
+static inline uint8_t BuffToUByte(const uint32_t* buf) {
+ return static_cast(buf[0]);
+}
+
+static inline int16_t BuffToShort(const uint32_t* buf) {
+ return (static_cast(buf[0]) << 8) | buf[1];
+}
+
+static inline uint16_t ToUShort(const uint8_t* buf) {
+ return (static_cast(buf[0]) << 8) | buf[1];
+}
+
+using namespace frc;
+
+namespace {
+template
+inline void ADISReportError(int32_t status, const char* file, int line,
+ const char* function, const S& format,
+ Args&&... args) {
+ frc::ReportErrorV(status, file, line, function, format,
+ fmt::make_args_checked(format, args...));
+}
+} // namespace
+
+#define REPORT_WARNING(msg) \
+ ADISReportError(warn::Warning, __FILE__, __LINE__, __FUNCTION__, "{}", msg);
+#define REPORT_ERROR(msg) \
+ ADISReportError(err::Error, __FILE__, __LINE__, __FUNCTION__, "{}", msg)
+
+ADIS16448_IMU::ADIS16448_IMU() : ADIS16448_IMU(kZ, SPI::Port::kMXP, 4) {}
+
+ADIS16448_IMU::ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port,
+ uint16_t cal_time)
+ : m_yaw_axis(yaw_axis), m_spi_port(port) {
+ // Force the IMU reset pin to toggle on startup (doesn't require DS enable)
+ // Relies on the RIO hardware by default configuring an output as low
+ // and configuring an input as high Z. The 10k pull-up resistor internal to
+ // the IMU then forces the reset line high for normal operation.
+ DigitalOutput* m_reset_out = new DigitalOutput(18); // Drive MXP DIO8 low
+ Wait(10_ms); // Wait 10ms
+ delete m_reset_out;
+ new DigitalInput(18); // Set MXP DIO8 high
+ Wait(500_ms); // Wait 500ms for reset to complete
+
+ ConfigCalTime(cal_time);
+
+ // Configure standard SPI
+ if (!SwitchToStandardSPI()) {
+ return;
+ }
+
+ // Set IMU internal decimation to 819.2 SPS
+ WriteRegister(SMPL_PRD, 0x0001);
+ // Enable Data Ready (LOW = Good Data) on DIO1 (PWM0 on MXP)
+ WriteRegister(MSC_CTRL, 0x0016);
+ // Disable IMU internal Bartlett filter
+ WriteRegister(SENS_AVG, 0x0400);
+ // Clear offset registers
+ WriteRegister(XGYRO_OFF, 0x0000);
+ WriteRegister(YGYRO_OFF, 0x0000);
+ WriteRegister(ZGYRO_OFF, 0x0000);
+ // Configure and enable auto SPI
+ if (!SwitchToAutoSPI()) {
+ return;
+ }
+ // Notify DS that IMU calibration delay is active
+ REPORT_WARNING("ADIS16448 IMU Detected. Starting initial calibration delay.");
+ // Wait for whatever time the user set as the start-up delay
+ Wait(static_cast(m_calibration_time) * 1.2_s);
+ // Execute calibration routine
+ Calibrate();
+ // Reset accumulated offsets
+ Reset();
+ // Tell the acquire loop that we're done starting up
+ m_start_up_mode = false;
+
+ // Let the user know the IMU was initiallized successfully
+ REPORT_WARNING("ADIS16448 IMU Successfully Initialized!");
+
+ // TODO: Find what the proper pin is to turn this LED
+ // Drive MXP PWM5 (IMU ready LED) low (active low)
+ new DigitalOutput(19);
+
+ // Report usage and post data to DS
+ HAL_Report(HALUsageReporting::kResourceType_ADIS16448, 0);
+
+ wpi::SendableRegistry::AddLW(this, "ADIS16448", port);
+}
+
+/**
+ * @brief Switches to standard SPI operation. Primarily used when exiting auto
+ *SPI mode.
+ *
+ * @return A boolean indicating the success or failure of setting up the SPI
+ *peripheral in standard SPI mode.
+ *
+ * This function switches the active SPI port to standard SPI and is used
+ *primarily when exiting auto SPI. Exiting auto SPI is required to read or write
+ *using SPI since the auto SPI configuration, once active, locks the SPI message
+ *being transacted. This function also verifies that the SPI port is operating
+ *in standard SPI mode by reading back the IMU product ID.
+ **/
+bool ADIS16448_IMU::SwitchToStandardSPI() {
+ // Check to see whether the acquire thread is active. If so, wait for it to
+ // stop producing data.
+ if (m_thread_active) {
+ m_thread_active = false;
+ while (!m_thread_idle) {
+ Wait(10_ms);
+ }
+ std::cout << "Paused the IMU processing thread successfully!" << std::endl;
+ // Maybe we're in auto SPI mode? If so, kill auto SPI, and then SPI.
+ if (m_spi != nullptr && m_auto_configured) {
+ m_spi->StopAuto();
+ // We need to get rid of all the garbage left in the auto SPI buffer after
+ // stopping it. Sometimes data magically reappears, so we have to check
+ // the buffer size a couple of times
+ // to be sure we got it all. Yuck.
+ uint32_t trashBuffer[200];
+ Wait(100_ms);
+ int data_count = m_spi->ReadAutoReceivedData(trashBuffer, 0, 0_s);
+ while (data_count > 0) {
+ /* Dequeue 200 at a time, or the remainder of the buffer if less than
+ * 200 */
+ m_spi->ReadAutoReceivedData(trashBuffer, std::min(200, data_count),
+ 0_s);
+ /* Update remaining buffer count */
+ data_count = m_spi->ReadAutoReceivedData(trashBuffer, 0, 0_s);
+ }
+ std::cout << "Paused the auto SPI successfully!" << std::endl;
+ }
+ }
+ // There doesn't seem to be a SPI port active. Let's try to set one up
+ if (m_spi == nullptr) {
+ std::cout << "Setting up a new SPI port." << std::endl;
+ m_spi = new SPI(m_spi_port);
+ m_spi->SetClockRate(1000000);
+ m_spi->SetMSBFirst();
+ m_spi->SetSampleDataOnTrailingEdge();
+ m_spi->SetClockActiveLow();
+ m_spi->SetChipSelectActiveLow();
+ ReadRegister(PROD_ID); // Dummy read
+
+ // Validate the product ID
+ uint16_t prod_id = ReadRegister(PROD_ID);
+ if (prod_id != 16448) {
+ REPORT_ERROR("Could not find ADIS16448!");
+ Close();
+ return false;
+ }
+ return true;
+ } else {
+ // Maybe the SPI port is active, but not in auto SPI mode? Try to read the
+ // product ID.
+ ReadRegister(PROD_ID); // Dummy read
+ uint16_t prod_id = ReadRegister(PROD_ID);
+ if (prod_id != 16448) {
+ REPORT_ERROR("Could not find ADIS16448!");
+ Close();
+ return false;
+ } else {
+ return true;
+ }
+ }
+}
+
+void ADIS16448_IMU::InitOffsetBuffer(int size) {
+ // avoid exceptions in the case of bad arguments
+ if (size < 1) {
+ size = 1;
+ }
+
+ // set average size to size (correct bad values)
+ m_avg_size = size;
+
+ // resize vector
+ if (m_offset_buffer != nullptr) {
+ delete[] m_offset_buffer;
+ }
+ m_offset_buffer = new offset_data[size];
+
+ // set accumulate count to 0
+ m_accum_count = 0;
+}
+
+/**
+ * This function switches the active SPI port to auto SPI and is used primarily
+ *when exiting standard SPI. Auto SPI is required to asynchronously read data
+ *over SPI as it utilizes special FPGA hardware to react to an external data
+ *ready (GPIO) input. Data captured using auto SPI is buffered in the FPGA and
+ *can be read by the CPU asynchronously. Standard SPI transactions are
+ * impossible on the selected SPI port once auto SPI is enabled. The stall
+ *settings, GPIO interrupt pin, and data packet settings used in this function
+ *are hard-coded to work only with the ADIS16448 IMU.
+ **/
+bool ADIS16448_IMU::SwitchToAutoSPI() {
+ // No SPI port has been set up. Go set one up first.
+ if (m_spi == nullptr) {
+ if (!SwitchToStandardSPI()) {
+ REPORT_ERROR("Failed to start/restart auto SPI");
+ return false;
+ }
+ }
+ // Only set up the interrupt if needed.
+ if (m_auto_interrupt == nullptr) {
+ m_auto_interrupt = new DigitalInput(10);
+ }
+ // The auto SPI controller gets angry if you try to set up two instances on
+ // one bus.
+ if (!m_auto_configured) {
+ m_spi->InitAuto(8200);
+ m_auto_configured = true;
+ }
+ // Set auto SPI packet data and size
+ m_spi->SetAutoTransmitData({{GLOB_CMD}}, 27);
+ // Configure auto stall time
+ m_spi->ConfigureAutoStall(HAL_SPI_kMXP, 100, 1000, 255);
+ // Kick off DMA SPI (Note: Device configration impossible after SPI DMA is
+ // activated)
+ m_spi->StartAutoTrigger(*m_auto_interrupt, true, false);
+ // Check to see if the acquire thread is running. If not, kick one off.
+ if (!m_thread_idle) {
+ m_first_run = true;
+ m_thread_active = true;
+ // Set up circular buffer
+ InitOffsetBuffer(m_avg_size);
+ // Kick off acquire thread
+ m_acquire_task = std::thread(&ADIS16448_IMU::Acquire, this);
+ std::cout << "New IMU Processing thread activated!" << std::endl;
+ } else {
+ m_first_run = true;
+ m_thread_active = true;
+ std::cout << "Old IMU Processing thread re-activated!" << std::endl;
+ }
+ // Looks like the thread didn't start for some reason. Abort.
+ /*
+ if(!m_thread_idle) {
+ REPORT_ERROR("Failed to start/restart the acquire() thread.");
+ Close();
+ return false;
+ }
+ */
+ return true;
+}
+
+/**
+ *
+ **/
+int ADIS16448_IMU::ConfigCalTime(int new_cal_time) {
+ if (m_calibration_time == new_cal_time) {
+ return 1;
+ } else {
+ m_calibration_time = static_cast(new_cal_time);
+ m_avg_size = m_calibration_time * 819;
+ InitOffsetBuffer(m_avg_size);
+ return 0;
+ }
+}
+
+/**
+ *
+ **/
+void ADIS16448_IMU::Calibrate() {
+ std::scoped_lock sync(m_mutex);
+ // Calculate the running average
+ int gyroAverageSize = std::min(m_accum_count, m_avg_size);
+ double m_gyro_accum_x = 0.0;
+ double m_gyro_accum_y = 0.0;
+ double m_gyro_accum_z = 0.0;
+ for (int i = 0; i < gyroAverageSize; i++) {
+ m_gyro_accum_x += m_offset_buffer[i].m_accum_gyro_x;
+ m_gyro_accum_y += m_offset_buffer[i].m_accum_gyro_y;
+ m_gyro_accum_z += m_offset_buffer[i].m_accum_gyro_z;
+ }
+ m_gyro_offset_x = m_gyro_accum_x / gyroAverageSize;
+ m_gyro_offset_y = m_gyro_accum_y / gyroAverageSize;
+ m_gyro_offset_z = m_gyro_accum_z / gyroAverageSize;
+ m_integ_gyro_x = 0.0;
+ m_integ_gyro_y = 0.0;
+ m_integ_gyro_z = 0.0;
+ // std::cout << "Avg Size: " << gyroAverageSize << " X off: " <<
+ // m_gyro_offset_x << " Y off: " << m_gyro_offset_y << " Z off: " <<
+ // m_gyro_offset_z << std::endl;
+}
+
+int ADIS16448_IMU::SetYawAxis(IMUAxis yaw_axis) {
+ if (m_yaw_axis == yaw_axis) {
+ return 1;
+ } else {
+ m_yaw_axis = yaw_axis;
+ Reset();
+ return 0;
+ }
+}
+
+/**
+ * This function reads the contents of an 8-bit register location by
+ *transmitting the register location byte along with a null (0x00) byte using
+ *the standard WPILib API. The response (two bytes) is read back using the
+ *WPILib API and joined using a helper function. This function assumes the
+ *controller is set to standard SPI mode.
+ **/
+uint16_t ADIS16448_IMU::ReadRegister(uint8_t reg) {
+ uint8_t buf[2];
+ buf[0] = reg & 0x7f;
+ buf[1] = 0;
+
+ m_spi->Write(buf, 2);
+ m_spi->Read(false, buf, 2);
+
+ return ToUShort(buf);
+}
+
+/**
+ * This function writes an unsigned, 16-bit value into adjacent 8-bit addresses
+ *via SPI. The upper and lower bytes that make up the 16-bit value are split
+ *into two unsined, 8-bit values and written to the upper and lower addresses of
+ *the specified register value. Only the lower (base) address must be specified.
+ *This function assumes the controller is set to standard SPI mode.
+ **/
+void ADIS16448_IMU::WriteRegister(uint8_t reg, uint16_t val) {
+ uint8_t buf[2];
+ buf[0] = 0x80 | reg;
+ buf[1] = val & 0xff;
+ m_spi->Write(buf, 2);
+ buf[0] = 0x81 | reg;
+ buf[1] = val >> 8;
+ m_spi->Write(buf, 2);
+}
+
+/**
+ * This function resets (zeros) the accumulated (integrated) angle estimates for
+ *the xgyro, ygyro, and zgyro outputs.
+ **/
+void ADIS16448_IMU::Reset() {
+ std::scoped_lock sync(m_mutex);
+ m_integ_gyro_x = 0.0;
+ m_integ_gyro_y = 0.0;
+ m_integ_gyro_z = 0.0;
+}
+
+void ADIS16448_IMU::Close() {
+ if (m_thread_active) {
+ m_thread_active = false;
+ if (m_acquire_task.joinable()) {
+ m_acquire_task.join();
+ }
+ }
+ if (m_spi != nullptr) {
+ if (m_auto_configured) {
+ m_spi->StopAuto();
+ }
+ delete m_spi;
+ m_auto_configured = false;
+ if (m_auto_interrupt != nullptr) {
+ delete m_auto_interrupt;
+ m_auto_interrupt = nullptr;
+ }
+ m_spi = nullptr;
+ }
+ delete[] m_offset_buffer;
+ std::cout << "Finished cleaning up after the IMU driver." << std::endl;
+}
+
+ADIS16448_IMU::~ADIS16448_IMU() {
+ Close();
+}
+
+void ADIS16448_IMU::Acquire() {
+ // Set data packet length
+ const int dataset_len = 29; // 18 data points + timestamp
+
+ const int BUFFER_SIZE = 4000;
+
+ // struct to store accumulate data
+ offset_data sample_data;
+
+ // This buffer can contain many datasets
+ uint32_t buffer[BUFFER_SIZE];
+ int data_count = 0;
+ int data_remainder = 0;
+ int data_to_read = 0;
+ int bufferAvgIndex = 0;
+ uint32_t previous_timestamp = 0;
+ double gyro_x = 0.0;
+ double gyro_y = 0.0;
+ double gyro_z = 0.0;
+ double accel_x = 0.0;
+ double accel_y = 0.0;
+ double accel_z = 0.0;
+ double mag_x = 0.0;
+ double mag_y = 0.0;
+ double mag_z = 0.0;
+ double baro = 0.0;
+ double temp = 0.0;
+ double gyro_x_si = 0.0;
+ double gyro_y_si = 0.0;
+ // double gyro_z_si = 0.0;
+ double accel_x_si = 0.0;
+ double accel_y_si = 0.0;
+ double accel_z_si = 0.0;
+ double compAngleX = 0.0;
+ double compAngleY = 0.0;
+ double accelAngleX = 0.0;
+ double accelAngleY = 0.0;
+
+ while (true) {
+ // Sleep loop for 10ms (wait for data)
+ Wait(10_ms);
+
+ if (m_thread_active) {
+ data_count = m_spi->ReadAutoReceivedData(
+ buffer, 0,
+ 0_s); // Read number of bytes currently stored in the buffer
+ data_remainder =
+ data_count % dataset_len; // Check if frame is incomplete
+ data_to_read = data_count -
+ data_remainder; // Remove incomplete data from read count
+ /* Want to cap the data to read in a single read at the buffer size */
+ if (data_to_read > BUFFER_SIZE) {
+ REPORT_WARNING(
+ "ADIS16448 data processing thread overrun has occurred!");
+ data_to_read = BUFFER_SIZE - (BUFFER_SIZE % dataset_len);
+ }
+ m_spi->ReadAutoReceivedData(buffer, data_to_read,
+ 0_s); // Read data from DMA buffer
+
+ // Could be multiple data sets in the buffer. Handle each one.
+ for (int i = 0; i < data_to_read; i += dataset_len) {
+ // Calculate CRC-16 on each data packet
+ uint16_t calc_crc = 0xFFFF; // Starting word
+ uint8_t byte = 0;
+ uint16_t imu_crc = 0;
+ for (int k = 5; k < 27;
+ k += 2) // Cycle through XYZ GYRO, XYZ ACCEL, XYZ MAG, BARO, TEMP
+ // (Ignore Status & CRC)
+ {
+ byte = BuffToUByte(&buffer[i + k + 1]); // Process LSB
+ calc_crc = (calc_crc >> 8) ^ adiscrc[(calc_crc & 0x00FF) ^ byte];
+ byte = BuffToUByte(&buffer[i + k]); // Process MSB
+ calc_crc = (calc_crc >> 8) ^ adiscrc[(calc_crc & 0x00FF) ^ byte];
+ }
+ calc_crc = ~calc_crc; // Complement
+ calc_crc = static_cast((calc_crc << 8) |
+ (calc_crc >> 8)); // Flip LSB & MSB
+ imu_crc =
+ BuffToUShort(&buffer[i + 27]); // Extract DUT CRC from data buffer
+
+ // Compare calculated vs read CRC. Don't update outputs or dt if CRC-16
+ // is bad
+ if (calc_crc == imu_crc) {
+ // Timestamp is at buffer[i]
+ m_dt = (buffer[i] - previous_timestamp) / 1000000.0;
+ // Split array and scale data
+ gyro_x = BuffToShort(&buffer[i + 5]) * 0.04;
+ gyro_y = BuffToShort(&buffer[i + 7]) * 0.04;
+ gyro_z = BuffToShort(&buffer[i + 9]) * 0.04;
+ accel_x = BuffToShort(&buffer[i + 11]) * 0.833;
+ accel_y = BuffToShort(&buffer[i + 13]) * 0.833;
+ accel_z = BuffToShort(&buffer[i + 15]) * 0.833;
+ mag_x = BuffToShort(&buffer[i + 17]) * 0.1429;
+ mag_y = BuffToShort(&buffer[i + 19]) * 0.1429;
+ mag_z = BuffToShort(&buffer[i + 21]) * 0.1429;
+ baro = BuffToShort(&buffer[i + 23]) * 0.02;
+ temp = BuffToShort(&buffer[i + 25]) * 0.07386 + 31.0;
+
+ /*std::cout << BuffToShort(&buffer[i + 3]) << "," <<
+ BuffToShort(&buffer[i + 5]) << "," << BuffToShort(&buffer[i + 7]) <<
+ "," << BuffToShort(&buffer[i + 9]) << "," << BuffToShort(&buffer[i +
+ 11]) << "," << BuffToShort(&buffer[i + 13]) << "," <<
+ BuffToShort(&buffer[i + 15]) << "," << BuffToShort(&buffer[i + 17]) <<
+ "," << BuffToShort(&buffer[i + 19]) << "," << BuffToShort(&buffer[i +
+ 21]) << "," << BuffToShort(&buffer[i + 23]) << "," <<
+ BuffToShort(&buffer[i + 25]) << "," <<
+ BuffToShort(&buffer[i + 27]) << std::endl; */
+
+ // Convert scaled sensor data to SI units
+ gyro_x_si = gyro_x * deg_to_rad;
+ gyro_y_si = gyro_y * deg_to_rad;
+ // gyro_z_si = gyro_z * deg_to_rad;
+ accel_x_si = accel_x * grav;
+ accel_y_si = accel_y * grav;
+ accel_z_si = accel_z * grav;
+ // Store timestamp for next iteration
+ previous_timestamp = buffer[i];
+ // Calculate alpha for use with the complementary filter
+ m_alpha = m_tau / (m_tau + m_dt);
+ // Calculate complementary filter
+ if (m_first_run) {
+ accelAngleX = atan2f(
+ -accel_x_si,
+ sqrtf((accel_y_si * accel_y_si) + (-accel_z_si * -accel_z_si)));
+ accelAngleY =
+ atan2f(accel_y_si, sqrtf((-accel_x_si * -accel_x_si) +
+ (-accel_z_si * -accel_z_si)));
+ compAngleX = accelAngleX;
+ compAngleY = accelAngleY;
+ } else {
+ accelAngleX = atan2f(
+ -accel_x_si,
+ sqrtf((accel_y_si * accel_y_si) + (-accel_z_si * -accel_z_si)));
+ accelAngleY =
+ atan2f(accel_y_si, sqrtf((-accel_x_si * -accel_x_si) +
+ (-accel_z_si * -accel_z_si)));
+ accelAngleX = FormatAccelRange(accelAngleX, -accel_z_si);
+ accelAngleY = FormatAccelRange(accelAngleY, -accel_z_si);
+ compAngleX = CompFilterProcess(compAngleX, accelAngleX, -gyro_y_si);
+ compAngleY = CompFilterProcess(compAngleY, accelAngleY, -gyro_x_si);
+ }
+
+ // Update global variables and state
+ {
+ std::scoped_lock sync(m_mutex);
+ // Ignore first, integrated sample
+ if (m_first_run) {
+ m_integ_gyro_x = 0.0;
+ m_integ_gyro_y = 0.0;
+ m_integ_gyro_z = 0.0;
+ } else {
+ // Accumulate gyro for offset calibration
+ // Build most recent sample data
+ sample_data.m_accum_gyro_x = gyro_x;
+ sample_data.m_accum_gyro_y = gyro_y;
+ sample_data.m_accum_gyro_z = gyro_z;
+ // Add to buffer
+ bufferAvgIndex = m_accum_count % m_avg_size;
+ m_offset_buffer[bufferAvgIndex] = sample_data;
+ // Increment counter
+ m_accum_count++;
+ }
+ // Don't post accumulated data to the global variables until an
+ // initial gyro offset has been calculated
+ if (!m_start_up_mode) {
+ m_gyro_x = gyro_x;
+ m_gyro_y = gyro_y;
+ m_gyro_z = gyro_z;
+ m_accel_x = accel_x;
+ m_accel_y = accel_y;
+ m_accel_z = accel_z;
+ m_mag_x = mag_x;
+ m_mag_y = mag_y;
+ m_mag_z = mag_z;
+ m_baro = baro;
+ m_temp = temp;
+ m_compAngleX = compAngleX * rad_to_deg;
+ m_compAngleY = compAngleY * rad_to_deg;
+ m_accelAngleX = accelAngleX * rad_to_deg;
+ m_accelAngleY = accelAngleY * rad_to_deg;
+ // Accumulate gyro for angle integration and publish to global
+ // variables
+ m_integ_gyro_x += (gyro_x - m_gyro_offset_x) * m_dt;
+ m_integ_gyro_y += (gyro_y - m_gyro_offset_y) * m_dt;
+ m_integ_gyro_z += (gyro_z - m_gyro_offset_z) * m_dt;
+ }
+ }
+ m_first_run = false;
+ } else {
+ /*
+ // Print notification when crc fails and bad data is rejected
+ std::cout << "IMU Data CRC Mismatch Detected." << std::endl;
+ std::cout << "Calculated CRC: " << calc_crc << std::endl;
+ std::cout << "Read CRC: " << imu_crc << std::endl;
+ // DEBUG: Plot sub-array data in terminal
+ std::cout << BuffToUShort(&buffer[i + 3]) << "," <<
+ BuffToUShort(&buffer[i + 5]) << "," << BuffToUShort(&buffer[i + 7]) <<
+ "," << BuffToUShort(&buffer[i + 9]) << "," << BuffToUShort(&buffer[i +
+ 11]) << "," << BuffToUShort(&buffer[i + 13]) << "," <<
+ BuffToUShort(&buffer[i + 15]) << "," << BuffToUShort(&buffer[i + 17])
+ << "," << BuffToUShort(&buffer[i + 19]) << "," <<
+ BuffToUShort(&buffer[i + 21]) << "," << BuffToUShort(&buffer[i + 23])
+ << "," << BuffToUShort(&buffer[i + 25]) << "," <<
+ BuffToUShort(&buffer[i + 27]) << std::endl; */
+ }
+ }
+ } else {
+ m_thread_idle = true;
+ data_count = 0;
+ data_remainder = 0;
+ data_to_read = 0;
+ previous_timestamp = 0.0;
+ gyro_x = 0.0;
+ gyro_y = 0.0;
+ gyro_z = 0.0;
+ accel_x = 0.0;
+ accel_y = 0.0;
+ accel_z = 0.0;
+ mag_x = 0.0;
+ mag_y = 0.0;
+ mag_z = 0.0;
+ baro = 0.0;
+ temp = 0.0;
+ gyro_x_si = 0.0;
+ gyro_y_si = 0.0;
+ // gyro_z_si = 0.0;
+ accel_x_si = 0.0;
+ accel_y_si = 0.0;
+ accel_z_si = 0.0;
+ compAngleX = 0.0;
+ compAngleY = 0.0;
+ accelAngleX = 0.0;
+ accelAngleY = 0.0;
+ }
+ }
+}
+
+/* Complementary filter functions */
+double ADIS16448_IMU::FormatFastConverge(double compAngle, double accAngle) {
+ if (compAngle > accAngle + wpi::numbers::pi) {
+ compAngle = compAngle - 2.0 * wpi::numbers::pi;
+ } else if (accAngle > compAngle + wpi::numbers::pi) {
+ compAngle = compAngle + 2.0 * wpi::numbers::pi;
+ }
+ return compAngle;
+}
+
+double ADIS16448_IMU::FormatRange0to2PI(double compAngle) {
+ while (compAngle >= 2 * wpi::numbers::pi) {
+ compAngle = compAngle - 2.0 * wpi::numbers::pi;
+ }
+ while (compAngle < 0.0) {
+ compAngle = compAngle + 2.0 * wpi::numbers::pi;
+ }
+ return compAngle;
+}
+
+double ADIS16448_IMU::FormatAccelRange(double accelAngle, double accelZ) {
+ if (accelZ < 0.0) {
+ accelAngle = wpi::numbers::pi - accelAngle;
+ } else if (accelZ > 0.0 && accelAngle < 0.0) {
+ accelAngle = 2.0 * wpi::numbers::pi + accelAngle;
+ }
+ return accelAngle;
+}
+
+double ADIS16448_IMU::CompFilterProcess(double compAngle, double accelAngle,
+ double omega) {
+ compAngle = FormatFastConverge(compAngle, accelAngle);
+ compAngle =
+ m_alpha * (compAngle + omega * m_dt) + (1.0 - m_alpha) * accelAngle;
+ compAngle = FormatRange0to2PI(compAngle);
+ if (compAngle > wpi::numbers::pi) {
+ compAngle = compAngle - 2.0 * wpi::numbers::pi;
+ }
+ return compAngle;
+}
+
+int ADIS16448_IMU::ConfigDecRate(uint16_t DecimationSetting) {
+ uint16_t writeValue = DecimationSetting;
+ uint16_t readbackValue;
+ if (!SwitchToStandardSPI()) {
+ REPORT_ERROR("Failed to configure/reconfigure standard SPI.");
+ return 2;
+ }
+
+ /* Check max */
+ if (DecimationSetting > 9) {
+ REPORT_ERROR("Attemted to write an invalid decimation value. Capping at 9");
+ DecimationSetting = 9;
+ }
+
+ /* Shift decimation setting to correct position and select internal sync */
+ writeValue = (DecimationSetting << 8) | 0x1;
+
+ /* Apply to IMU */
+ WriteRegister(SMPL_PRD, writeValue);
+
+ /* Perform read back to verify write */
+ readbackValue = ReadRegister(SMPL_PRD);
+
+ /* Throw error for invalid write */
+ if (readbackValue != writeValue) {
+ REPORT_ERROR("ADIS16448 SMPL_PRD write failed.");
+ }
+
+ if (!SwitchToAutoSPI()) {
+ REPORT_ERROR("Failed to configure/reconfigure auto SPI.");
+ return 2;
+ }
+ return 0;
+}
+
+/**
+ * This function returns the most recent integrated angle for the axis chosen by
+ *m_yaw_axis. This function is most useful in situations where the yaw axis may
+ *not coincide with the IMU Z axis.
+ **/
+double ADIS16448_IMU::GetAngle() const {
+ switch (m_yaw_axis) {
+ case kX:
+ return GetGyroAngleX();
+ case kY:
+ return GetGyroAngleY();
+ case kZ:
+ return GetGyroAngleZ();
+ default:
+ return 0.0;
+ }
+}
+
+double ADIS16448_IMU::GetRate() const {
+ switch (m_yaw_axis) {
+ case kX:
+ return GetGyroInstantX();
+ case kY:
+ return GetGyroInstantY();
+ case kZ:
+ return GetGyroInstantZ();
+ default:
+ return 0.0;
+ }
+}
+
+ADIS16448_IMU::IMUAxis ADIS16448_IMU::GetYawAxis() const {
+ return m_yaw_axis;
+}
+
+double ADIS16448_IMU::GetGyroAngleX() const {
+ std::scoped_lock sync(m_mutex);
+ return m_integ_gyro_x;
+}
+
+double ADIS16448_IMU::GetGyroAngleY() const {
+ std::scoped_lock sync(m_mutex);
+ return m_integ_gyro_y;
+}
+
+double ADIS16448_IMU::GetGyroAngleZ() const {
+ std::scoped_lock sync(m_mutex);
+ return m_integ_gyro_z;
+}
+
+double ADIS16448_IMU::GetGyroInstantX() const {
+ std::scoped_lock sync(m_mutex);
+ return m_gyro_x;
+}
+
+double ADIS16448_IMU::GetGyroInstantY() const {
+ std::scoped_lock sync(m_mutex);
+ return m_gyro_y;
+}
+
+double ADIS16448_IMU::GetGyroInstantZ() const {
+ std::scoped_lock sync(m_mutex);
+ return m_gyro_z;
+}
+
+double ADIS16448_IMU::GetAccelInstantX() const {
+ std::scoped_lock sync(m_mutex);
+ return m_accel_x;
+}
+
+double ADIS16448_IMU::GetAccelInstantY() const {
+ std::scoped_lock sync(m_mutex);
+ return m_accel_y;
+}
+
+double ADIS16448_IMU::GetAccelInstantZ() const {
+ std::scoped_lock sync(m_mutex);
+ return m_accel_z;
+}
+
+double ADIS16448_IMU::GetMagInstantX() const {
+ std::scoped_lock sync(m_mutex);
+ return m_mag_x;
+}
+
+double ADIS16448_IMU::GetMagInstantY() const {
+ std::scoped_lock sync(m_mutex);
+ return m_mag_y;
+}
+
+double ADIS16448_IMU::GetMagInstantZ() const {
+ std::scoped_lock sync(m_mutex);
+ return m_mag_z;
+}
+
+double ADIS16448_IMU::GetXComplementaryAngle() const {
+ std::scoped_lock sync(m_mutex);
+ return m_compAngleX;
+}
+
+double ADIS16448_IMU::GetYComplementaryAngle() const {
+ std::scoped_lock sync(m_mutex);
+ return m_compAngleY;
+}
+
+double ADIS16448_IMU::GetXFilteredAccelAngle() const {
+ std::scoped_lock sync(m_mutex);
+ return m_accelAngleX;
+}
+
+double ADIS16448_IMU::GetYFilteredAccelAngle() const {
+ std::scoped_lock sync(m_mutex);
+ return m_accelAngleY;
+}
+
+double ADIS16448_IMU::GetBarometricPressure() const {
+ std::scoped_lock sync(m_mutex);
+ return m_baro;
+}
+
+double ADIS16448_IMU::GetTemperature() const {
+ std::scoped_lock sync(m_mutex);
+ return m_temp;
+}
+
+/**
+ * @brief Builds a Sendable object to push IMU data to the driver station.
+ *
+ * This function pushes the most recent angle estimates for all axes to the
+ *driver station.
+ **/
+void ADIS16448_IMU::InitSendable(nt::NTSendableBuilder& builder) {
+ builder.SetSmartDashboardType("ADIS16448 IMU");
+ auto yaw_angle = builder.GetEntry("Yaw Angle").GetHandle();
+ builder.SetUpdateTable(
+ [=]() { nt::NetworkTableEntry(yaw_angle).SetDouble(GetAngle()); });
+}
diff --git a/wpilibc/src/main/native/cpp/ADIS_16470_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS_16470_IMU.cpp
new file mode 100644
index 0000000000..47cc8d819e
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/ADIS_16470_IMU.cpp
@@ -0,0 +1,795 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2020 Analog Devices Inc. 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. */
+/* */
+/* Juan Chong - frcsupport@analog.com */
+/*----------------------------------------------------------------------------*/
+
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+
+#include "frc/Errors.h"
+
+/* Helpful conversion functions */
+static inline int32_t ToInt(const uint32_t* buf) {
+ return static_cast((buf[0] << 24) | (buf[1] << 16) | (buf[2] << 8) |
+ buf[3]);
+}
+
+static inline int16_t BuffToShort(const uint32_t* buf) {
+ return (static_cast(buf[0]) << 8) | buf[1];
+}
+
+static inline uint16_t ToUShort(const uint8_t* buf) {
+ return (static_cast(buf[0]) << 8) | buf[1];
+}
+
+using namespace frc;
+
+namespace {
+template
+inline void ADISReportError(int32_t status, const char* file, int line,
+ const char* function, const S& format,
+ Args&&... args) {
+ frc::ReportErrorV(status, file, line, function, format,
+ fmt::make_args_checked(format, args...));
+}
+} // namespace
+
+#define REPORT_WARNING(msg) \
+ ADISReportError(warn::Warning, __FILE__, __LINE__, __FUNCTION__, "{}", msg);
+#define REPORT_ERROR(msg) \
+ ADISReportError(err::Error, __FILE__, __LINE__, __FUNCTION__, "{}", msg)
+
+/**
+ * Constructor.
+ */
+ADIS16470_IMU::ADIS16470_IMU()
+ : ADIS16470_IMU(kZ, SPI::Port::kOnboardCS0, ADIS16470CalibrationTime::_4s) {
+}
+
+ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, SPI::Port port,
+ ADIS16470CalibrationTime cal_time)
+ : m_yaw_axis(yaw_axis),
+ m_spi_port(port),
+ m_calibration_time(static_cast(cal_time)) {
+ // Force the IMU reset pin to toggle on startup (doesn't require DS enable)
+ // Relies on the RIO hardware by default configuring an output as low
+ // and configuring an input as high Z. The 10k pull-up resistor internal to
+ // the IMU then forces the reset line high for normal operation.
+ DigitalOutput* m_reset_out =
+ new DigitalOutput(27); // Drive SPI CS2 (IMU RST) low
+ Wait(10_ms); // Wait 10ms
+ delete m_reset_out;
+ new DigitalInput(27); // Set SPI CS2 (IMU RST) high
+ Wait(500_ms); // Wait 500ms for reset to complete
+
+ // Configure standard SPI
+ if (!SwitchToStandardSPI()) {
+ return;
+ }
+
+ // Set IMU internal decimation to 4 (output data rate of 2000 SPS / (4 + 1) =
+ // 400Hz)
+ WriteRegister(DEC_RATE, 0x0004);
+ // Set data ready polarity (HIGH = Good Data), Disable gSense Compensation and
+ // PoP
+ WriteRegister(MSC_CTRL, 0x0001);
+ // Configure IMU internal Bartlett filter
+ WriteRegister(FILT_CTRL, 0x0000);
+ // Configure continuous bias calibration time based on user setting
+ WriteRegister(NULL_CNFG, m_calibration_time | 0x700);
+
+ // Notify DS that IMU calibration delay is active
+ REPORT_WARNING("ADIS16470 IMU Detected. Starting initial calibration delay.");
+
+ // Wait for samples to accumulate internal to the IMU (110% of user-defined
+ // time)
+ Wait(units::second_t{pow(2, m_calibration_time) / 2000 * 64 * 1.1});
+
+ // Write offset calibration command to IMU
+ WriteRegister(GLOB_CMD, 0x0001);
+
+ // Configure and enable auto SPI
+ if (!SwitchToAutoSPI()) {
+ return;
+ }
+
+ // Let the user know the IMU was initiallized successfully
+ REPORT_WARNING("ADIS16470 IMU Successfully Initialized!");
+
+ // Drive SPI CS3 (IMU ready LED) low (active low)
+ new DigitalOutput(28);
+
+ // Report usage and post data to DS
+ HAL_Report(HALUsageReporting::kResourceType_ADIS16470, 0);
+
+ wpi::SendableRegistry::AddLW(this, "ADIS16470", port);
+}
+
+/**
+ * @brief Switches to standard SPI operation. Primarily used when exiting auto
+ *SPI mode.
+ *
+ * @return A boolean indicating the success or failure of setting up the SPI
+ *peripheral in standard SPI mode.
+ *
+ * This function switches the active SPI port to standard SPI and is used
+ *primarily when exiting auto SPI. Exiting auto SPI is required to read or write
+ *using SPI since the auto SPI configuration, once active, locks the SPI message
+ *being transacted. This function also verifies that the SPI port is operating
+ *in standard SPI mode by reading back the IMU product ID.
+ **/
+bool ADIS16470_IMU::SwitchToStandardSPI() {
+ // Check to see whether the acquire thread is active. If so, wait for it to
+ // stop producing data.
+ if (m_thread_active) {
+ m_thread_active = false;
+ while (!m_thread_idle) {
+ Wait(10_ms);
+ }
+ std::cout << "Paused the IMU processing thread successfully!" << std::endl;
+ // Maybe we're in auto SPI mode? If so, kill auto SPI, and then SPI.
+ if (m_spi != nullptr && m_auto_configured) {
+ m_spi->StopAuto();
+ // We need to get rid of all the garbage left in the auto SPI buffer after
+ // stopping it. Sometimes data magically reappears, so we have to check
+ // the buffer size a couple of times
+ // to be sure we got it all. Yuck.
+ uint32_t trashBuffer[200];
+ Wait(100_ms);
+ int data_count = m_spi->ReadAutoReceivedData(trashBuffer, 0, 0_s);
+ while (data_count > 0) {
+ /* Receive data, max of 200 words at a time (prevent potential segfault)
+ */
+ m_spi->ReadAutoReceivedData(trashBuffer, std::min(data_count, 200),
+ 0_s);
+ /*Get the reamining data count */
+ data_count = m_spi->ReadAutoReceivedData(trashBuffer, 0, 0_s);
+ }
+ std::cout << "Paused the auto SPI successfully!" << std::endl;
+ }
+ }
+ // There doesn't seem to be a SPI port active. Let's try to set one up
+ if (m_spi == nullptr) {
+ std::cout << "Setting up a new SPI port." << std::endl;
+ m_spi = new SPI(m_spi_port);
+ m_spi->SetClockRate(2000000);
+ m_spi->SetMSBFirst();
+ m_spi->SetSampleDataOnTrailingEdge();
+ m_spi->SetClockActiveLow();
+ m_spi->SetChipSelectActiveLow();
+ ReadRegister(PROD_ID); // Dummy read
+
+ // Validate the product ID
+ uint16_t prod_id = ReadRegister(PROD_ID);
+ if (prod_id != 16982 && prod_id != 16470) {
+ REPORT_ERROR("Could not find ADIS16470!");
+ Close();
+ return false;
+ }
+ return true;
+ } else {
+ // Maybe the SPI port is active, but not in auto SPI mode? Try to read the
+ // product ID.
+ ReadRegister(PROD_ID); // Dummy read
+ uint16_t prod_id = ReadRegister(PROD_ID);
+ if (prod_id != 16982 && prod_id != 16470) {
+ REPORT_ERROR("Could not find ADIS16470!");
+ Close();
+ return false;
+ } else {
+ return true;
+ }
+ }
+}
+
+/**
+ * @brief Switches to auto SPI operation. Primarily used when exiting standard
+ *SPI mode.
+ *
+ * @return A boolean indicating the success or failure of setting up the SPI
+ *peripheral in auto SPI mode.
+ *
+ * This function switches the active SPI port to auto SPI and is used primarily
+ *when exiting standard SPI. Auto SPI is required to asynchronously read data
+ *over SPI as it utilizes special FPGA hardware to react to an external data
+ *ready (GPIO) input. Data captured using auto SPI is buffered in the FPGA and
+ *can be read by the CPU asynchronously. Standard SPI transactions are
+ * impossible on the selected SPI port once auto SPI is enabled. The stall
+ *settings, GPIO interrupt pin, and data packet settings used in this function
+ *are hard-coded to work only with the ADIS16470 IMU.
+ **/
+bool ADIS16470_IMU::SwitchToAutoSPI() {
+ // No SPI port has been set up. Go set one up first.
+ if (m_spi == nullptr) {
+ if (!SwitchToStandardSPI()) {
+ REPORT_ERROR("Failed to start/restart auto SPI");
+ return false;
+ }
+ }
+ // Only set up the interrupt if needed.
+ if (m_auto_interrupt == nullptr) {
+ m_auto_interrupt = new DigitalInput(26);
+ }
+ // The auto SPI controller gets angry if you try to set up two instances on
+ // one bus.
+ if (!m_auto_configured) {
+ m_spi->InitAuto(8200);
+ m_auto_configured = true;
+ }
+ // Do we need to change auto SPI settings?
+ switch (m_yaw_axis) {
+ case kX:
+ m_spi->SetAutoTransmitData(m_autospi_x_packet, 2);
+ break;
+ case kY:
+ m_spi->SetAutoTransmitData(m_autospi_y_packet, 2);
+ break;
+ default:
+ m_spi->SetAutoTransmitData(m_autospi_z_packet, 2);
+ break;
+ }
+ // Configure auto stall time
+ m_spi->ConfigureAutoStall(HAL_SPI_kOnboardCS0, 5, 1000, 1);
+ // Kick off DMA SPI (Note: Device configration impossible after SPI DMA is
+ // activated) DR High = Data good (data capture should be triggered on the
+ // rising edge)
+ m_spi->StartAutoTrigger(*m_auto_interrupt, true, false);
+ // Check to see if the acquire thread is running. If not, kick one off.
+ if (!m_thread_idle) {
+ m_first_run = true;
+ m_thread_active = true;
+ m_acquire_task = std::thread(&ADIS16470_IMU::Acquire, this);
+ std::cout << "New IMU Processing thread activated!" << std::endl;
+ } else {
+ m_first_run = true;
+ m_thread_active = true;
+ std::cout << "Old IMU Processing thread re-activated!" << std::endl;
+ }
+ // Looks like the thread didn't start for some reason. Abort.
+ /*
+ if(!m_thread_idle) {
+ REPORT_ERROR("Failed to start/restart the acquire() thread.");
+ Close();
+ return false;
+ }
+ */
+ return true;
+}
+
+/**
+ * @brief Switches the active SPI port to standard SPI mode, writes a new value
+ *to the NULL_CNFG register in the IMU, and re-enables auto SPI.
+ *
+ * @param new_cal_time Calibration time to be set.
+ *
+ * @return An int indicating the success or failure of writing the new NULL_CNFG
+ *setting and returning to auto SPI mode. 0 = Success, 1 = No Change, 2 =
+ *Failure
+ *
+ * This function enters standard SPI mode, writes a new NULL_CNFG setting to the
+ *IMU, and re-enters auto SPI mode. This function does not include a blocking
+ *sleep, so the user must keep track of the elapsed offset calibration time
+ * themselves. After waiting the configured calibration time, the Calibrate()
+ *function should be called to activate the new offset calibration.
+ **/
+int ADIS16470_IMU::ConfigCalTime(ADIS16470CalibrationTime new_cal_time) {
+ if (m_calibration_time == static_cast(new_cal_time)) {
+ return 1;
+ }
+ if (!SwitchToStandardSPI()) {
+ REPORT_ERROR("Failed to configure/reconfigure standard SPI.");
+ return 2;
+ }
+ m_calibration_time = static_cast(new_cal_time);
+ WriteRegister(NULL_CNFG, m_calibration_time | 0x700);
+ if (!SwitchToAutoSPI()) {
+ REPORT_ERROR("Failed to configure/reconfigure auto SPI.");
+ return 2;
+ }
+ return 0;
+}
+
+/**
+ * @brief Switches the active SPI port to standard SPI mode, writes a new value
+ *to the DECIMATE register in the IMU, and re-enables auto SPI.
+ *
+ * @param reg Decimation value to be set.
+ *
+ * @return An int indicating the success or failure of writing the new DECIMATE
+ *setting and returning to auto SPI mode. 0 = Success, 1 = No Change, 2 =
+ *Failure
+ *
+ * This function enters standard SPI mode, writes a new DECIMATE setting to the
+ *IMU, adjusts the sample scale factor, and re-enters auto SPI mode.
+ **/
+int ADIS16470_IMU::ConfigDecRate(uint16_t reg) {
+ uint16_t m_reg = reg;
+ if (!SwitchToStandardSPI()) {
+ REPORT_ERROR("Failed to configure/reconfigure standard SPI.");
+ return 2;
+ }
+ if (m_reg > 1999) {
+ REPORT_ERROR("Attempted to write an invalid decimation value.");
+ m_reg = 1999;
+ }
+ m_scaled_sample_rate = (((m_reg + 1.0) / 2000.0) * 1000000.0);
+ WriteRegister(DEC_RATE, m_reg);
+ if (!SwitchToAutoSPI()) {
+ REPORT_ERROR("Failed to configure/reconfigure auto SPI.");
+ return 2;
+ }
+ return 0;
+}
+
+/**
+ * @brief Switches the active SPI port to standard SPI mode, writes the command
+ *to activate the new null configuration, and re-enables auto SPI.
+ *
+ * This function enters standard SPI mode, writes 0x0001 to the GLOB_CMD
+ *register (thus making the new offset active in the IMU), and re-enters auto
+ *SPI mode. This function does not include a blocking sleep, so the user must
+ *keep track of the elapsed offset calibration time themselves.
+ **/
+void ADIS16470_IMU::Calibrate() {
+ if (!SwitchToStandardSPI()) {
+ REPORT_ERROR("Failed to configure/reconfigure standard SPI.");
+ }
+ WriteRegister(GLOB_CMD, 0x0001);
+ if (!SwitchToAutoSPI()) {
+ REPORT_ERROR("Failed to configure/reconfigure auto SPI.");
+ }
+}
+
+int ADIS16470_IMU::SetYawAxis(IMUAxis yaw_axis) {
+ if (m_yaw_axis == yaw_axis) {
+ return 1;
+ }
+ if (!SwitchToStandardSPI()) {
+ REPORT_ERROR("Failed to configure/reconfigure standard SPI.");
+ return 2;
+ }
+ m_yaw_axis = yaw_axis;
+ if (!SwitchToAutoSPI()) {
+ REPORT_ERROR("Failed to configure/reconfigure auto SPI.");
+ return 2;
+ }
+ return 0;
+}
+
+/**
+ * @brief Reads the contents of a specified register location over SPI.
+ *
+ * @param reg An unsigned, 8-bit register location.
+ *
+ * @return An unsigned, 16-bit number representing the contents of the requested
+ *register location.
+ *
+ * This function reads the contents of an 8-bit register location by
+ *transmitting the register location byte along with a null (0x00) byte using
+ *the standard WPILib API. The response (two bytes) is read back using the
+ *WPILib API and joined using a helper function. This function assumes the
+ *controller is set to standard SPI mode.
+ **/
+uint16_t ADIS16470_IMU::ReadRegister(uint8_t reg) {
+ uint8_t buf[2];
+ buf[0] = reg & 0x7f;
+ buf[1] = 0;
+
+ m_spi->Write(buf, 2);
+ m_spi->Read(false, buf, 2);
+
+ return ToUShort(buf);
+}
+
+/**
+ * @brief Writes an unsigned, 16-bit value to two adjacent, 8-bit register
+ *locations over SPI.
+ *
+ * @param reg An unsigned, 8-bit register location.
+ *
+ * @param val An unsigned, 16-bit value to be written to the specified register
+ *location.
+ *
+ * This function writes an unsigned, 16-bit value into adjacent 8-bit addresses
+ *via SPI. The upper and lower bytes that make up the 16-bit value are split
+ *into two unsined, 8-bit values and written to the upper and lower addresses of
+ *the specified register value. Only the lower (base) address must be specified.
+ *This function assumes the controller is set to standard SPI mode.
+ **/
+void ADIS16470_IMU::WriteRegister(uint8_t reg, uint16_t val) {
+ uint8_t buf[2];
+ buf[0] = 0x80 | reg;
+ buf[1] = val & 0xff;
+ m_spi->Write(buf, 2);
+ buf[0] = 0x81 | reg;
+ buf[1] = val >> 8;
+ m_spi->Write(buf, 2);
+}
+
+/**
+ * @brief Resets (zeros) the xgyro, ygyro, and zgyro angle integrations.
+ *
+ * This function resets (zeros) the accumulated (integrated) angle estimates for
+ *the xgyro, ygyro, and zgyro outputs.
+ **/
+void ADIS16470_IMU::Reset() {
+ std::scoped_lock sync(m_mutex);
+ m_integ_angle = 0.0;
+}
+
+void ADIS16470_IMU::Close() {
+ if (m_thread_active) {
+ m_thread_active = false;
+ if (m_acquire_task.joinable()) {
+ m_acquire_task.join();
+ }
+ }
+ if (m_spi != nullptr) {
+ if (m_auto_configured) {
+ m_spi->StopAuto();
+ }
+ delete m_spi;
+ m_auto_configured = false;
+ if (m_auto_interrupt != nullptr) {
+ delete m_auto_interrupt;
+ m_auto_interrupt = nullptr;
+ }
+ m_spi = nullptr;
+ }
+ std::cout << "Finished cleaning up after the IMU driver." << std::endl;
+}
+
+ADIS16470_IMU::~ADIS16470_IMU() {
+ Close();
+}
+
+/**
+ * @brief Main acquisition loop. Typically called asynchronously and free-wheels
+ *while the robot code is active.
+ *
+ * This is the main acquisiton loop for the IMU. During each iteration, data
+ *read using auto SPI is extracted from the FPGA FIFO, split, scaled, and
+ *integrated. Each X, Y, and Z value is 32-bits split across 4 indices (bytes)
+ *in the buffer. Auto SPI puts one byte in each index location. Each index is
+ *32-bits wide because the timestamp is an unsigned 32-bit int. The timestamp is
+ *always located at the beginning of the frame. Two indices (request_1 and
+ *request_2 below) are always invalid (garbage) and can be disregarded.
+ *
+ * Data order: [timestamp, request_1, request_2, x_1, x_2, x_3, x_4, y_1, y_2,
+ *y_3, y_4, z_1, z_2, z_3, z_4] x = delta x (32-bit x_1 = highest bit) y = delta
+ *y (32-bit y_1 = highest bit) z = delta z (32-bit z_1 = highest bit)
+ *
+ * Complementary filter code was borrowed from
+ *https://github.com/tcleg/Six_Axis_Complementary_Filter
+ **/
+void ADIS16470_IMU::Acquire() {
+ // Set data packet length
+ const int dataset_len = 19; // 18 data points + timestamp
+
+ /* Fixed buffer size */
+ const int BUFFER_SIZE = 4000;
+
+ // This buffer can contain many datasets
+ uint32_t buffer[BUFFER_SIZE];
+ int data_count = 0;
+ int data_remainder = 0;
+ int data_to_read = 0;
+ uint32_t previous_timestamp = 0;
+ double delta_angle = 0.0;
+ double gyro_x = 0.0;
+ double gyro_y = 0.0;
+ double gyro_z = 0.0;
+ double accel_x = 0.0;
+ double accel_y = 0.0;
+ double accel_z = 0.0;
+ double gyro_x_si = 0.0;
+ double gyro_y_si = 0.0;
+ // double gyro_z_si = 0.0;
+ double accel_x_si = 0.0;
+ double accel_y_si = 0.0;
+ double accel_z_si = 0.0;
+ double compAngleX = 0.0;
+ double compAngleY = 0.0;
+ double accelAngleX = 0.0;
+ double accelAngleY = 0.0;
+
+ while (true) {
+ // Sleep loop for 10ms (wait for data)
+ Wait(10_ms);
+
+ if (m_thread_active) {
+ m_thread_idle = false;
+
+ data_count = m_spi->ReadAutoReceivedData(
+ buffer, 0,
+ 0_s); // Read number of bytes currently stored in the buffer
+ data_remainder =
+ data_count % dataset_len; // Check if frame is incomplete. Add 1
+ // because of timestamp
+ data_to_read = data_count -
+ data_remainder; // Remove incomplete data from read count
+ /* Want to cap the data to read in a single read at the buffer size */
+ if (data_to_read > BUFFER_SIZE) {
+ REPORT_WARNING(
+ "ADIS16470 data processing thread overrun has occurred!");
+ data_to_read = BUFFER_SIZE - (BUFFER_SIZE % dataset_len);
+ }
+ m_spi->ReadAutoReceivedData(
+ buffer, data_to_read,
+ 0_s); // Read data from DMA buffer (only complete sets)
+
+ /*
+ // DEBUG: Print buffer size and contents to terminal
+ std::cout << "Start - " << data_count << "," << data_remainder << "," <<
+ data_to_read << std::endl; for (int m = 0; m < data_to_read - 1; m++ )
+ {
+ std::cout << buffer[m] << ",";
+ }
+ std::cout << " " << std::endl;
+ std::cout << "End" << std::endl;
+ std::cout << "Reading " << data_count << " bytes." << std::endl;
+ */
+
+ // Could be multiple data sets in the buffer. Handle each one.
+ for (int i = 0; i < data_to_read; i += dataset_len) {
+ // Timestamp is at buffer[i]
+ m_dt = (buffer[i] - previous_timestamp) / 1000000.0;
+ /* Get delta angle value for selected yaw axis and scale by the elapsed
+ * time (based on timestamp) */
+ delta_angle = (ToInt(&buffer[i + 3]) * delta_angle_sf) /
+ (m_scaled_sample_rate / (buffer[i] - previous_timestamp));
+ gyro_x = (BuffToShort(&buffer[i + 7]) / 10.0);
+ gyro_y = (BuffToShort(&buffer[i + 9]) / 10.0);
+ gyro_z = (BuffToShort(&buffer[i + 11]) / 10.0);
+ accel_x = (BuffToShort(&buffer[i + 13]) / 800.0);
+ accel_y = (BuffToShort(&buffer[i + 15]) / 800.0);
+ accel_z = (BuffToShort(&buffer[i + 17]) / 800.0);
+
+ // Convert scaled sensor data to SI units
+ gyro_x_si = gyro_x * deg_to_rad;
+ gyro_y_si = gyro_y * deg_to_rad;
+ // gyro_z_si = gyro_z * deg_to_rad;
+ accel_x_si = accel_x * grav;
+ accel_y_si = accel_y * grav;
+ accel_z_si = accel_z * grav;
+
+ // Store timestamp for next iteration
+ previous_timestamp = buffer[i];
+
+ /*
+ // DEBUG: Print timestamp and delta values
+ std::cout << previous_timestamp << "," << delta_x << "," << delta_y <<
+ "," << delta_z << std::endl;
+ */
+
+ m_alpha = m_tau / (m_tau + m_dt);
+
+ if (m_first_run) {
+ accelAngleX = atan2f(accel_x_si, sqrtf((accel_y_si * accel_y_si) +
+ (accel_z_si * accel_z_si)));
+ accelAngleY = atan2f(accel_y_si, sqrtf((accel_x_si * accel_x_si) +
+ (accel_z_si * accel_z_si)));
+ compAngleX = accelAngleX;
+ compAngleY = accelAngleY;
+ } else {
+ // Process X angle
+ accelAngleX = atan2f(accel_x_si, sqrtf((accel_y_si * accel_y_si) +
+ (accel_z_si * accel_z_si)));
+ accelAngleY = atan2f(accel_y_si, sqrtf((accel_x_si * accel_x_si) +
+ (accel_z_si * accel_z_si)));
+ accelAngleX = FormatAccelRange(accelAngleX, accel_z_si);
+ accelAngleY = FormatAccelRange(accelAngleY, accel_z_si);
+ compAngleX = CompFilterProcess(compAngleX, accelAngleX, -gyro_y_si);
+ compAngleY = CompFilterProcess(compAngleY, accelAngleY, gyro_x_si);
+ }
+
+ // DEBUG: Print accumulated values
+ // std::cout << m_compAngleX << "," << m_compAngleY << std::endl;
+
+ {
+ std::scoped_lock sync(m_mutex);
+ /* Push data to global variables */
+ if (m_first_run) {
+ /* Don't accumulate first run. previous_timestamp will be "very" old
+ * and the integration will end up way off */
+ m_integ_angle = 0.0;
+ } else {
+ m_integ_angle += delta_angle;
+ }
+ m_gyro_x = gyro_x;
+ m_gyro_y = gyro_y;
+ m_gyro_z = gyro_z;
+ m_accel_x = accel_x;
+ m_accel_y = accel_y;
+ m_accel_z = accel_z;
+ m_compAngleX = compAngleX * rad_to_deg;
+ m_compAngleY = compAngleY * rad_to_deg;
+ m_accelAngleX = accelAngleX * rad_to_deg;
+ m_accelAngleY = accelAngleY * rad_to_deg;
+ }
+ m_first_run = false;
+ }
+ } else {
+ m_thread_idle = true;
+ data_count = 0;
+ data_remainder = 0;
+ data_to_read = 0;
+ previous_timestamp = 0.0;
+ delta_angle = 0.0;
+ gyro_x = 0.0;
+ gyro_y = 0.0;
+ gyro_z = 0.0;
+ accel_x = 0.0;
+ accel_y = 0.0;
+ accel_z = 0.0;
+ gyro_x_si = 0.0;
+ gyro_y_si = 0.0;
+ // gyro_z_si = 0.0;
+ accel_x_si = 0.0;
+ accel_y_si = 0.0;
+ accel_z_si = 0.0;
+ compAngleX = 0.0;
+ compAngleY = 0.0;
+ accelAngleX = 0.0;
+ accelAngleY = 0.0;
+ }
+ }
+}
+
+/* Complementary filter functions */
+double ADIS16470_IMU::FormatFastConverge(double compAngle, double accAngle) {
+ if (compAngle > accAngle + wpi::numbers::pi) {
+ compAngle = compAngle - 2.0 * wpi::numbers::pi;
+ } else if (accAngle > compAngle + wpi::numbers::pi) {
+ compAngle = compAngle + 2.0 * wpi::numbers::pi;
+ }
+ return compAngle;
+}
+
+double ADIS16470_IMU::FormatRange0to2PI(double compAngle) {
+ while (compAngle >= 2 * wpi::numbers::pi) {
+ compAngle = compAngle - 2.0 * wpi::numbers::pi;
+ }
+ while (compAngle < 0.0) {
+ compAngle = compAngle + 2.0 * wpi::numbers::pi;
+ }
+ return compAngle;
+}
+
+double ADIS16470_IMU::FormatAccelRange(double accelAngle, double accelZ) {
+ if (accelZ < 0.0) {
+ accelAngle = wpi::numbers::pi - accelAngle;
+ } else if (accelZ > 0.0 && accelAngle < 0.0) {
+ accelAngle = 2.0 * wpi::numbers::pi + accelAngle;
+ }
+ return accelAngle;
+}
+
+double ADIS16470_IMU::CompFilterProcess(double compAngle, double accelAngle,
+ double omega) {
+ compAngle = FormatFastConverge(compAngle, accelAngle);
+ compAngle =
+ m_alpha * (compAngle + omega * m_dt) + (1.0 - m_alpha) * accelAngle;
+ compAngle = FormatRange0to2PI(compAngle);
+ if (compAngle > wpi::numbers::pi) {
+ compAngle = compAngle - 2.0 * wpi::numbers::pi;
+ }
+ return compAngle;
+}
+
+/**
+ * @brief Returns the current integrated angle for the axis specified.
+ *
+ * @param m_yaw_axis An enum indicating the axis chosen to act as the yaw axis.
+ *
+ * @return The current integrated angle in degrees.
+ *
+ * This function returns the most recent integrated angle for the axis chosen by
+ *m_yaw_axis. This function is most useful in situations where the yaw axis may
+ *not coincide with the IMU Z axis.
+ **/
+double ADIS16470_IMU::GetAngle() const {
+ std::scoped_lock sync(m_mutex);
+ return m_integ_angle;
+}
+
+double ADIS16470_IMU::GetRate() const {
+ std::scoped_lock sync(m_mutex);
+ switch (m_yaw_axis) {
+ case kX:
+ return m_gyro_x;
+ case kY:
+ return m_gyro_y;
+ case kZ:
+ return m_gyro_z;
+ default:
+ return 0.0;
+ }
+}
+
+ADIS16470_IMU::IMUAxis ADIS16470_IMU::GetYawAxis() const {
+ return m_yaw_axis;
+}
+
+double ADIS16470_IMU::GetGyroInstantX() const {
+ std::scoped_lock sync(m_mutex);
+ return m_gyro_x;
+}
+
+double ADIS16470_IMU::GetGyroInstantY() const {
+ std::scoped_lock sync(m_mutex);
+ return m_gyro_y;
+}
+
+double ADIS16470_IMU::GetGyroInstantZ() const {
+ std::scoped_lock sync(m_mutex);
+ return m_gyro_z;
+}
+
+double ADIS16470_IMU::GetAccelInstantX() const {
+ std::scoped_lock sync(m_mutex);
+ return m_accel_x;
+}
+
+double ADIS16470_IMU::GetAccelInstantY() const {
+ std::scoped_lock sync(m_mutex);
+ return m_accel_y;
+}
+
+double ADIS16470_IMU::GetAccelInstantZ() const {
+ std::scoped_lock sync(m_mutex);
+ return m_accel_z;
+}
+
+double ADIS16470_IMU::GetXComplementaryAngle() const {
+ std::scoped_lock sync(m_mutex);
+ return m_compAngleX;
+}
+
+double ADIS16470_IMU::GetYComplementaryAngle() const {
+ std::scoped_lock sync(m_mutex);
+ return m_compAngleY;
+}
+
+double ADIS16470_IMU::GetXFilteredAccelAngle() const {
+ std::scoped_lock sync(m_mutex);
+ return m_accelAngleX;
+}
+
+double ADIS16470_IMU::GetYFilteredAccelAngle() const {
+ std::scoped_lock sync(m_mutex);
+ return m_accelAngleY;
+}
+
+/**
+ * @brief Builds a Sendable object to push IMU data to the driver station.
+ *
+ * This function pushes the most recent angle estimates for all axes to the
+ *driver station.
+ **/
+void ADIS16470_IMU::InitSendable(nt::NTSendableBuilder& builder) {
+ builder.SetSmartDashboardType("ADIS16470 IMU");
+ auto yaw_angle = builder.GetEntry("Yaw Angle").GetHandle();
+ builder.SetUpdateTable(
+ [=]() { nt::NetworkTableEntry(yaw_angle).SetDouble(GetAngle()); });
+}
diff --git a/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h b/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h
new file mode 100644
index 0000000000..64c11f3b90
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h
@@ -0,0 +1,378 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2020 Analog Devices Inc. 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. */
+/* */
+/* Modified by Juan Chong - frcsupport@analog.com */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+
+// Not always defined in cmath (not part of standard)
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+namespace frc {
+
+/** @brief ADIS16448 Register Map Declaration */
+static constexpr uint8_t FLASH_CNT = 0x00; // Flash memory write count
+static constexpr uint8_t XGYRO_OUT = 0x04; // X-axis gyroscope output
+static constexpr uint8_t YGYRO_OUT = 0x06; // Y-axis gyroscope output
+static constexpr uint8_t ZGYRO_OUT = 0x08; // Z-axis gyroscope output
+static constexpr uint8_t XACCL_OUT = 0x0A; // X-axis accelerometer output
+static constexpr uint8_t YACCL_OUT = 0x0C; // Y-axis accelerometer output
+static constexpr uint8_t ZACCL_OUT = 0x0E; // Z-axis accelerometer output
+static constexpr uint8_t XMAGN_OUT = 0x10; // X-axis magnetometer output
+static constexpr uint8_t YMAGN_OUT = 0x12; // Y-axis magnetometer output
+static constexpr uint8_t ZMAGN_OUT = 0x14; // Z-axis magnetometer output
+static constexpr uint8_t BARO_OUT =
+ 0x16; // Barometer pressure measurement, high word
+static constexpr uint8_t TEMP_OUT = 0x18; // Temperature output
+static constexpr uint8_t XGYRO_OFF =
+ 0x1A; // X-axis gyroscope bias offset factor
+static constexpr uint8_t YGYRO_OFF =
+ 0x1C; // Y-axis gyroscope bias offset factor
+static constexpr uint8_t ZGYRO_OFF =
+ 0x1E; // Z-axis gyroscope bias offset factor
+static constexpr uint8_t XACCL_OFF =
+ 0x20; // X-axis acceleration bias offset factor
+static constexpr uint8_t YACCL_OFF =
+ 0x22; // Y-axis acceleration bias offset factor
+static constexpr uint8_t ZACCL_OFF =
+ 0x24; // Z-axis acceleration bias offset factor
+static constexpr uint8_t XMAGN_HIC =
+ 0x26; // X-axis magnetometer, hard iron factor
+static constexpr uint8_t YMAGN_HIC =
+ 0x28; // Y-axis magnetometer, hard iron factor
+static constexpr uint8_t ZMAGN_HIC =
+ 0x2A; // Z-axis magnetometer, hard iron factor
+static constexpr uint8_t XMAGN_SIC =
+ 0x2C; // X-axis magnetometer, soft iron factor
+static constexpr uint8_t YMAGN_SIC =
+ 0x2E; // Y-axis magnetometer, soft iron factor
+static constexpr uint8_t ZMAGN_SIC =
+ 0x30; // Z-axis magnetometer, soft iron factor
+static constexpr uint8_t GPIO_CTRL = 0x32; // GPIO control
+static constexpr uint8_t MSC_CTRL = 0x34; // MISC control
+static constexpr uint8_t SMPL_PRD =
+ 0x36; // Sample clock/Decimation filter control
+static constexpr uint8_t SENS_AVG = 0x38; // Digital filter control
+static constexpr uint8_t SEQ_CNT = 0x3A; // MAGN_OUT and BARO_OUT counter
+static constexpr uint8_t DIAG_STAT = 0x3C; // System status
+static constexpr uint8_t GLOB_CMD = 0x3E; // System command
+static constexpr uint8_t ALM_MAG1 = 0x40; // Alarm 1 amplitude threshold
+static constexpr uint8_t ALM_MAG2 = 0x42; // Alarm 2 amplitude threshold
+static constexpr uint8_t ALM_SMPL1 = 0x44; // Alarm 1 sample size
+static constexpr uint8_t ALM_SMPL2 = 0x46; // Alarm 2 sample size
+static constexpr uint8_t ALM_CTRL = 0x48; // Alarm control
+static constexpr uint8_t LOT_ID1 = 0x52; // Lot identification number
+static constexpr uint8_t LOT_ID2 = 0x54; // Lot identification number
+static constexpr uint8_t PROD_ID = 0x56; // Product identifier
+static constexpr uint8_t SERIAL_NUM = 0x58; // Lot-specific serial number
+
+/** @brief ADIS16448 Static Constants */
+const double rad_to_deg = 57.2957795;
+const double deg_to_rad = 0.0174532;
+const double grav = 9.81;
+
+/** @brief struct to store offset data */
+struct offset_data {
+ double m_accum_gyro_x = 0.0;
+ double m_accum_gyro_y = 0.0;
+ double m_accum_gyro_z = 0.0;
+};
+
+/**
+ * Use DMA SPI to read rate, acceleration, and magnetometer data from the
+ * ADIS16448 IMU and return the robots heading relative to a starting position,
+ * AHRS, and instant measurements
+ *
+ * The ADIS16448 gyro angle outputs track the robot's heading based on the
+ * starting position. As the robot rotates the new heading is computed by
+ * integrating the rate of rotation returned by the IMU. When the class is
+ * instantiated, a short calibration routine is performed where the IMU samples
+ * the gyros while at rest to determine the initial offset. This is subtracted
+ * from each sample to determine the heading.
+ *
+ * This class is for the ADIS16448 IMU connected via the SPI port available on
+ * the RoboRIO MXP port.
+ */
+
+class ADIS16448_IMU : public nt::NTSendable,
+ public wpi::SendableHelper {
+ public:
+ enum IMUAxis { kX, kY, kZ };
+
+ /**
+ * IMU constructor on onboard MXP CS0, Z-up orientation, and complementary
+ * AHRS computation.
+ */
+ ADIS16448_IMU();
+
+ /**
+ * IMU constructor on the specified MXP port and orientation.
+ *
+ * @param yaw_axis The axis where gravity is present. Valid options are kX,
+ * kY, and kZ
+ * @param port The SPI port where the IMU is connected.
+ * @param cal_time The calibration time that should be used on start-up.
+ */
+ explicit ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port, uint16_t cal_time);
+
+ ~ADIS16448_IMU() override;
+
+ ADIS16448_IMU(ADIS16448_IMU&&) = default;
+ ADIS16448_IMU& operator=(ADIS16448_IMU&&) = default;
+
+ /**
+ * Initialize the IMU.
+ *
+ * Perform gyro offset calibration by collecting data for a number of seconds
+ * and computing the center value. The center value is subtracted from
+ * subsequent measurements.
+ *
+ * It's important to make sure that the robot is not moving while the
+ * centering calculations are in progress, this is typically done when the
+ * robot is first turned on while it's sitting at rest before the match
+ * starts.
+ *
+ * The calibration routine can be triggered by the user during runtime.
+ */
+ void Calibrate();
+
+ /**
+ * Configures the calibration time used for the next calibrate.
+ *
+ * @param new_cal_time The calibration time that should be used
+ */
+ int ConfigCalTime(int new_cal_time);
+
+ /**
+ * Reset the gyro.
+ *
+ * Resets the gyro accumulations to a heading of zero. This can be used if
+ * there is significant drift in the gyro and it needs to be recalibrated
+ * after running.
+ */
+ void Reset();
+
+ /**
+ * Return the actual angle in degrees that the robot is currently facing.
+ *
+ * The angle is based on the current accumulator value corrected by
+ * offset calibration and built-in IMU calibration. The angle is continuous,
+ * that is it will continue from 360->361 degrees. This allows algorithms
+ * that wouldn't want to see a discontinuity in the gyro output as it sweeps
+ * from 360 to 0 on the second time around. The axis returned by this
+ * function is adjusted fased on the configured yaw_axis.
+ *
+ * @return the current heading of the robot in degrees. This heading is based
+ * on integration of the returned rate from the gyro.
+ */
+ double GetAngle() const;
+
+ /**
+ * Return the rate of rotation of the yaw_axis gyro.
+ *
+ * The rate is based on the most recent reading of the gyro value
+ *
+ * @return the current rate in degrees per second
+ */
+ double GetRate() const;
+
+ double GetGyroAngleX() const;
+
+ double GetGyroAngleY() const;
+
+ double GetGyroAngleZ() const;
+
+ double GetGyroInstantX() const;
+
+ double GetGyroInstantY() const;
+
+ double GetGyroInstantZ() const;
+
+ double GetAccelInstantX() const;
+
+ double GetAccelInstantY() const;
+
+ double GetAccelInstantZ() const;
+
+ double GetXComplementaryAngle() const;
+
+ double GetYComplementaryAngle() const;
+
+ double GetXFilteredAccelAngle() const;
+
+ double GetYFilteredAccelAngle() const;
+
+ double GetMagInstantX() const;
+
+ double GetMagInstantY() const;
+
+ double GetMagInstantZ() const;
+
+ double GetBarometricPressure() const;
+
+ double GetTemperature() const;
+
+ IMUAxis GetYawAxis() const;
+
+ int SetYawAxis(IMUAxis yaw_axis);
+
+ int ConfigDecRate(uint16_t DecimationRate);
+
+ void InitSendable(nt::NTSendableBuilder& builder) override;
+
+ private:
+ bool SwitchToStandardSPI();
+
+ bool SwitchToAutoSPI();
+
+ uint16_t ReadRegister(uint8_t reg);
+
+ void WriteRegister(uint8_t reg, uint16_t val);
+
+ void Acquire();
+
+ void Close();
+
+ // User-specified yaw axis
+ IMUAxis m_yaw_axis;
+
+ // Last read values (post-scaling)
+ double m_gyro_x = 0.0;
+ double m_gyro_y = 0.0;
+ double m_gyro_z = 0.0;
+ double m_accel_x = 0.0;
+ double m_accel_y = 0.0;
+ double m_accel_z = 0.0;
+ double m_mag_x = 0.0;
+ double m_mag_y = 0.0;
+ double m_mag_z = 0.0;
+ double m_baro = 0.0;
+ double m_temp = 0.0;
+
+ // Complementary filter variables
+ double m_tau = 0.5;
+ double m_dt, m_alpha = 0.0;
+ double m_compAngleX, m_compAngleY, m_accelAngleX, m_accelAngleY = 0.0;
+
+ // vector for storing most recent imu values
+ offset_data* m_offset_buffer = nullptr;
+
+ double m_gyro_offset_x = 0.0;
+ double m_gyro_offset_y = 0.0;
+ double m_gyro_offset_z = 0.0;
+
+ // function to re-init offset buffer
+ void InitOffsetBuffer(int size);
+
+ // Accumulated gyro values (for offset calculation)
+ int m_avg_size = 0;
+ int m_accum_count = 0;
+
+ // Integrated gyro values
+ double m_integ_gyro_x = 0.0;
+ double m_integ_gyro_y = 0.0;
+ double m_integ_gyro_z = 0.0;
+
+ // Complementary filter functions
+ double FormatFastConverge(double compAngle, double accAngle);
+
+ double FormatRange0to2PI(double compAngle);
+
+ double FormatAccelRange(double accelAngle, double accelZ);
+
+ double CompFilterProcess(double compAngle, double accelAngle, double omega);
+
+ // State and resource variables
+ volatile bool m_thread_active = false;
+ volatile bool m_first_run = true;
+ volatile bool m_thread_idle = false;
+ volatile bool m_start_up_mode = true;
+
+ bool m_auto_configured = false;
+ SPI::Port m_spi_port;
+ uint16_t m_calibration_time;
+ SPI* m_spi = nullptr;
+ DigitalInput* m_auto_interrupt = nullptr;
+
+ std::thread m_acquire_task;
+
+ struct NonMovableMutexWrapper {
+ wpi::mutex mutex;
+ NonMovableMutexWrapper() = default;
+
+ NonMovableMutexWrapper(const NonMovableMutexWrapper&) = delete;
+ NonMovableMutexWrapper& operator=(const NonMovableMutexWrapper&) = delete;
+
+ NonMovableMutexWrapper(NonMovableMutexWrapper&&) {}
+ NonMovableMutexWrapper& operator=(NonMovableMutexWrapper&&) {
+ return *this;
+ }
+
+ void lock() { mutex.lock(); }
+
+ void unlock() { mutex.unlock(); }
+
+ bool try_lock() noexcept { return mutex.try_lock(); }
+ };
+
+ mutable NonMovableMutexWrapper m_mutex;
+
+ // CRC-16 Look-Up Table
+ static constexpr uint16_t adiscrc[256] = {
+ 0x0000, 0x17CE, 0x0FDF, 0x1811, 0x1FBE, 0x0870, 0x1061, 0x07AF, 0x1F3F,
+ 0x08F1, 0x10E0, 0x072E, 0x0081, 0x174F, 0x0F5E, 0x1890, 0x1E3D, 0x09F3,
+ 0x11E2, 0x062C, 0x0183, 0x164D, 0x0E5C, 0x1992, 0x0102, 0x16CC, 0x0EDD,
+ 0x1913, 0x1EBC, 0x0972, 0x1163, 0x06AD, 0x1C39, 0x0BF7, 0x13E6, 0x0428,
+ 0x0387, 0x1449, 0x0C58, 0x1B96, 0x0306, 0x14C8, 0x0CD9, 0x1B17, 0x1CB8,
+ 0x0B76, 0x1367, 0x04A9, 0x0204, 0x15CA, 0x0DDB, 0x1A15, 0x1DBA, 0x0A74,
+ 0x1265, 0x05AB, 0x1D3B, 0x0AF5, 0x12E4, 0x052A, 0x0285, 0x154B, 0x0D5A,
+ 0x1A94, 0x1831, 0x0FFF, 0x17EE, 0x0020, 0x078F, 0x1041, 0x0850, 0x1F9E,
+ 0x070E, 0x10C0, 0x08D1, 0x1F1F, 0x18B0, 0x0F7E, 0x176F, 0x00A1, 0x060C,
+ 0x11C2, 0x09D3, 0x1E1D, 0x19B2, 0x0E7C, 0x166D, 0x01A3, 0x1933, 0x0EFD,
+ 0x16EC, 0x0122, 0x068D, 0x1143, 0x0952, 0x1E9C, 0x0408, 0x13C6, 0x0BD7,
+ 0x1C19, 0x1BB6, 0x0C78, 0x1469, 0x03A7, 0x1B37, 0x0CF9, 0x14E8, 0x0326,
+ 0x0489, 0x1347, 0x0B56, 0x1C98, 0x1A35, 0x0DFB, 0x15EA, 0x0224, 0x058B,
+ 0x1245, 0x0A54, 0x1D9A, 0x050A, 0x12C4, 0x0AD5, 0x1D1B, 0x1AB4, 0x0D7A,
+ 0x156B, 0x02A5, 0x1021, 0x07EF, 0x1FFE, 0x0830, 0x0F9F, 0x1851, 0x0040,
+ 0x178E, 0x0F1E, 0x18D0, 0x00C1, 0x170F, 0x10A0, 0x076E, 0x1F7F, 0x08B1,
+ 0x0E1C, 0x19D2, 0x01C3, 0x160D, 0x11A2, 0x066C, 0x1E7D, 0x09B3, 0x1123,
+ 0x06ED, 0x1EFC, 0x0932, 0x0E9D, 0x1953, 0x0142, 0x168C, 0x0C18, 0x1BD6,
+ 0x03C7, 0x1409, 0x13A6, 0x0468, 0x1C79, 0x0BB7, 0x1327, 0x04E9, 0x1CF8,
+ 0x0B36, 0x0C99, 0x1B57, 0x0346, 0x1488, 0x1225, 0x05EB, 0x1DFA, 0x0A34,
+ 0x0D9B, 0x1A55, 0x0244, 0x158A, 0x0D1A, 0x1AD4, 0x02C5, 0x150B, 0x12A4,
+ 0x056A, 0x1D7B, 0x0AB5, 0x0810, 0x1FDE, 0x07CF, 0x1001, 0x17AE, 0x0060,
+ 0x1871, 0x0FBF, 0x172F, 0x00E1, 0x18F0, 0x0F3E, 0x0891, 0x1F5F, 0x074E,
+ 0x1080, 0x162D, 0x01E3, 0x19F2, 0x0E3C, 0x0993, 0x1E5D, 0x064C, 0x1182,
+ 0x0912, 0x1EDC, 0x06CD, 0x1103, 0x16AC, 0x0162, 0x1973, 0x0EBD, 0x1429,
+ 0x03E7, 0x1BF6, 0x0C38, 0x0B97, 0x1C59, 0x0448, 0x1386, 0x0B16, 0x1CD8,
+ 0x04C9, 0x1307, 0x14A8, 0x0366, 0x1B77, 0x0CB9, 0x0A14, 0x1DDA, 0x05CB,
+ 0x1205, 0x15AA, 0x0264, 0x1A75, 0x0DBB, 0x152B, 0x02E5, 0x1AF4, 0x0D3A,
+ 0x0A95, 0x1D5B, 0x054A, 0x1284};
+};
+
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h b/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h
new file mode 100644
index 0000000000..d515f14166
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h
@@ -0,0 +1,390 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2020 Analog Devices Inc. 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. */
+/* */
+/* Juan Chong - frcsupport@analog.com */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+
+namespace frc {
+
+/* ADIS16470 Calibration Time Enum Class */
+enum class ADIS16470CalibrationTime {
+ _32ms = 0,
+ _64ms = 1,
+ _128ms = 2,
+ _256ms = 3,
+ _512ms = 4,
+ _1s = 5,
+ _2s = 6,
+ _4s = 7,
+ _8s = 8,
+ _16s = 9,
+ _32s = 10,
+ _64s = 11
+};
+
+/* ADIS16470 Register Map Declaration */
+static constexpr uint8_t FLASH_CNT = 0x00; // Flash memory write count
+static constexpr uint8_t DIAG_STAT = 0x02; // Diagnostic and operational status
+static constexpr uint8_t X_GYRO_LOW =
+ 0x04; // X-axis gyroscope output, lower word
+static constexpr uint8_t X_GYRO_OUT =
+ 0x06; // X-axis gyroscope output, upper word
+static constexpr uint8_t Y_GYRO_LOW =
+ 0x08; // Y-axis gyroscope output, lower word
+static constexpr uint8_t Y_GYRO_OUT =
+ 0x0A; // Y-axis gyroscope output, upper word
+static constexpr uint8_t Z_GYRO_LOW =
+ 0x0C; // Z-axis gyroscope output, lower word
+static constexpr uint8_t Z_GYRO_OUT =
+ 0x0E; // Z-axis gyroscope output, upper word
+static constexpr uint8_t X_ACCL_LOW =
+ 0x10; // X-axis accelerometer output, lower word
+static constexpr uint8_t X_ACCL_OUT =
+ 0x12; // X-axis accelerometer output, upper word
+static constexpr uint8_t Y_ACCL_LOW =
+ 0x14; // Y-axis accelerometer output, lower word
+static constexpr uint8_t Y_ACCL_OUT =
+ 0x16; // Y-axis accelerometer output, upper word
+static constexpr uint8_t Z_ACCL_LOW =
+ 0x18; // Z-axis accelerometer output, lower word
+static constexpr uint8_t Z_ACCL_OUT =
+ 0x1A; // Z-axis accelerometer output, upper word
+static constexpr uint8_t TEMP_OUT =
+ 0x1C; // Temperature output (internal, not calibrated)
+static constexpr uint8_t TIME_STAMP = 0x1E; // PPS mode time stamp
+static constexpr uint8_t X_DELTANG_LOW =
+ 0x24; // X-axis delta angle output, lower word
+static constexpr uint8_t X_DELTANG_OUT =
+ 0x26; // X-axis delta angle output, upper word
+static constexpr uint8_t Y_DELTANG_LOW =
+ 0x28; // Y-axis delta angle output, lower word
+static constexpr uint8_t Y_DELTANG_OUT =
+ 0x2A; // Y-axis delta angle output, upper word
+static constexpr uint8_t Z_DELTANG_LOW =
+ 0x2C; // Z-axis delta angle output, lower word
+static constexpr uint8_t Z_DELTANG_OUT =
+ 0x2E; // Z-axis delta angle output, upper word
+static constexpr uint8_t X_DELTVEL_LOW =
+ 0x30; // X-axis delta velocity output, lower word
+static constexpr uint8_t X_DELTVEL_OUT =
+ 0x32; // X-axis delta velocity output, upper word
+static constexpr uint8_t Y_DELTVEL_LOW =
+ 0x34; // Y-axis delta velocity output, lower word
+static constexpr uint8_t Y_DELTVEL_OUT =
+ 0x36; // Y-axis delta velocity output, upper word
+static constexpr uint8_t Z_DELTVEL_LOW =
+ 0x38; // Z-axis delta velocity output, lower word
+static constexpr uint8_t Z_DELTVEL_OUT =
+ 0x3A; // Z-axis delta velocity output, upper word
+static constexpr uint8_t XG_BIAS_LOW =
+ 0x40; // X-axis gyroscope bias offset correction, lower word
+static constexpr uint8_t XG_BIAS_HIGH =
+ 0x42; // X-axis gyroscope bias offset correction, upper word
+static constexpr uint8_t YG_BIAS_LOW =
+ 0x44; // Y-axis gyroscope bias offset correction, lower word
+static constexpr uint8_t YG_BIAS_HIGH =
+ 0x46; // Y-axis gyroscope bias offset correction, upper word
+static constexpr uint8_t ZG_BIAS_LOW =
+ 0x48; // Z-axis gyroscope bias offset correction, lower word
+static constexpr uint8_t ZG_BIAS_HIGH =
+ 0x4A; // Z-axis gyroscope bias offset correction, upper word
+static constexpr uint8_t XA_BIAS_LOW =
+ 0x4C; // X-axis accelerometer bias offset correction, lower word
+static constexpr uint8_t XA_BIAS_HIGH =
+ 0x4E; // X-axis accelerometer bias offset correction, upper word
+static constexpr uint8_t YA_BIAS_LOW =
+ 0x50; // Y-axis accelerometer bias offset correction, lower word
+static constexpr uint8_t YA_BIAS_HIGH =
+ 0x52; // Y-axis accelerometer bias offset correction, upper word
+static constexpr uint8_t ZA_BIAS_LOW =
+ 0x54; // Z-axis accelerometer bias offset correction, lower word
+static constexpr uint8_t ZA_BIAS_HIGH =
+ 0x56; // Z-axis accelerometer bias offset correction, upper word
+static constexpr uint8_t FILT_CTRL = 0x5C; // Filter control
+static constexpr uint8_t MSC_CTRL = 0x60; // Miscellaneous control
+static constexpr uint8_t UP_SCALE = 0x62; // Clock scale factor, PPS mode
+static constexpr uint8_t DEC_RATE =
+ 0x64; // Decimation rate control (output data rate)
+static constexpr uint8_t NULL_CNFG = 0x66; // Auto-null configuration control
+static constexpr uint8_t GLOB_CMD = 0x68; // Global commands
+static constexpr uint8_t FIRM_REV = 0x6C; // Firmware revision
+static constexpr uint8_t FIRM_DM =
+ 0x6E; // Firmware revision date, month and day
+static constexpr uint8_t FIRM_Y = 0x70; // Firmware revision date, year
+static constexpr uint8_t PROD_ID = 0x72; // Product identification
+static constexpr uint8_t SERIAL_NUM =
+ 0x74; // Serial number (relative to assembly lot)
+static constexpr uint8_t USER_SCR1 = 0x76; // User scratch register 1
+static constexpr uint8_t USER_SCR2 = 0x78; // User scratch register 2
+static constexpr uint8_t USER_SCR3 = 0x7A; // User scratch register 3
+static constexpr uint8_t FLSHCNT_LOW = 0x7C; // Flash update count, lower word
+static constexpr uint8_t FLSHCNT_HIGH = 0x7E; // Flash update count, upper word
+
+/* ADIS16470 Auto SPI Data Packets */
+static constexpr uint8_t m_autospi_x_packet[16] = {
+ X_DELTANG_OUT, FLASH_CNT, X_DELTANG_LOW, FLASH_CNT, X_GYRO_OUT, FLASH_CNT,
+ Y_GYRO_OUT, FLASH_CNT, Z_GYRO_OUT, FLASH_CNT, X_ACCL_OUT, FLASH_CNT,
+ Y_ACCL_OUT, FLASH_CNT, Z_ACCL_OUT, FLASH_CNT};
+
+static constexpr uint8_t m_autospi_y_packet[16] = {
+ Y_DELTANG_OUT, FLASH_CNT, Y_DELTANG_LOW, FLASH_CNT, X_GYRO_OUT, FLASH_CNT,
+ Y_GYRO_OUT, FLASH_CNT, Z_GYRO_OUT, FLASH_CNT, X_ACCL_OUT, FLASH_CNT,
+ Y_ACCL_OUT, FLASH_CNT, Z_ACCL_OUT, FLASH_CNT};
+
+static constexpr uint8_t m_autospi_z_packet[16] = {
+ Z_DELTANG_OUT, FLASH_CNT, Z_DELTANG_LOW, FLASH_CNT, X_GYRO_OUT, FLASH_CNT,
+ Y_GYRO_OUT, FLASH_CNT, Z_GYRO_OUT, FLASH_CNT, X_ACCL_OUT, FLASH_CNT,
+ Y_ACCL_OUT, FLASH_CNT, Z_ACCL_OUT, FLASH_CNT};
+
+/* ADIS16470 Constants */
+const double delta_angle_sf = 2160.0 / 2147483648.0; /* 2160 / (2^31) */
+const double rad_to_deg = 57.2957795;
+const double deg_to_rad = 0.0174532;
+const double grav = 9.81;
+
+/**
+ * Use DMA SPI to read rate and acceleration data from the ADIS16470 IMU and
+ * return the robot's heading relative to a starting position and instant
+ * measurements
+ *
+ * The ADIS16470 gyro angle outputs track the robot's heading based on the
+ * starting position. As the robot rotates the new heading is computed by
+ * integrating the rate of rotation returned by the IMU. When the class is
+ * instantiated, a short calibration routine is performed where the IMU samples
+ * the gyros while at rest to determine the initial offset. This is subtracted
+ * from each sample to determine the heading.
+ *
+ * This class is for the ADIS16470 IMU connected via the primary SPI port
+ * available on the RoboRIO.
+ */
+
+class ADIS16470_IMU : public nt::NTSendable,
+ public wpi::SendableHelper {
+ public:
+ enum IMUAxis { kX, kY, kZ };
+
+ /**
+ * @brief Default constructor. Uses CS0 on the 10-pin SPI port, the yaw axis
+ * is set to the IMU Z axis, and calibration time is defaulted to 4 seconds.
+ */
+ ADIS16470_IMU();
+
+ /**
+ * @brief Customizable constructor. Allows the SPI port and CS to be
+ * customized, the yaw axis used for GetAngle() is adjustable, and initial
+ * calibration time can be modified.
+ *
+ * @param yaw_axis Selects the "default" axis to use for GetAngle() and
+ * GetRate()
+ *
+ * @param port The SPI port and CS where the IMU is connected.
+ *
+ * @param cal_time The calibration time that should be used on start-up.
+ */
+ explicit ADIS16470_IMU(IMUAxis yaw_axis, SPI::Port port,
+ ADIS16470CalibrationTime cal_time);
+
+ /**
+ * @brief Destructor. Kills the acquisiton loop and closes the SPI peripheral.
+ */
+ ~ADIS16470_IMU() override;
+
+ ADIS16470_IMU(ADIS16470_IMU&&) = default;
+ ADIS16470_IMU& operator=(ADIS16470_IMU&&) = default;
+
+ int ConfigDecRate(uint16_t reg);
+
+ /**
+ * @brief Switches the active SPI port to standard SPI mode, writes the
+ * command to activate the new null configuration, and re-enables auto SPI.
+ */
+ void Calibrate();
+
+ /**
+ * @brief Switches the active SPI port to standard SPI mode, writes a new
+ * value to the NULL_CNFG register in the IMU, and re-enables auto SPI.
+ */
+ int ConfigCalTime(ADIS16470CalibrationTime new_cal_time);
+
+ /**
+ * @brief Resets (zeros) the xgyro, ygyro, and zgyro angle integrations.
+ *
+ * Resets the gyro accumulations to a heading of zero. This can be used if
+ * the "zero" orientation of the sensor needs to be changed in runtime.
+ */
+ void Reset();
+
+ /**
+ * @brief Returns the current integrated angle for the axis specified.
+ *
+ * The angle is based on the current accumulator value corrected by
+ * offset calibration and built-in IMU calibration. The angle is continuous,
+ * that is it will continue from 360->361 degrees. This allows algorithms
+ * that wouldn't want to see a discontinuity in the gyro output as it sweeps
+ * from 360 to 0 on the second time around. The axis returned by this
+ * function is adjusted based on the configured yaw_axis.
+ *
+ * @return the current heading of the robot in degrees. This heading is based
+ * on integration of the returned rate from the gyro.
+ */
+ double GetAngle() const;
+
+ double GetRate() const;
+
+ double GetGyroInstantX() const;
+
+ double GetGyroInstantY() const;
+
+ double GetGyroInstantZ() const;
+
+ double GetAccelInstantX() const;
+
+ double GetAccelInstantY() const;
+
+ double GetAccelInstantZ() const;
+
+ double GetXComplementaryAngle() const;
+
+ double GetYComplementaryAngle() const;
+
+ double GetXFilteredAccelAngle() const;
+
+ double GetYFilteredAccelAngle() const;
+
+ IMUAxis GetYawAxis() const;
+
+ int SetYawAxis(IMUAxis yaw_axis);
+
+ // IMU yaw axis
+ IMUAxis m_yaw_axis;
+
+ void InitSendable(nt::NTSendableBuilder& builder) override;
+
+ private:
+ /**
+ * @brief Switches to standard SPI operation. Primarily used when exiting auto
+ * SPI mode.
+ *
+ * @return A boolean indicating the success or failure of setting up the SPI
+ * peripheral in standard SPI mode.
+ */
+ bool SwitchToStandardSPI();
+
+ /**
+ * @brief Switches to auto SPI operation. Primarily used when exiting standard
+ * SPI mode.
+ *
+ * @return A boolean indicating the success or failure of setting up the SPI
+ * peripheral in auto SPI mode.
+ */
+ bool SwitchToAutoSPI();
+
+ /**
+ * @brief Reads the contents of a specified register location over SPI.
+ *
+ * @param reg An unsigned, 8-bit register location.
+ *
+ * @return An unsigned, 16-bit number representing the contents of the
+ * requested register location.
+ */
+ uint16_t ReadRegister(uint8_t reg);
+
+ /**
+ * @brief Writes an unsigned, 16-bit value to two adjacent, 8-bit register
+ * locations over SPI.
+ *
+ * @param reg An unsigned, 8-bit register location.
+ *
+ * @param val An unsigned, 16-bit value to be written to the specified
+ * register location.
+ */
+ void WriteRegister(uint8_t reg, uint16_t val);
+
+ /**
+ * @brief Main acquisition loop. Typically called asynchronously and
+ * free-wheels while the robot code is active.
+ */
+ void Acquire();
+
+ void Close();
+
+ // Integrated gyro value
+ double m_integ_angle = 0.0;
+
+ // Instant raw outputs
+ double m_gyro_x, m_gyro_y, m_gyro_z, m_accel_x, m_accel_y, m_accel_z = 0.0;
+
+ // Complementary filter variables
+ double m_tau = 1.0;
+ double m_dt, m_alpha = 0.0;
+ double m_compAngleX, m_compAngleY, m_accelAngleX, m_accelAngleY = 0.0;
+
+ // Complementary filter functions
+ double FormatFastConverge(double compAngle, double accAngle);
+
+ double FormatRange0to2PI(double compAngle);
+
+ double FormatAccelRange(double accelAngle, double accelZ);
+
+ double CompFilterProcess(double compAngle, double accelAngle, double omega);
+
+ // State and resource variables
+ volatile bool m_thread_active = false;
+ volatile bool m_first_run = true;
+ volatile bool m_thread_idle = false;
+ bool m_auto_configured = false;
+ SPI::Port m_spi_port;
+ uint16_t m_calibration_time;
+ SPI* m_spi = nullptr;
+ DigitalInput* m_auto_interrupt = nullptr;
+ double m_scaled_sample_rate = 2500.0; // Default sample rate setting
+
+ std::thread m_acquire_task;
+
+ struct NonMovableMutexWrapper {
+ wpi::mutex mutex;
+ NonMovableMutexWrapper() = default;
+
+ NonMovableMutexWrapper(const NonMovableMutexWrapper&) = delete;
+ NonMovableMutexWrapper& operator=(const NonMovableMutexWrapper&) = delete;
+
+ NonMovableMutexWrapper(NonMovableMutexWrapper&&) {}
+ NonMovableMutexWrapper& operator=(NonMovableMutexWrapper&&) {
+ return *this;
+ }
+
+ void lock() { mutex.lock(); }
+
+ void unlock() { mutex.unlock(); }
+
+ bool try_lock() noexcept { return mutex.try_lock(); }
+ };
+
+ mutable NonMovableMutexWrapper m_mutex;
+};
+
+} // namespace frc
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java
new file mode 100644
index 0000000000..a583e12f31
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java
@@ -0,0 +1,1021 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2020 Analog Devices Inc. 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. */
+/* */
+/* Modified by Juan Chong - frcsupport@analog.com */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.networktables.NTSendable;
+import edu.wpi.first.networktables.NTSendableBuilder;
+import edu.wpi.first.wpilibj.interfaces.Gyro;
+
+// CHECKSTYLE.OFF: TypeName
+// CHECKSTYLE.OFF: MemberName
+// CHECKSTYLE.OFF: SummaryJavadoc
+// CHECKSTYLE.OFF: UnnecessaryParentheses
+// CHECKSTYLE.OFF: OverloadMethodsDeclarationOrder
+// CHECKSTYLE.OFF: NonEmptyAtclauseDescription
+// CHECKSTYLE.OFF: MissingOverride
+// CHECKSTYLE.OFF: AtclauseOrder
+// CHECKSTYLE.OFF: LocalVariableName
+// CHECKSTYLE.OFF: RedundantModifier
+// CHECKSTYLE.OFF: AbbreviationAsWordInName
+// CHECKSTYLE.OFF: ParameterName
+// CHECKSTYLE.OFF: EmptyCatchBlock
+// CHECKSTYLE.OFF: MissingJavadocMethod
+// CHECKSTYLE.OFF: MissingSwitchDefault
+// CHECKSTYLE.OFF: VariableDeclarationUsageDistance
+// CHECKSTYLE.OFF: ArrayTypeStyle
+
+/** This class is for the ADIS16448 IMU that connects to the RoboRIO MXP port. */
+@SuppressWarnings({
+ "unused",
+ "PMD.RedundantFieldInitializer",
+ "PMD.ImmutableField",
+ "PMD.SingularField",
+ "PMD.CollapsibleIfStatements",
+ "PMD.MissingOverride",
+ "PMD.EmptyIfStmt",
+ "PMD.EmptyStatementNotInLoop"
+})
+public class ADIS16448_IMU implements Gyro, NTSendable {
+ /** ADIS16448 Register Map Declaration */
+ private static final int FLASH_CNT = 0x00; // Flash memory write count
+
+ private static final int XGYRO_OUT = 0x04; // X-axis gyroscope output
+ private static final int YGYRO_OUT = 0x06; // Y-axis gyroscope output
+ private static final int ZGYRO_OUT = 0x08; // Z-axis gyroscope output
+ private static final int XACCL_OUT = 0x0A; // X-axis accelerometer output
+ private static final int YACCL_OUT = 0x0C; // Y-axis accelerometer output
+ private static final int ZACCL_OUT = 0x0E; // Z-axis accelerometer output
+ private static final int XMAGN_OUT = 0x10; // X-axis magnetometer output
+ private static final int YMAGN_OUT = 0x12; // Y-axis magnetometer output
+ private static final int ZMAGN_OUT = 0x14; // Z-axis magnetometer output
+ private static final int BARO_OUT = 0x16; // Barometer pressure measurement, high word
+ private static final int TEMP_OUT = 0x18; // Temperature output
+ private static final int XGYRO_OFF = 0x1A; // X-axis gyroscope bias offset factor
+ private static final int YGYRO_OFF = 0x1C; // Y-axis gyroscope bias offset factor
+ private static final int ZGYRO_OFF = 0x1E; // Z-axis gyroscope bias offset factor
+ private static final int XACCL_OFF = 0x20; // X-axis acceleration bias offset factor
+ private static final int YACCL_OFF = 0x22; // Y-axis acceleration bias offset factor
+ private static final int ZACCL_OFF = 0x24; // Z-axis acceleration bias offset factor
+ private static final int XMAGN_HIC = 0x26; // X-axis magnetometer, hard iron factor
+ private static final int YMAGN_HIC = 0x28; // Y-axis magnetometer, hard iron factor
+ private static final int ZMAGN_HIC = 0x2A; // Z-axis magnetometer, hard iron factor
+ private static final int XMAGN_SIC = 0x2C; // X-axis magnetometer, soft iron factor
+ private static final int YMAGN_SIC = 0x2E; // Y-axis magnetometer, soft iron factor
+ private static final int ZMAGN_SIC = 0x30; // Z-axis magnetometer, soft iron factor
+ private static final int GPIO_CTRL = 0x32; // GPIO control
+ private static final int MSC_CTRL = 0x34; // MISC control
+ private static final int SMPL_PRD = 0x36; // Sample clock/Decimation filter control
+ private static final int SENS_AVG = 0x38; // Digital filter control
+ private static final int SEQ_CNT = 0x3A; // MAGN_OUT and BARO_OUT counter
+ private static final int DIAG_STAT = 0x3C; // System status
+ private static final int GLOB_CMD = 0x3E; // System command
+ private static final int ALM_MAG1 = 0x40; // Alarm 1 amplitude threshold
+ private static final int ALM_MAG2 = 0x42; // Alarm 2 amplitude threshold
+ private static final int ALM_SMPL1 = 0x44; // Alarm 1 sample size
+ private static final int ALM_SMPL2 = 0x46; // Alarm 2 sample size
+ private static final int ALM_CTRL = 0x48; // Alarm control
+ private static final int LOT_ID1 = 0x52; // Lot identification number
+ private static final int LOT_ID2 = 0x54; // Lot identification number
+ private static final int PROD_ID = 0x56; // Product identifier
+ private static final int SERIAL_NUM = 0x58; // Lot-specific serial number
+
+ public enum IMUAxis {
+ kX,
+ kY,
+ kZ
+ }
+
+ // * Static Constants */
+ private static final double rad_to_deg = 57.2957795;
+ private static final double deg_to_rad = 0.0174532;
+ private static final double grav = 9.81;
+
+ /* User-specified yaw axis */
+ private IMUAxis m_yaw_axis;
+
+ /* Offset data storage */
+ private double m_accum_gyro_x[];
+ private double m_accum_gyro_y[];
+ private double m_accum_gyro_z[];
+
+ /* Instant raw output variables */
+ private double m_gyro_x = 0.0;
+ private double m_gyro_y = 0.0;
+ private double m_gyro_z = 0.0;
+ private double m_accel_x = 0.0;
+ private double m_accel_y = 0.0;
+ private double m_accel_z = 0.0;
+ private double m_mag_x = 0.0;
+ private double m_mag_y = 0.0;
+ private double m_mag_z = 0.0;
+ private double m_baro = 0.0;
+ private double m_temp = 0.0;
+
+ /* IMU gyro offset variables */
+ private double m_gyro_offset_x = 0.0;
+ private double m_gyro_offset_y = 0.0;
+ private double m_gyro_offset_z = 0.0;
+ private int m_avg_size = 0;
+ private int m_accum_count = 0;
+
+ /* Integrated gyro angle variables */
+ private double m_integ_gyro_x = 0.0;
+ private double m_integ_gyro_y = 0.0;
+ private double m_integ_gyro_z = 0.0;
+
+ /* Complementary filter variables */
+ private double m_dt = 0.0;
+ private double m_alpha = 0.0;
+ private double m_tau = 1.0;
+ private double m_compAngleX = 0.0;
+ private double m_compAngleY = 0.0;
+ private double m_accelAngleX = 0.0;
+ private double m_accelAngleY = 0.0;
+
+ /* State variables */
+ private volatile boolean m_thread_active = false;
+ private int m_calibration_time = 0;
+ private volatile boolean m_first_run = true;
+ private volatile boolean m_thread_idle = false;
+ private boolean m_auto_configured = false;
+ private boolean m_start_up_mode = true;
+
+ /* Resources */
+ private SPI m_spi;
+ private SPI.Port m_spi_port;
+ private DigitalInput m_auto_interrupt;
+ private DigitalOutput m_reset_out;
+ private DigitalInput m_reset_in;
+ private DigitalOutput m_status_led;
+ private Thread m_acquire_task;
+
+ /* CRC-16 Look-Up Table */
+ int adiscrc[] =
+ new int[] {
+ 0x0000, 0x17CE, 0x0FDF, 0x1811, 0x1FBE, 0x0870, 0x1061, 0x07AF,
+ 0x1F3F, 0x08F1, 0x10E0, 0x072E, 0x0081, 0x174F, 0x0F5E, 0x1890,
+ 0x1E3D, 0x09F3, 0x11E2, 0x062C, 0x0183, 0x164D, 0x0E5C, 0x1992,
+ 0x0102, 0x16CC, 0x0EDD, 0x1913, 0x1EBC, 0x0972, 0x1163, 0x06AD,
+ 0x1C39, 0x0BF7, 0x13E6, 0x0428, 0x0387, 0x1449, 0x0C58, 0x1B96,
+ 0x0306, 0x14C8, 0x0CD9, 0x1B17, 0x1CB8, 0x0B76, 0x1367, 0x04A9,
+ 0x0204, 0x15CA, 0x0DDB, 0x1A15, 0x1DBA, 0x0A74, 0x1265, 0x05AB,
+ 0x1D3B, 0x0AF5, 0x12E4, 0x052A, 0x0285, 0x154B, 0x0D5A, 0x1A94,
+ 0x1831, 0x0FFF, 0x17EE, 0x0020, 0x078F, 0x1041, 0x0850, 0x1F9E,
+ 0x070E, 0x10C0, 0x08D1, 0x1F1F, 0x18B0, 0x0F7E, 0x176F, 0x00A1,
+ 0x060C, 0x11C2, 0x09D3, 0x1E1D, 0x19B2, 0x0E7C, 0x166D, 0x01A3,
+ 0x1933, 0x0EFD, 0x16EC, 0x0122, 0x068D, 0x1143, 0x0952, 0x1E9C,
+ 0x0408, 0x13C6, 0x0BD7, 0x1C19, 0x1BB6, 0x0C78, 0x1469, 0x03A7,
+ 0x1B37, 0x0CF9, 0x14E8, 0x0326, 0x0489, 0x1347, 0x0B56, 0x1C98,
+ 0x1A35, 0x0DFB, 0x15EA, 0x0224, 0x058B, 0x1245, 0x0A54, 0x1D9A,
+ 0x050A, 0x12C4, 0x0AD5, 0x1D1B, 0x1AB4, 0x0D7A, 0x156B, 0x02A5,
+ 0x1021, 0x07EF, 0x1FFE, 0x0830, 0x0F9F, 0x1851, 0x0040, 0x178E,
+ 0x0F1E, 0x18D0, 0x00C1, 0x170F, 0x10A0, 0x076E, 0x1F7F, 0x08B1,
+ 0x0E1C, 0x19D2, 0x01C3, 0x160D, 0x11A2, 0x066C, 0x1E7D, 0x09B3,
+ 0x1123, 0x06ED, 0x1EFC, 0x0932, 0x0E9D, 0x1953, 0x0142, 0x168C,
+ 0x0C18, 0x1BD6, 0x03C7, 0x1409, 0x13A6, 0x0468, 0x1C79, 0x0BB7,
+ 0x1327, 0x04E9, 0x1CF8, 0x0B36, 0x0C99, 0x1B57, 0x0346, 0x1488,
+ 0x1225, 0x05EB, 0x1DFA, 0x0A34, 0x0D9B, 0x1A55, 0x0244, 0x158A,
+ 0x0D1A, 0x1AD4, 0x02C5, 0x150B, 0x12A4, 0x056A, 0x1D7B, 0x0AB5,
+ 0x0810, 0x1FDE, 0x07CF, 0x1001, 0x17AE, 0x0060, 0x1871, 0x0FBF,
+ 0x172F, 0x00E1, 0x18F0, 0x0F3E, 0x0891, 0x1F5F, 0x074E, 0x1080,
+ 0x162D, 0x01E3, 0x19F2, 0x0E3C, 0x0993, 0x1E5D, 0x064C, 0x1182,
+ 0x0912, 0x1EDC, 0x06CD, 0x1103, 0x16AC, 0x0162, 0x1973, 0x0EBD,
+ 0x1429, 0x03E7, 0x1BF6, 0x0C38, 0x0B97, 0x1C59, 0x0448, 0x1386,
+ 0x0B16, 0x1CD8, 0x04C9, 0x1307, 0x14A8, 0x0366, 0x1B77, 0x0CB9,
+ 0x0A14, 0x1DDA, 0x05CB, 0x1205, 0x15AA, 0x0264, 0x1A75, 0x0DBB,
+ 0x152B, 0x02E5, 0x1AF4, 0x0D3A, 0x0A95, 0x1D5B, 0x054A, 0x1284
+ };
+
+ private static class AcquireTask implements Runnable {
+ private final ADIS16448_IMU imu;
+
+ public AcquireTask(final ADIS16448_IMU imu) {
+ this.imu = imu;
+ }
+
+ @Override
+ public void run() {
+ imu.acquire();
+ }
+ }
+
+ public ADIS16448_IMU() {
+ this(IMUAxis.kZ, SPI.Port.kMXP, 4);
+ }
+
+ /**
+ * @param yaw_axis The axis that measures the yaw
+ * @param port The SPI Port the gyro is plugged into
+ * @param cal_time Calibration time
+ */
+ public ADIS16448_IMU(final IMUAxis yaw_axis, SPI.Port port, int cal_time) {
+ m_yaw_axis = yaw_axis;
+ m_spi_port = port;
+
+ m_acquire_task = new Thread(new AcquireTask(this));
+
+ // Force the IMU reset pin to toggle on startup (doesn't require DS enable)
+ m_reset_out = new DigitalOutput(18); // Drive MXP DIO8 low
+ Timer.delay(0.01); // Wait 10ms
+ m_reset_out.close();
+ m_reset_in = new DigitalInput(18); // Set MXP DIO8 high
+ Timer.delay(0.25); // Wait 250ms
+
+ configCalTime(cal_time);
+
+ if (!switchToStandardSPI()) {
+ return;
+ }
+
+ // Set IMU internal decimation to 819.2 SPS
+ writeRegister(SMPL_PRD, 0x0001);
+ // Enable Data Ready (LOW = Good Data) on DIO1 (PWM0 on MXP)
+ writeRegister(MSC_CTRL, 0x0016);
+ // Disable IMU internal Bartlett filter
+ writeRegister(SENS_AVG, 0x0400);
+ // Clear offset registers
+ writeRegister(XGYRO_OFF, 0x0000);
+ writeRegister(YGYRO_OFF, 0x0000);
+ writeRegister(ZGYRO_OFF, 0x0000);
+
+ // Configure standard SPI
+ if (!switchToAutoSPI()) {
+ return;
+ }
+ // Notify DS that IMU calibration delay is active
+ DriverStation.reportWarning(
+ "ADIS16448 IMU Detected. Starting initial calibration delay.", false);
+ // Wait for whatever time the user set as the start-up delay
+ try {
+ Thread.sleep((long) (m_calibration_time * 1.2 * 1000));
+ } catch (InterruptedException e) {
+ }
+ // Execute calibration routine
+ calibrate();
+ // Reset accumulated offsets
+ reset();
+ // Tell the acquire loop that we're done starting up
+ m_start_up_mode = false;
+ // Let the user know the IMU was initiallized successfully
+ DriverStation.reportWarning("ADIS16448 IMU Successfully Initialized!", false);
+ // Drive MXP PWM5 (IMU ready LED) low (active low)
+ m_status_led = new DigitalOutput(19);
+ // Report usage and post data to DS
+ HAL.report(tResourceType.kResourceType_ADIS16448, 0);
+ }
+
+ /** */
+ private static int toUShort(byte[] buf) {
+ return (((buf[0] & 0xFF) << 8) + ((buf[1] & 0xFF) << 0));
+ }
+
+ private static int toUByte(int... buf) {
+ return (buf[0] & 0xFF);
+ }
+
+ public static int toUShort(int... buf) {
+ return (((buf[0] & 0xFF) << 8) + (buf[1] & 0xFF));
+ }
+
+ /** */
+ private static long toULong(int sint) {
+ return sint & 0x00000000FFFFFFFFL;
+ }
+
+ /** */
+ private static int toShort(int... buf) {
+ return (short) (((buf[0] & 0xFF) << 8) + ((buf[1] & 0xFF) << 0));
+ }
+
+ /** */
+ private static int toInt(int... buf) {
+ return (int)
+ ((buf[0] & 0xFF) << 24 | (buf[1] & 0xFF) << 16 | (buf[2] & 0xFF) << 8 | (buf[3] & 0xFF));
+ }
+
+ /** */
+ private boolean switchToStandardSPI() {
+ // Check to see whether the acquire thread is active. If so, wait for it to stop
+ // producing data.
+ if (m_thread_active) {
+ m_thread_active = false;
+ while (!m_thread_idle) {
+ try {
+ Thread.sleep(10);
+ } catch (InterruptedException e) {
+ }
+ }
+ System.out.println("Paused the IMU processing thread successfully!");
+ // Maybe we're in auto SPI mode? If so, kill auto SPI, and then SPI.
+ if (m_spi != null && m_auto_configured) {
+ m_spi.stopAuto();
+ // We need to get rid of all the garbage left in the auto SPI buffer after
+ // stopping it.
+ // Sometimes data magically reappears, so we have to check the buffer size a
+ // couple of times
+ // to be sure we got it all. Yuck.
+ int[] trashBuffer = new int[200];
+ try {
+ Thread.sleep(100);
+ } catch (InterruptedException e) {
+ }
+ int data_count = m_spi.readAutoReceivedData(trashBuffer, 0, 0);
+ while (data_count > 0) {
+ /* Dequeue 200 at a time, or the remainder of the buffer if less than 200 */
+ m_spi.readAutoReceivedData(trashBuffer, Math.min(200, data_count), 0);
+ /* Update remaining buffer count */
+ data_count = m_spi.readAutoReceivedData(trashBuffer, 0, 0);
+ }
+ System.out.println("Paused auto SPI successfully.");
+ }
+ }
+ // There doesn't seem to be a SPI port active. Let's try to set one up
+ if (m_spi == null) {
+ System.out.println("Setting up a new SPI port.");
+ m_spi = new SPI(m_spi_port);
+ m_spi.setClockRate(1000000);
+ m_spi.setMSBFirst();
+ m_spi.setSampleDataOnTrailingEdge();
+ m_spi.setClockActiveLow();
+ m_spi.setChipSelectActiveLow();
+ readRegister(PROD_ID); // Dummy read
+
+ // Validate the product ID
+ if (readRegister(PROD_ID) != 16448) {
+ DriverStation.reportError("Could not find ADIS16448", false);
+ close();
+ return false;
+ }
+ return true;
+ } else {
+ // Maybe the SPI port is active, but not in auto SPI mode? Try to read the
+ // product ID.
+ readRegister(PROD_ID); // dummy read
+ if (readRegister(PROD_ID) != 16448) {
+ DriverStation.reportError("Could not find an ADIS16448", false);
+ close();
+ return false;
+ } else {
+ return true;
+ }
+ }
+ }
+
+ /** */
+ boolean switchToAutoSPI() {
+ // No SPI port has been set up. Go set one up first.
+ if (m_spi == null) {
+ if (!switchToStandardSPI()) {
+ DriverStation.reportError("Failed to start/restart auto SPI", false);
+ return false;
+ }
+ }
+ // Only set up the interrupt if needed.
+ if (m_auto_interrupt == null) {
+ m_auto_interrupt = new DigitalInput(10); // MXP DIO0
+ }
+ // The auto SPI controller gets angry if you try to set up two instances on one
+ // bus.
+ if (!m_auto_configured) {
+ m_spi.initAuto(8200);
+ m_auto_configured = true;
+ }
+ // Set auto SPI packet data and size
+ m_spi.setAutoTransmitData(new byte[] {GLOB_CMD}, 27);
+ // Configure auto stall time
+ m_spi.configureAutoStall(100, 1000, 255);
+ // Kick off auto SPI (Note: Device configration impossible after auto SPI is
+ // activated)
+ m_spi.startAutoTrigger(m_auto_interrupt, true, false);
+
+ // Check to see if the acquire thread is running. If not, kick one off.
+ if (!m_acquire_task.isAlive()) {
+ m_first_run = true;
+ m_thread_active = true;
+ m_acquire_task.start();
+ System.out.println("New IMU Processing thread activated!");
+ } else {
+ // The thread was running, re-init run variables and start it up again.
+ m_first_run = true;
+ m_thread_active = true;
+ System.out.println("Old IMU Processing thread re-activated!");
+ }
+ // Looks like the thread didn't start for some reason. Abort.
+ if (!m_acquire_task.isAlive()) {
+ DriverStation.reportError("Failed to start/restart the acquire() thread.", false);
+ close();
+ return false;
+ }
+ return true;
+ }
+
+ public int configDecRate(int m_decRate) {
+ int writeValue = m_decRate;
+ int readbackValue;
+ if (!switchToStandardSPI()) {
+ DriverStation.reportError("Failed to configure/reconfigure standard SPI.", false);
+ return 2;
+ }
+
+ /* Check max */
+ if (m_decRate > 9) {
+ DriverStation.reportError(
+ "Attemted to write an invalid decimation value. Capping at 9", false);
+ m_decRate = 9;
+ }
+ if (m_decRate < 0) {
+ DriverStation.reportError(
+ "Attemted to write an invalid decimation value. Capping at 0", false);
+ m_decRate = 0;
+ }
+
+ /* Shift decimation setting to correct position and select internal sync */
+ writeValue = (m_decRate << 8) | 0x1;
+
+ /* Apply to IMU */
+ writeRegister(SMPL_PRD, writeValue);
+
+ /* Perform read back to verify write */
+ readbackValue = readRegister(SMPL_PRD);
+
+ /* Throw error for invalid write */
+ if (readbackValue != writeValue) {
+ DriverStation.reportError("ADIS16448 SMPL_PRD write failed.", false);
+ }
+
+ if (!switchToAutoSPI()) {
+ DriverStation.reportError("Failed to configure/reconfigure auto SPI.", false);
+ return 2;
+ }
+ return 0;
+ }
+
+ /**
+ * Configures calibration time
+ *
+ * @param new_cal_time New calibration time
+ * @return 1 if the new calibration time is the same as the current one else 0
+ */
+ public int configCalTime(int new_cal_time) {
+ if (m_calibration_time == new_cal_time) {
+ return 1;
+ } else {
+ m_calibration_time = new_cal_time;
+ m_avg_size = m_calibration_time * 819;
+ initOffsetBuffer(m_avg_size);
+ return 0;
+ }
+ }
+
+ private void initOffsetBuffer(int size) {
+ // Avoid exceptions in the case of bad arguments
+ if (size < 1) {
+ size = 1;
+ }
+ // Set average size to size (correct bad values)
+ m_avg_size = size;
+ synchronized (this) {
+ // Resize vector
+ m_accum_gyro_x = new double[size];
+ m_accum_gyro_y = new double[size];
+ m_accum_gyro_z = new double[size];
+ // Set acculumate count to 0
+ m_accum_count = 0;
+ }
+ }
+
+ /** {@inheritDoc} */
+ @Override
+ public void calibrate() {
+ synchronized (this) {
+ int gyroAverageSize = Math.min(m_accum_count, m_avg_size);
+ double m_gyro_accum_x = 0.0;
+ double m_gyro_accum_y = 0.0;
+ double m_gyro_accum_z = 0.0;
+ for (int i = 0; i < gyroAverageSize; i++) {
+ m_gyro_accum_x += m_accum_gyro_x[i];
+ m_gyro_accum_y += m_accum_gyro_y[i];
+ m_gyro_accum_z += m_accum_gyro_z[i];
+ }
+ m_gyro_offset_x = m_gyro_accum_x / gyroAverageSize;
+ m_gyro_offset_y = m_gyro_accum_y / gyroAverageSize;
+ m_gyro_offset_z = m_gyro_accum_z / gyroAverageSize;
+ m_integ_gyro_x = 0.0;
+ m_integ_gyro_y = 0.0;
+ m_integ_gyro_z = 0.0;
+ // System.out.println("Avg Size: " + gyroAverageSize + "X Off: " +
+ // m_gyro_offset_x + "Y Off: " + m_gyro_offset_y + "Z Off: " + m_gyro_offset_z);
+ }
+ }
+
+ /**
+ * Sets the yaw axis
+ *
+ * @param yaw_axis The new yaw axis to use
+ * @return 1 if the new yaw axis is the same as the current one else 0.
+ */
+ public int setYawAxis(IMUAxis yaw_axis) {
+ if (m_yaw_axis == yaw_axis) {
+ return 1;
+ }
+ m_yaw_axis = yaw_axis;
+ reset();
+ return 0;
+ }
+
+ private int readRegister(final int reg) {
+ // ByteBuffer buf = ByteBuffer.allocateDirect(2);
+ final byte[] buf = new byte[2];
+ // buf.order(ByteOrder.BIG_ENDIAN);
+ buf[0] = (byte) (reg & 0x7f);
+ buf[1] = (byte) 0;
+
+ m_spi.write(buf, 2);
+ m_spi.read(false, buf, 2);
+
+ return toUShort(buf);
+ }
+
+ private void writeRegister(final int reg, final int val) {
+ final byte[] buf = new byte[2];
+ // low byte
+ buf[0] = (byte) (0x80 | reg);
+ buf[1] = (byte) (val & 0xff);
+ m_spi.write(buf, 2);
+ // high byte
+ buf[0] = (byte) (0x81 | reg);
+ buf[1] = (byte) (val >> 8);
+ m_spi.write(buf, 2);
+ }
+
+ /** {@inheritDoc} */
+ public void reset() {
+ synchronized (this) {
+ m_integ_gyro_x = 0.0;
+ m_integ_gyro_y = 0.0;
+ m_integ_gyro_z = 0.0;
+ }
+ }
+
+ /** Delete (free) the spi port used for the IMU. */
+ @Override
+ public void close() {
+ if (m_thread_active) {
+ m_thread_active = false;
+ try {
+ if (m_acquire_task != null) {
+ m_acquire_task.join();
+ m_acquire_task = null;
+ }
+ } catch (InterruptedException e) {
+ }
+ if (m_spi != null) {
+ if (m_auto_configured) {
+ m_spi.stopAuto();
+ }
+ m_spi.close();
+ m_auto_configured = false;
+ if (m_auto_interrupt != null) {
+ m_auto_interrupt.close();
+ m_auto_interrupt = null;
+ }
+ m_spi = null;
+ }
+ }
+ System.out.println("Finished cleaning up after the IMU driver.");
+ }
+
+ /** */
+ private void acquire() {
+ // Set data packet length
+ final int dataset_len = 29; // 18 data points + timestamp
+ final int BUFFER_SIZE = 4000;
+
+ // Set up buffers and variables
+ int[] buffer = new int[BUFFER_SIZE];
+ int data_count = 0;
+ int data_remainder = 0;
+ int data_to_read = 0;
+ int bufferAvgIndex = 0;
+ double previous_timestamp = 0.0;
+ double delta_angle = 0.0;
+ double gyro_x = 0.0;
+ double gyro_y = 0.0;
+ double gyro_z = 0.0;
+ double accel_x = 0.0;
+ double accel_y = 0.0;
+ double accel_z = 0.0;
+ double mag_x = 0.0;
+ double mag_y = 0.0;
+ double mag_z = 0.0;
+ double baro = 0.0;
+ double temp = 0.0;
+ double gyro_x_si = 0.0;
+ double gyro_y_si = 0.0;
+ double gyro_z_si = 0.0;
+ double accel_x_si = 0.0;
+ double accel_y_si = 0.0;
+ double accel_z_si = 0.0;
+ double compAngleX = 0.0;
+ double compAngleY = 0.0;
+ double accelAngleX = 0.0;
+ double accelAngleY = 0.0;
+
+ while (true) {
+ // Sleep loop for 5ms
+ try {
+ Thread.sleep(5);
+ } catch (InterruptedException e) {
+ }
+
+ if (m_thread_active) {
+ m_thread_idle = false;
+
+ data_count =
+ m_spi.readAutoReceivedData(
+ buffer, 0, 0); // Read number of bytes currently stored in the buffer
+ data_remainder =
+ data_count % dataset_len; // Check if frame is incomplete. Add 1 because of timestamp
+ data_to_read = data_count - data_remainder; // Remove incomplete data from read count
+ if (data_to_read > BUFFER_SIZE) {
+ DriverStation.reportWarning(
+ "ADIS16448 data processing thread overrun has occurred!", false);
+ data_to_read = BUFFER_SIZE - (BUFFER_SIZE % dataset_len);
+ }
+ m_spi.readAutoReceivedData(
+ buffer, data_to_read, 0); // Read data from DMA buffer (only complete sets)
+
+ // Could be multiple data sets in the buffer. Handle each one.
+ for (int i = 0; i < data_to_read; i += dataset_len) {
+ // Calculate CRC-16 on each data packet
+ int calc_crc = 0x0000FFFF; // Starting word
+ int read_byte = 0;
+ int imu_crc = 0;
+ for (int k = 5;
+ k < 27;
+ k += 2) { // Cycle through XYZ GYRO, XYZ ACCEL, XYZ MAG, BARO, TEMP (Ignore Status &
+ // CRC)
+ read_byte = buffer[i + k + 1]; // Process LSB
+ calc_crc = (calc_crc >>> 8) ^ adiscrc[(calc_crc & 0x000000FF) ^ read_byte];
+ read_byte = buffer[i + k]; // Process MSB
+ calc_crc = (calc_crc >>> 8) ^ adiscrc[(calc_crc & 0x000000FF) ^ read_byte];
+ }
+ calc_crc = ~calc_crc & 0xFFFF; // Complement
+ calc_crc = ((calc_crc << 8) | (calc_crc >> 8)) & 0xFFFF; // Flip LSB & MSB
+ imu_crc = toUShort(buffer[i + 27], buffer[i + 28]); // Extract DUT CRC from data buffer
+
+ if (calc_crc == imu_crc) {
+ // Timestamp is at buffer[i]
+ m_dt = ((double) buffer[i] - previous_timestamp) / 1000000.0;
+
+ // Scale sensor data
+ gyro_x = (toShort(buffer[i + 5], buffer[i + 6]) * 0.04);
+ gyro_y = (toShort(buffer[i + 7], buffer[i + 8]) * 0.04);
+ gyro_z = (toShort(buffer[i + 9], buffer[i + 10]) * 0.04);
+ accel_x = (toShort(buffer[i + 11], buffer[i + 12]) * 0.833);
+ accel_y = (toShort(buffer[i + 13], buffer[i + 14]) * 0.833);
+ accel_z = (toShort(buffer[i + 15], buffer[i + 16]) * 0.833);
+ mag_x = (toShort(buffer[i + 17], buffer[i + 18]) * 0.1429);
+ mag_y = (toShort(buffer[i + 19], buffer[i + 20]) * 0.1429);
+ mag_z = (toShort(buffer[i + 21], buffer[i + 22]) * 0.1429);
+ baro = (toShort(buffer[i + 23], buffer[i + 24]) * 0.02);
+ temp = (toShort(buffer[i + 25], buffer[i + 26]) * 0.07386 + 31.0);
+
+ // Convert scaled sensor data to SI units (for tilt calculations)
+ // TODO: Should the unit outputs be selectable?
+ gyro_x_si = gyro_x * deg_to_rad;
+ gyro_y_si = gyro_y * deg_to_rad;
+ gyro_z_si = gyro_z * deg_to_rad;
+ accel_x_si = accel_x * grav;
+ accel_y_si = accel_y * grav;
+ accel_z_si = accel_z * grav;
+ // Store timestamp for next iteration
+ previous_timestamp = buffer[i];
+ // Calculate alpha for use with the complementary filter
+ m_alpha = m_tau / (m_tau + m_dt);
+ // Calculate complementary filter
+ if (m_first_run) {
+ // Set up inclinometer calculations for first run
+ accelAngleX =
+ Math.atan2(
+ -accel_x_si,
+ Math.sqrt((accel_y_si * accel_y_si) + (-accel_z_si * -accel_z_si)));
+ accelAngleY =
+ Math.atan2(
+ accel_y_si,
+ Math.sqrt((-accel_x_si * -accel_x_si) + (-accel_z_si * -accel_z_si)));
+ compAngleX = accelAngleX;
+ compAngleY = accelAngleY;
+ } else {
+ // Run inclinometer calculations
+ accelAngleX =
+ Math.atan2(
+ -accel_x_si,
+ Math.sqrt((accel_y_si * accel_y_si) + (-accel_z_si * -accel_z_si)));
+ accelAngleY =
+ Math.atan2(
+ accel_y_si,
+ Math.sqrt((-accel_x_si * -accel_x_si) + (-accel_z_si * -accel_z_si)));
+ accelAngleX = formatAccelRange(accelAngleX, -accel_z_si);
+ accelAngleY = formatAccelRange(accelAngleY, -accel_z_si);
+ compAngleX = compFilterProcess(compAngleX, accelAngleX, -gyro_y_si);
+ compAngleY = compFilterProcess(compAngleY, accelAngleY, -gyro_x_si);
+ }
+
+ // Update global variables and state
+ synchronized (this) {
+ // Ignore first, integrated sample
+ if (m_first_run) {
+ m_integ_gyro_x = 0.0;
+ m_integ_gyro_y = 0.0;
+ m_integ_gyro_z = 0.0;
+ } else {
+ // Accumulate gyro for offset calibration
+ // Add to buffer
+ bufferAvgIndex = m_accum_count % m_avg_size;
+ m_accum_gyro_x[bufferAvgIndex] = gyro_x;
+ m_accum_gyro_y[bufferAvgIndex] = gyro_y;
+ m_accum_gyro_z[bufferAvgIndex] = gyro_z;
+ // Increment counter
+ m_accum_count++;
+ }
+ if (!m_start_up_mode) {
+ m_gyro_x = gyro_x;
+ m_gyro_y = gyro_y;
+ m_gyro_z = gyro_z;
+ m_accel_x = accel_x;
+ m_accel_y = accel_y;
+ m_accel_z = accel_z;
+ m_mag_x = mag_x;
+ m_mag_y = mag_y;
+ m_mag_z = mag_z;
+ m_baro = baro;
+ m_temp = temp;
+ m_compAngleX = compAngleX * rad_to_deg;
+ m_compAngleY = compAngleY * rad_to_deg;
+ m_accelAngleX = accelAngleX * rad_to_deg;
+ m_accelAngleY = accelAngleY * rad_to_deg;
+ // Accumulate gyro for angle integration and publish to global variables
+ m_integ_gyro_x += (gyro_x - m_gyro_offset_x) * m_dt;
+ m_integ_gyro_y += (gyro_y - m_gyro_offset_y) * m_dt;
+ m_integ_gyro_z += (gyro_z - m_gyro_offset_z) * m_dt;
+ }
+ // System.out.println("Good CRC");
+ }
+ m_first_run = false;
+ } else {
+ // System.out.println("Bad CRC");
+ /*
+ * System.out.println("Calc CRC: " + calc_crc);
+ * System.out.println("IMU CRC: " + imu_crc);
+ * System.out.println(
+ * buffer[i] + " " +
+ * (buffer[i + 1]) + " " + (buffer[i + 2]) + " " +
+ * (buffer[i + 3]) + " " + (buffer[i + 4]) + " " +
+ * (buffer[i + 5]) + " " + (buffer[i + 6]) + " " +
+ * (buffer[i + 7]) + " " + (buffer[i + 8]) + " " +
+ * (buffer[i + 9]) + " " + (buffer[i + 10]) + " " +
+ * (buffer[i + 11]) + " " + (buffer[i + 12]) + " " +
+ * (buffer[i + 13]) + " " + (buffer[i + 14]) + " " +
+ * (buffer[i + 15]) + " " + (buffer[i + 16]) + " " +
+ * (buffer[i + 17]) + " " + (buffer[i + 18]) + " " +
+ * (buffer[i + 19]) + " " + (buffer[i + 20]) + " " +
+ * (buffer[i + 21]) + " " + (buffer[i + 22]) + " " +
+ * (buffer[i + 23]) + " " + (buffer[i + 24]) + " " +
+ * (buffer[i + 25]) + " " + (buffer[i + 26]) + " " +
+ * (buffer[i + 27]) + " " + (buffer[i + 28]));
+ */
+ }
+ }
+ } else {
+ m_thread_idle = true;
+ data_count = 0;
+ data_remainder = 0;
+ data_to_read = 0;
+ previous_timestamp = 0.0;
+ delta_angle = 0.0;
+ gyro_x = 0.0;
+ gyro_y = 0.0;
+ gyro_z = 0.0;
+ accel_x = 0.0;
+ accel_y = 0.0;
+ accel_z = 0.0;
+ mag_x = 0.0;
+ mag_y = 0.0;
+ mag_z = 0.0;
+ baro = 0.0;
+ temp = 0.0;
+ gyro_x_si = 0.0;
+ gyro_y_si = 0.0;
+ gyro_z_si = 0.0;
+ accel_x_si = 0.0;
+ accel_y_si = 0.0;
+ accel_z_si = 0.0;
+ compAngleX = 0.0;
+ compAngleY = 0.0;
+ accelAngleX = 0.0;
+ accelAngleY = 0.0;
+ }
+ }
+ }
+
+ /**
+ * @param compAngle
+ * @param accAngle
+ * @return
+ */
+ private double formatFastConverge(double compAngle, double accAngle) {
+ if (compAngle > accAngle + Math.PI) {
+ compAngle = compAngle - 2.0 * Math.PI;
+ } else if (accAngle > compAngle + Math.PI) {
+ compAngle = compAngle + 2.0 * Math.PI;
+ }
+ return compAngle;
+ }
+
+ /**
+ * @param compAngle
+ * @return
+ */
+ private double formatRange0to2PI(double compAngle) {
+ while (compAngle >= 2 * Math.PI) {
+ compAngle = compAngle - 2.0 * Math.PI;
+ }
+ while (compAngle < 0.0) {
+ compAngle = compAngle + 2.0 * Math.PI;
+ }
+ return compAngle;
+ }
+
+ /**
+ * @param accelAngle
+ * @param accelZ
+ * @return
+ */
+ private double formatAccelRange(double accelAngle, double accelZ) {
+ if (accelZ < 0.0) {
+ accelAngle = Math.PI - accelAngle;
+ } else if (accelZ > 0.0 && accelAngle < 0.0) {
+ accelAngle = 2.0 * Math.PI + accelAngle;
+ }
+ return accelAngle;
+ }
+
+ /**
+ * @param compAngle
+ * @param accelAngle
+ * @param omega
+ * @return
+ */
+ private double compFilterProcess(double compAngle, double accelAngle, double omega) {
+ compAngle = formatFastConverge(compAngle, accelAngle);
+ compAngle = m_alpha * (compAngle + omega * m_dt) + (1.0 - m_alpha) * accelAngle;
+ compAngle = formatRange0to2PI(compAngle);
+ if (compAngle > Math.PI) {
+ compAngle = compAngle - 2.0 * Math.PI;
+ }
+ return compAngle;
+ }
+
+ /** */
+ public synchronized double getAngle() {
+ switch (m_yaw_axis) {
+ case kX:
+ return getGyroAngleX();
+ case kY:
+ return getGyroAngleY();
+ case kZ:
+ return getGyroAngleZ();
+ default:
+ return 0.0;
+ }
+ }
+
+ /** */
+ public synchronized double getRate() {
+ switch (m_yaw_axis) {
+ case kX:
+ return getGyroInstantX();
+ case kY:
+ return getGyroInstantY();
+ case kZ:
+ return getGyroInstantZ();
+ default:
+ return 0.0;
+ }
+ }
+
+ /** @return Yaw Axis */
+ public IMUAxis getYawAxis() {
+ return m_yaw_axis;
+ }
+
+ /** @return accumulated gyro angle in the X axis */
+ public synchronized double getGyroAngleX() {
+ return m_integ_gyro_x;
+ }
+
+ /** @return accumulated gyro angle in the Y axis */
+ public synchronized double getGyroAngleY() {
+ return m_integ_gyro_y;
+ }
+
+ /** @return accumulated gyro angle in the Z axis */
+ public synchronized double getGyroAngleZ() {
+ return m_integ_gyro_z;
+ }
+
+ /** @return current gyro angle in the X axis */
+ public synchronized double getGyroInstantX() {
+ return m_gyro_x;
+ }
+
+ /** @return current gyro angle in the Y axis */
+ public synchronized double getGyroInstantY() {
+ return m_gyro_y;
+ }
+
+ /** @return current gyro angle in the Z axis */
+ public synchronized double getGyroInstantZ() {
+ return m_gyro_z;
+ }
+
+ /** @return urrent acceleration in the X axis */
+ public synchronized double getAccelInstantX() {
+ return m_accel_x;
+ }
+
+ /** @return current acceleration in the Y axis */
+ public synchronized double getAccelInstantY() {
+ return m_accel_y;
+ }
+
+ /** @return current acceleration in the Z axis */
+ public synchronized double getAccelInstantZ() {
+ return m_accel_z;
+ }
+
+ /** @return Mag instant X */
+ public synchronized double getMagInstantX() {
+ return m_mag_x;
+ }
+
+ /** @return Mag instant Y */
+ public synchronized double getMagInstantY() {
+ return m_mag_y;
+ }
+
+ /** @return Mag instant Z */
+ public synchronized double getMagInstantZ() {
+ return m_mag_z;
+ }
+
+ /** @return X axis complementary angle */
+ public synchronized double getXComplementaryAngle() {
+ return m_compAngleX;
+ }
+
+ /** @return Y axis complementary angle */
+ public synchronized double getYComplementaryAngle() {
+ return m_compAngleY;
+ }
+
+ /** @return X axis filtered acceleration angle */
+ public synchronized double getXFilteredAccelAngle() {
+ return m_accelAngleX;
+ }
+
+ /** @return Y axis filtered acceleration angle */
+ public synchronized double getYFilteredAccelAngle() {
+ return m_accelAngleY;
+ }
+
+ /** @return Barometric Pressure */
+ public synchronized double getBarometricPressure() {
+ return m_baro;
+ }
+
+ /** @return Temperature */
+ public synchronized double getTemperature() {
+ return m_temp;
+ }
+
+ @Override
+ public void initSendable(NTSendableBuilder builder) {
+ builder.setSmartDashboardType("Gyro");
+ builder.addDoubleProperty("Value", this::getAngle, null);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java
new file mode 100644
index 0000000000..e3a41feeed
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java
@@ -0,0 +1,969 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016. 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+// import java.lang.FdLibm.Pow;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.networktables.NTSendable;
+import edu.wpi.first.networktables.NTSendableBuilder;
+import edu.wpi.first.wpilibj.interfaces.Gyro;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+
+// CHECKSTYLE.OFF: TypeName
+// CHECKSTYLE.OFF: MemberName
+// CHECKSTYLE.OFF: SummaryJavadoc
+// CHECKSTYLE.OFF: UnnecessaryParentheses
+// CHECKSTYLE.OFF: OverloadMethodsDeclarationOrder
+// CHECKSTYLE.OFF: NonEmptyAtclauseDescription
+// CHECKSTYLE.OFF: MissingOverride
+// CHECKSTYLE.OFF: AtclauseOrder
+// CHECKSTYLE.OFF: LocalVariableName
+// CHECKSTYLE.OFF: RedundantModifier
+// CHECKSTYLE.OFF: AbbreviationAsWordInName
+// CHECKSTYLE.OFF: ParameterName
+// CHECKSTYLE.OFF: EmptyCatchBlock
+// CHECKSTYLE.OFF: MissingJavadocMethod
+// CHECKSTYLE.OFF: MissingSwitchDefault
+// CHECKSTYLE.OFF: VariableDeclarationUsageDistance
+// CHECKSTYLE.OFF: ArrayTypeStyle
+
+/** This class is for the ADIS16470 IMU that connects to the RoboRIO SPI port. */
+@SuppressWarnings({
+ "unused",
+ "PMD.RedundantFieldInitializer",
+ "PMD.ImmutableField",
+ "PMD.SingularField",
+ "PMD.CollapsibleIfStatements",
+ "PMD.MissingOverride",
+ "PMD.EmptyIfStmt",
+ "PMD.EmptyStatementNotInLoop"
+})
+public class ADIS16470_IMU implements Gyro, NTSendable {
+ /* ADIS16470 Register Map Declaration */
+ private static final int FLASH_CNT = 0x00; // Flash memory write count
+ private static final int DIAG_STAT = 0x02; // Diagnostic and operational status
+ private static final int X_GYRO_LOW = 0x04; // X-axis gyroscope output, lower word
+ private static final int X_GYRO_OUT = 0x06; // X-axis gyroscope output, upper word
+ private static final int Y_GYRO_LOW = 0x08; // Y-axis gyroscope output, lower word
+ private static final int Y_GYRO_OUT = 0x0A; // Y-axis gyroscope output, upper word
+ private static final int Z_GYRO_LOW = 0x0C; // Z-axis gyroscope output, lower word
+ private static final int Z_GYRO_OUT = 0x0E; // Z-axis gyroscope output, upper word
+ private static final int X_ACCL_LOW = 0x10; // X-axis accelerometer output, lower word
+ private static final int X_ACCL_OUT = 0x12; // X-axis accelerometer output, upper word
+ private static final int Y_ACCL_LOW = 0x14; // Y-axis accelerometer output, lower word
+ private static final int Y_ACCL_OUT = 0x16; // Y-axis accelerometer output, upper word
+ private static final int Z_ACCL_LOW = 0x18; // Z-axis accelerometer output, lower word
+ private static final int Z_ACCL_OUT = 0x1A; // Z-axis accelerometer output, upper word
+ private static final int TEMP_OUT = 0x1C; // Temperature output (internal, not calibrated)
+ private static final int TIME_STAMP = 0x1E; // PPS mode time stamp
+ private static final int X_DELTANG_LOW = 0x24; // X-axis delta angle output, lower word
+ private static final int X_DELTANG_OUT = 0x26; // X-axis delta angle output, upper word
+ private static final int Y_DELTANG_LOW = 0x28; // Y-axis delta angle output, lower word
+ private static final int Y_DELTANG_OUT = 0x2A; // Y-axis delta angle output, upper word
+ private static final int Z_DELTANG_LOW = 0x2C; // Z-axis delta angle output, lower word
+ private static final int Z_DELTANG_OUT = 0x2E; // Z-axis delta angle output, upper word
+ private static final int X_DELTVEL_LOW = 0x30; // X-axis delta velocity output, lower word
+ private static final int X_DELTVEL_OUT = 0x32; // X-axis delta velocity output, upper word
+ private static final int Y_DELTVEL_LOW = 0x34; // Y-axis delta velocity output, lower word
+ private static final int Y_DELTVEL_OUT = 0x36; // Y-axis delta velocity output, upper word
+ private static final int Z_DELTVEL_LOW = 0x38; // Z-axis delta velocity output, lower word
+ private static final int Z_DELTVEL_OUT = 0x3A; // Z-axis delta velocity output, upper word
+ private static final int XG_BIAS_LOW =
+ 0x40; // X-axis gyroscope bias offset correction, lower word
+ private static final int XG_BIAS_HIGH =
+ 0x42; // X-axis gyroscope bias offset correction, upper word
+ private static final int YG_BIAS_LOW =
+ 0x44; // Y-axis gyroscope bias offset correction, lower word
+ private static final int YG_BIAS_HIGH =
+ 0x46; // Y-axis gyroscope bias offset correction, upper word
+ private static final int ZG_BIAS_LOW =
+ 0x48; // Z-axis gyroscope bias offset correction, lower word
+ private static final int ZG_BIAS_HIGH =
+ 0x4A; // Z-axis gyroscope bias offset correction, upper word
+ private static final int XA_BIAS_LOW =
+ 0x4C; // X-axis accelerometer bias offset correction, lower word
+ private static final int XA_BIAS_HIGH =
+ 0x4E; // X-axis accelerometer bias offset correction, upper word
+ private static final int YA_BIAS_LOW =
+ 0x50; // Y-axis accelerometer bias offset correction, lower word
+ private static final int YA_BIAS_HIGH =
+ 0x52; // Y-axis accelerometer bias offset correction, upper word
+ private static final int ZA_BIAS_LOW =
+ 0x54; // Z-axis accelerometer bias offset correction, lower word
+ private static final int ZA_BIAS_HIGH =
+ 0x56; // Z-axis accelerometer bias offset correction, upper word
+ private static final int FILT_CTRL = 0x5C; // Filter control
+ private static final int MSC_CTRL = 0x60; // Miscellaneous control
+ private static final int UP_SCALE = 0x62; // Clock scale factor, PPS mode
+ private static final int DEC_RATE = 0x64; // Decimation rate control (output data rate)
+ private static final int NULL_CNFG = 0x66; // Auto-null configuration control
+ private static final int GLOB_CMD = 0x68; // Global commands
+ private static final int FIRM_REV = 0x6C; // Firmware revision
+ private static final int FIRM_DM = 0x6E; // Firmware revision date, month and day
+ private static final int FIRM_Y = 0x70; // Firmware revision date, year
+ private static final int PROD_ID = 0x72; // Product identification
+ private static final int SERIAL_NUM = 0x74; // Serial number (relative to assembly lot)
+ private static final int USER_SCR1 = 0x76; // User scratch register 1
+ private static final int USER_SCR2 = 0x78; // User scratch register 2
+ private static final int USER_SCR3 = 0x7A; // User scratch register 3
+ private static final int FLSHCNT_LOW = 0x7C; // Flash update count, lower word
+ private static final int FLSHCNT_HIGH = 0x7E; // Flash update count, upper word
+
+ private static final byte[] m_autospi_x_packet = {
+ X_DELTANG_OUT,
+ FLASH_CNT,
+ X_DELTANG_LOW,
+ FLASH_CNT,
+ X_GYRO_OUT,
+ FLASH_CNT,
+ Y_GYRO_OUT,
+ FLASH_CNT,
+ Z_GYRO_OUT,
+ FLASH_CNT,
+ X_ACCL_OUT,
+ FLASH_CNT,
+ Y_ACCL_OUT,
+ FLASH_CNT,
+ Z_ACCL_OUT,
+ FLASH_CNT
+ };
+
+ private static final byte[] m_autospi_y_packet = {
+ Y_DELTANG_OUT,
+ FLASH_CNT,
+ Y_DELTANG_LOW,
+ FLASH_CNT,
+ X_GYRO_OUT,
+ FLASH_CNT,
+ Y_GYRO_OUT,
+ FLASH_CNT,
+ Z_GYRO_OUT,
+ FLASH_CNT,
+ X_ACCL_OUT,
+ FLASH_CNT,
+ Y_ACCL_OUT,
+ FLASH_CNT,
+ Z_ACCL_OUT,
+ FLASH_CNT
+ };
+
+ private static final byte[] m_autospi_z_packet = {
+ Z_DELTANG_OUT,
+ FLASH_CNT,
+ Z_DELTANG_LOW,
+ FLASH_CNT,
+ X_GYRO_OUT,
+ FLASH_CNT,
+ Y_GYRO_OUT,
+ FLASH_CNT,
+ Z_GYRO_OUT,
+ FLASH_CNT,
+ X_ACCL_OUT,
+ FLASH_CNT,
+ Y_ACCL_OUT,
+ FLASH_CNT,
+ Z_ACCL_OUT,
+ FLASH_CNT
+ };
+
+ public enum IMUAxis {
+ kX,
+ kY,
+ kZ
+ }
+
+ public enum ADIS16470CalibrationTime {
+ _32ms(0),
+ _64ms(1),
+ _128ms(2),
+ _256ms(3),
+ _512ms(4),
+ _1s(5),
+ _2s(6),
+ _4s(7),
+ _8s(8),
+ _16s(9),
+ _32s(10),
+ _64s(11);
+
+ private int value;
+
+ private ADIS16470CalibrationTime(int value) {
+ this.value = value;
+ }
+ }
+
+ // Static Constants
+ private static final double delta_angle_sf = 2160.0 / 2147483648.0; /* 2160 / (2^31) */
+ private static final double rad_to_deg = 57.2957795;
+ private static final double deg_to_rad = 0.0174532;
+ private static final double grav = 9.81;
+
+ // User-specified yaw axis
+ private IMUAxis m_yaw_axis;
+
+ // Instant raw outputs
+ private double m_gyro_x = 0.0;
+ private double m_gyro_y = 0.0;
+ private double m_gyro_z = 0.0;
+ private double m_accel_x = 0.0;
+ private double m_accel_y = 0.0;
+ private double m_accel_z = 0.0;
+
+ // Integrated gyro angle
+ private double m_integ_angle = 0.0;
+
+ // Complementary filter variables
+ private double m_dt = 0.0;
+ private double m_alpha = 0.0;
+ private double m_tau = 1.0;
+ private double m_compAngleX = 0.0;
+ private double m_compAngleY = 0.0;
+ private double m_accelAngleX = 0.0;
+ private double m_accelAngleY = 0.0;
+
+ // State variables
+ private volatile boolean m_thread_active = false;
+ private int m_calibration_time = 0;
+ private volatile boolean m_first_run = true;
+ private volatile boolean m_thread_idle = false;
+ private boolean m_auto_configured = false;
+ private double m_scaled_sample_rate = 2500.0;
+
+ // Resources
+ private SPI m_spi;
+ private SPI.Port m_spi_port;
+ private DigitalInput m_auto_interrupt;
+ private DigitalOutput m_reset_out;
+ private DigitalInput m_reset_in;
+ private DigitalOutput m_status_led;
+ private Thread m_acquire_task;
+
+ private static class AcquireTask implements Runnable {
+ private ADIS16470_IMU imu;
+
+ public AcquireTask(ADIS16470_IMU imu) {
+ this.imu = imu;
+ }
+
+ @Override
+ public void run() {
+ imu.acquire();
+ }
+ }
+
+ public ADIS16470_IMU() {
+ this(IMUAxis.kZ, SPI.Port.kOnboardCS0, ADIS16470CalibrationTime._4s);
+ }
+
+ /**
+ * @param yaw_axis The axis that measures the yaw
+ * @param port The SPI Port the gyro is plugged into
+ * @param cal_time Calibration time
+ */
+ public ADIS16470_IMU(IMUAxis yaw_axis, SPI.Port port, ADIS16470CalibrationTime cal_time) {
+ m_yaw_axis = yaw_axis;
+ m_calibration_time = cal_time.value;
+ m_spi_port = port;
+
+ m_acquire_task = new Thread(new AcquireTask(this));
+
+ // Force the IMU reset pin to toggle on startup (doesn't require DS enable)
+ // Relies on the RIO hardware by default configuring an output as low
+ // and configuring an input as high Z. The 10k pull-up resistor internal to the
+ // IMU then forces the reset line high for normal operation.
+ m_reset_out = new DigitalOutput(27); // Drive SPI CS2 (IMU RST) low
+ Timer.delay(0.01); // Wait 10ms
+ m_reset_out.close();
+ m_reset_in = new DigitalInput(27); // Set SPI CS2 (IMU RST) high
+ Timer.delay(0.25); // Wait 250ms for reset to complete
+
+ if (!switchToStandardSPI()) {
+ return;
+ }
+
+ // Set IMU internal decimation to 4 (output data rate of 2000 SPS / (4 + 1) =
+ // 400Hz)
+ writeRegister(DEC_RATE, 4);
+
+ // Set data ready polarity (HIGH = Good Data), Disable gSense Compensation and
+ // PoP
+ writeRegister(MSC_CTRL, 1);
+
+ // Configure IMU internal Bartlett filter
+ writeRegister(FILT_CTRL, 0);
+
+ // Configure continuous bias calibration time based on user setting
+ writeRegister(NULL_CNFG, (m_calibration_time | 0x0700));
+
+ // Notify DS that IMU calibration delay is active
+ DriverStation.reportWarning(
+ "ADIS16470 IMU Detected. Starting initial calibration delay.", false);
+
+ // Wait for samples to accumulate internal to the IMU (110% of user-defined
+ // time)
+ try {
+ Thread.sleep((long) (Math.pow(2.0, m_calibration_time) / 2000 * 64 * 1.1 * 1000));
+ } catch (InterruptedException e) {
+ }
+
+ // Write offset calibration command to IMU
+ writeRegister(GLOB_CMD, 0x0001);
+
+ // Configure and enable auto SPI
+ if (!switchToAutoSPI()) {
+ return;
+ }
+
+ // Let the user know the IMU was initiallized successfully
+ DriverStation.reportWarning("ADIS16470 IMU Successfully Initialized!", false);
+
+ // Drive "Ready" LED low
+ m_status_led = new DigitalOutput(28); // Set SPI CS3 (IMU Ready LED) low
+
+ // Report usage and post data to DS
+ HAL.report(tResourceType.kResourceType_ADIS16470, 0);
+ }
+
+ /**
+ * @param buf
+ * @return
+ */
+ private static int toUShort(ByteBuffer buf) {
+ return (buf.getShort(0)) & 0xFFFF;
+ }
+
+ /**
+ * @param sint
+ * @return
+ */
+ private static long toULong(int sint) {
+ return sint & 0x00000000FFFFFFFFL;
+ }
+
+ /**
+ * @param buf
+ * @return
+ */
+ private static int toShort(int... buf) {
+ return (short) (((buf[0] & 0xFF) << 8) + ((buf[1] & 0xFF) << 0));
+ }
+
+ /**
+ * @param buf
+ * @return
+ */
+ private static int toInt(int... buf) {
+ return (int)
+ ((buf[0] & 0xFF) << 24 | (buf[1] & 0xFF) << 16 | (buf[2] & 0xFF) << 8 | (buf[3] & 0xFF));
+ }
+
+ /**
+ * Switch to standard SPI mode.
+ *
+ * @return
+ */
+ private boolean switchToStandardSPI() {
+ // Check to see whether the acquire thread is active. If so, wait for it to stop
+ // producing data.
+ if (m_thread_active) {
+ m_thread_active = false;
+ while (!m_thread_idle) {
+ try {
+ Thread.sleep(10);
+ } catch (InterruptedException e) {
+ }
+ }
+ System.out.println("Paused the IMU processing thread successfully!");
+ // Maybe we're in auto SPI mode? If so, kill auto SPI, and then SPI.
+ if (m_spi != null && m_auto_configured) {
+ m_spi.stopAuto();
+ // We need to get rid of all the garbage left in the auto SPI buffer after
+ // stopping it.
+ // Sometimes data magically reappears, so we have to check the buffer size a
+ // couple of times
+ // to be sure we got it all. Yuck.
+ int[] trashBuffer = new int[200];
+ try {
+ Thread.sleep(100);
+ } catch (InterruptedException e) {
+ }
+ int data_count = m_spi.readAutoReceivedData(trashBuffer, 0, 0);
+ while (data_count > 0) {
+ m_spi.readAutoReceivedData(trashBuffer, Math.min(data_count, 200), 0);
+ data_count = m_spi.readAutoReceivedData(trashBuffer, 0, 0);
+ }
+ System.out.println("Paused auto SPI successfully.");
+ }
+ }
+ // There doesn't seem to be a SPI port active. Let's try to set one up
+ if (m_spi == null) {
+ System.out.println("Setting up a new SPI port.");
+ m_spi = new SPI(m_spi_port);
+ m_spi.setClockRate(2000000);
+ m_spi.setMSBFirst();
+ m_spi.setSampleDataOnTrailingEdge();
+ m_spi.setClockActiveLow();
+ m_spi.setChipSelectActiveLow();
+ readRegister(PROD_ID); // Dummy read
+
+ // Validate the product ID
+ if (readRegister(PROD_ID) != 16982) {
+ DriverStation.reportError("Could not find ADIS16470", false);
+ close();
+ return false;
+ }
+ return true;
+ } else {
+ // Maybe the SPI port is active, but not in auto SPI mode? Try to read the
+ // product ID.
+ readRegister(PROD_ID); // dummy read
+ if (readRegister(PROD_ID) != 16982) {
+ DriverStation.reportError("Could not find an ADIS16470", false);
+ close();
+ return false;
+ } else {
+ return true;
+ }
+ }
+ }
+
+ /** @return */
+ boolean switchToAutoSPI() {
+ // No SPI port has been set up. Go set one up first.
+ if (m_spi == null) {
+ if (!switchToStandardSPI()) {
+ DriverStation.reportError("Failed to start/restart auto SPI", false);
+ return false;
+ }
+ }
+ // Only set up the interrupt if needed.
+ if (m_auto_interrupt == null) {
+ // Configure interrupt on SPI CS1
+ m_auto_interrupt = new DigitalInput(26);
+ }
+ // The auto SPI controller gets angry if you try to set up two instances on one
+ // bus.
+ if (!m_auto_configured) {
+ m_spi.initAuto(8200);
+ m_auto_configured = true;
+ }
+ // Do we need to change auto SPI settings?
+ switch (m_yaw_axis) {
+ case kX:
+ m_spi.setAutoTransmitData(m_autospi_x_packet, 2);
+ break;
+ case kY:
+ m_spi.setAutoTransmitData(m_autospi_y_packet, 2);
+ break;
+ default:
+ m_spi.setAutoTransmitData(m_autospi_z_packet, 2);
+ break;
+ }
+ // Configure auto stall time
+ m_spi.configureAutoStall(5, 1000, 1);
+ // Kick off auto SPI (Note: Device configration impossible after auto SPI is
+ // activated)
+ // DR High = Data good (data capture should be triggered on the rising edge)
+ m_spi.startAutoTrigger(m_auto_interrupt, true, false);
+
+ // Check to see if the acquire thread is running. If not, kick one off.
+ if (!m_acquire_task.isAlive()) {
+ m_first_run = true;
+ m_thread_active = true;
+ m_acquire_task.start();
+ System.out.println("Processing thread activated!");
+ } else {
+ // The thread was running, re-init run variables and start it up again.
+ m_first_run = true;
+ m_thread_active = true;
+ System.out.println("Processing thread activated!");
+ }
+ // Looks like the thread didn't start for some reason. Abort.
+ if (!m_acquire_task.isAlive()) {
+ DriverStation.reportError("Failed to start/restart the acquire() thread.", false);
+ close();
+ return false;
+ }
+ return true;
+ }
+
+ /**
+ * Configures calibration time
+ *
+ * @param new_cal_time New calibration time
+ * @return 1 if the new calibration time is the same as the current one else 0
+ */
+ public int configCalTime(ADIS16470CalibrationTime new_cal_time) {
+ if (m_calibration_time == new_cal_time.value) {
+ return 1;
+ }
+ if (!switchToStandardSPI()) {
+ DriverStation.reportError("Failed to configure/reconfigure standard SPI.", false);
+ return 2;
+ }
+ m_calibration_time = new_cal_time.value;
+ writeRegister(NULL_CNFG, (m_calibration_time | 0x700));
+ if (!switchToAutoSPI()) {
+ DriverStation.reportError("Failed to configure/reconfigure auto SPI.", false);
+ return 2;
+ }
+ return 0;
+ }
+
+ public int configDecRate(int reg) {
+ int m_reg = reg;
+ if (!switchToStandardSPI()) {
+ DriverStation.reportError("Failed to configure/reconfigure standard SPI.", false);
+ return 2;
+ }
+ if (m_reg > 1999) {
+ DriverStation.reportError("Attemted to write an invalid deimation value.", false);
+ m_reg = 1999;
+ }
+ m_scaled_sample_rate = (((m_reg + 1.0) / 2000.0) * 1000000.0);
+ writeRegister(DEC_RATE, m_reg);
+ System.out.println("Decimation register: " + readRegister(DEC_RATE));
+ if (!switchToAutoSPI()) {
+ DriverStation.reportError("Failed to configure/reconfigure auto SPI.", false);
+ return 2;
+ }
+ return 0;
+ }
+
+ /** {@inheritDoc} */
+ @Override
+ public void calibrate() {
+ if (!switchToStandardSPI()) {
+ DriverStation.reportError("Failed to configure/reconfigure standard SPI.", false);
+ }
+ writeRegister(GLOB_CMD, 0x0001);
+ if (!switchToAutoSPI()) {
+ DriverStation.reportError("Failed to configure/reconfigure auto SPI.", false);
+ }
+ ;
+ }
+
+ /**
+ * Sets the yaw axis
+ *
+ * @param yaw_axis The new yaw axis to use
+ * @return 1 if the new yaw axis is the same as the current one, 2 if the switch to Standard SPI
+ * failed, else 0.
+ */
+ public int setYawAxis(IMUAxis yaw_axis) {
+ if (m_yaw_axis == yaw_axis) {
+ return 1;
+ }
+ if (!switchToStandardSPI()) {
+ DriverStation.reportError("Failed to configure/reconfigure standard SPI.", false);
+ return 2;
+ }
+ m_yaw_axis = yaw_axis;
+ if (!switchToAutoSPI()) {
+ DriverStation.reportError("Failed to configure/reconfigure auto SPI.", false);
+ }
+ return 0;
+ }
+
+ /**
+ * @param reg
+ * @return
+ */
+ private int readRegister(int reg) {
+ ByteBuffer buf = ByteBuffer.allocateDirect(2);
+ buf.order(ByteOrder.BIG_ENDIAN);
+ buf.put(0, (byte) (reg & 0x7f));
+ buf.put(1, (byte) 0);
+
+ m_spi.write(buf, 2);
+ m_spi.read(false, buf, 2);
+
+ return toUShort(buf);
+ }
+
+ /**
+ * @param reg
+ * @param val
+ */
+ private void writeRegister(int reg, int val) {
+ ByteBuffer buf = ByteBuffer.allocateDirect(2);
+ // low byte
+ buf.put(0, (byte) ((0x80 | reg)));
+ buf.put(1, (byte) (val & 0xff));
+ m_spi.write(buf, 2);
+ // high byte
+ buf.put(0, (byte) (0x81 | reg));
+ buf.put(1, (byte) (val >> 8));
+ m_spi.write(buf, 2);
+ }
+
+ /** {@inheritDoc} */
+ public void reset() {
+ synchronized (this) {
+ m_integ_angle = 0.0;
+ }
+ }
+
+ /** Delete (free) the spi port used for the IMU. */
+ @Override
+ public void close() {
+ if (m_thread_active) {
+ m_thread_active = false;
+ try {
+ if (m_acquire_task != null) {
+ m_acquire_task.join();
+ m_acquire_task = null;
+ }
+ } catch (InterruptedException e) {
+ }
+ if (m_spi != null) {
+ if (m_auto_configured) {
+ m_spi.stopAuto();
+ }
+ m_spi.close();
+ m_auto_configured = false;
+ if (m_auto_interrupt != null) {
+ m_auto_interrupt.close();
+ m_auto_interrupt = null;
+ }
+ m_spi = null;
+ }
+ }
+ System.out.println("Finished cleaning up after the IMU driver.");
+ }
+
+ /** */
+ private void acquire() {
+ // Set data packet length
+ final int dataset_len = 19; // 18 data points + timestamp
+ final int BUFFER_SIZE = 4000;
+
+ // Set up buffers and variables
+ int[] buffer = new int[BUFFER_SIZE];
+ int data_count = 0;
+ int data_remainder = 0;
+ int data_to_read = 0;
+ double previous_timestamp = 0.0;
+ double delta_angle = 0.0;
+ double gyro_x = 0.0;
+ double gyro_y = 0.0;
+ double gyro_z = 0.0;
+ double accel_x = 0.0;
+ double accel_y = 0.0;
+ double accel_z = 0.0;
+ double gyro_x_si = 0.0;
+ double gyro_y_si = 0.0;
+ double gyro_z_si = 0.0;
+ double accel_x_si = 0.0;
+ double accel_y_si = 0.0;
+ double accel_z_si = 0.0;
+ double compAngleX = 0.0;
+ double compAngleY = 0.0;
+ double accelAngleX = 0.0;
+ double accelAngleY = 0.0;
+
+ while (true) {
+ // Sleep loop for 10ms
+ try {
+ Thread.sleep(10);
+ } catch (InterruptedException e) {
+ }
+
+ if (m_thread_active) {
+ m_thread_idle = false;
+
+ data_count =
+ m_spi.readAutoReceivedData(
+ buffer, 0, 0); // Read number of bytes currently stored in the
+ // buffer
+ data_remainder =
+ data_count % dataset_len; // Check if frame is incomplete. Add 1 because of timestamp
+ data_to_read = data_count - data_remainder; // Remove incomplete data from read count
+ /* Want to cap the data to read in a single read at the buffer size */
+ if (data_to_read > BUFFER_SIZE) {
+ DriverStation.reportWarning(
+ "ADIS16470 data processing thread overrun has occurred!", false);
+ data_to_read = BUFFER_SIZE - (BUFFER_SIZE % dataset_len);
+ }
+ m_spi.readAutoReceivedData(
+ buffer, data_to_read, 0); // Read data from DMA buffer (only complete sets)
+
+ // Could be multiple data sets in the buffer. Handle each one.
+ for (int i = 0; i < data_to_read; i += dataset_len) {
+ // Timestamp is at buffer[i]
+ m_dt = ((double) buffer[i] - previous_timestamp) / 1000000.0;
+
+ /*
+ * System.out.println(((toInt(buffer[i + 3], buffer[i + 4], buffer[i + 5],
+ * buffer[i + 6]))*delta_angle_sf) / ((10000.0 / (buffer[i] -
+ * previous_timestamp)) / 100.0));
+ * // DEBUG: Plot Sub-Array Data in Terminal
+ * for (int j = 0; j < data_to_read; j++) {
+ * System.out.print(buffer[j]);
+ * System.out.print(" ,");
+ * }
+ * System.out.println(" ");
+ * //System.out.println(((toInt(buffer[i + 3], buffer[i + 4], buffer[i + 5],
+ * buffer[i + 6]))*delta_angle_sf) / ((10000.0 / (buffer[i] -
+ * previous_timestamp)) / 100.0) + "," + buffer[3] + "," + buffer[4] + "," +
+ * buffer[5] + "," + buffer[6]
+ * /*toShort(buffer[7], buffer[8]) + "," +
+ * toShort(buffer[9], buffer[10]) + "," +
+ * toShort(buffer[11], buffer[12]) + "," +
+ * toShort(buffer[13], buffer[14]) + "," +
+ * toShort(buffer[15], buffer[16]) + ","
+ * + toShort(buffer[17], buffer[18]));
+ */
+
+ /*
+ * Get delta angle value for selected yaw axis and scale by the elapsed time
+ * (based on timestamp)
+ */
+ delta_angle =
+ (toInt(buffer[i + 3], buffer[i + 4], buffer[i + 5], buffer[i + 6]) * delta_angle_sf)
+ / (m_scaled_sample_rate / (buffer[i] - previous_timestamp));
+ gyro_x = (toShort(buffer[i + 7], buffer[i + 8]) / 10.0);
+ gyro_y = (toShort(buffer[i + 9], buffer[i + 10]) / 10.0);
+ gyro_z = (toShort(buffer[i + 11], buffer[i + 12]) / 10.0);
+ accel_x = (toShort(buffer[i + 13], buffer[i + 14]) / 800.0);
+ accel_y = (toShort(buffer[i + 15], buffer[i + 16]) / 800.0);
+ accel_z = (toShort(buffer[i + 17], buffer[i + 18]) / 800.0);
+
+ // Convert scaled sensor data to SI units (for tilt calculations)
+ // TODO: Should the unit outputs be selectable?
+ gyro_x_si = gyro_x * deg_to_rad;
+ gyro_y_si = gyro_y * deg_to_rad;
+ gyro_z_si = gyro_z * deg_to_rad;
+ accel_x_si = accel_x * grav;
+ accel_y_si = accel_y * grav;
+ accel_z_si = accel_z * grav;
+
+ // Store timestamp for next iteration
+ previous_timestamp = buffer[i];
+
+ m_alpha = m_tau / (m_tau + m_dt);
+
+ if (m_first_run) {
+ // Set up inclinometer calculations for first run
+ accelAngleX =
+ Math.atan2(
+ accel_x_si, Math.sqrt((accel_y_si * accel_y_si) + (accel_z_si * accel_z_si)));
+ accelAngleY =
+ Math.atan2(
+ accel_y_si, Math.sqrt((accel_x_si * accel_x_si) + (accel_z_si * accel_z_si)));
+ compAngleX = accelAngleX;
+ compAngleY = accelAngleY;
+ } else {
+ // Run inclinometer calculations
+ accelAngleX =
+ Math.atan2(
+ accel_x_si, Math.sqrt((accel_y_si * accel_y_si) + (accel_z_si * accel_z_si)));
+ accelAngleY =
+ Math.atan2(
+ accel_y_si, Math.sqrt((accel_x_si * accel_x_si) + (accel_z_si * accel_z_si)));
+ accelAngleX = formatAccelRange(accelAngleX, accel_z_si);
+ accelAngleY = formatAccelRange(accelAngleY, accel_z_si);
+ compAngleX = compFilterProcess(compAngleX, accelAngleX, -gyro_y_si);
+ compAngleY = compFilterProcess(compAngleY, accelAngleY, gyro_x_si);
+ }
+
+ synchronized (this) {
+ /* Push data to global variables */
+ if (m_first_run) {
+ /*
+ * Don't accumulate first run. previous_timestamp will be "very" old and the
+ * integration will end up way off
+ */
+ m_integ_angle = 0.0;
+ } else {
+ m_integ_angle += delta_angle;
+ }
+ m_gyro_x = gyro_x;
+ m_gyro_y = gyro_y;
+ m_gyro_z = gyro_z;
+ m_accel_x = accel_x;
+ m_accel_y = accel_y;
+ m_accel_z = accel_z;
+ m_compAngleX = compAngleX * rad_to_deg;
+ m_compAngleY = compAngleY * rad_to_deg;
+ m_accelAngleX = accelAngleX * rad_to_deg;
+ m_accelAngleY = accelAngleY * rad_to_deg;
+ }
+ m_first_run = false;
+ }
+ } else {
+ m_thread_idle = true;
+ data_count = 0;
+ data_remainder = 0;
+ data_to_read = 0;
+ previous_timestamp = 0.0;
+ delta_angle = 0.0;
+ gyro_x = 0.0;
+ gyro_y = 0.0;
+ gyro_z = 0.0;
+ accel_x = 0.0;
+ accel_y = 0.0;
+ accel_z = 0.0;
+ gyro_x_si = 0.0;
+ gyro_y_si = 0.0;
+ gyro_z_si = 0.0;
+ accel_x_si = 0.0;
+ accel_y_si = 0.0;
+ accel_z_si = 0.0;
+ compAngleX = 0.0;
+ compAngleY = 0.0;
+ accelAngleX = 0.0;
+ accelAngleY = 0.0;
+ }
+ }
+ }
+
+ /**
+ * @param compAngle
+ * @param accAngle
+ * @return
+ */
+ private double formatFastConverge(double compAngle, double accAngle) {
+ if (compAngle > accAngle + Math.PI) {
+ compAngle = compAngle - 2.0 * Math.PI;
+ } else if (accAngle > compAngle + Math.PI) {
+ compAngle = compAngle + 2.0 * Math.PI;
+ }
+ return compAngle;
+ }
+
+ /**
+ * @param compAngle
+ * @return
+ */
+ private double formatRange0to2PI(double compAngle) {
+ while (compAngle >= 2 * Math.PI) {
+ compAngle = compAngle - 2.0 * Math.PI;
+ }
+ while (compAngle < 0.0) {
+ compAngle = compAngle + 2.0 * Math.PI;
+ }
+ return compAngle;
+ }
+
+ /**
+ * @param accelAngle
+ * @param accelZ
+ * @return
+ */
+ private double formatAccelRange(double accelAngle, double accelZ) {
+ if (accelZ < 0.0) {
+ accelAngle = Math.PI - accelAngle;
+ } else if (accelZ > 0.0 && accelAngle < 0.0) {
+ accelAngle = 2.0 * Math.PI + accelAngle;
+ }
+ return accelAngle;
+ }
+
+ /**
+ * @param compAngle
+ * @param accelAngle
+ * @param omega
+ * @return
+ */
+ private double compFilterProcess(double compAngle, double accelAngle, double omega) {
+ compAngle = formatFastConverge(compAngle, accelAngle);
+ compAngle = m_alpha * (compAngle + omega * m_dt) + (1.0 - m_alpha) * accelAngle;
+ compAngle = formatRange0to2PI(compAngle);
+ if (compAngle > Math.PI) {
+ compAngle = compAngle - 2.0 * Math.PI;
+ }
+ return compAngle;
+ }
+
+ /** {@inheritDoc} */
+ public synchronized double getAngle() {
+ return m_integ_angle;
+ }
+
+ /** {@inheritDoc} */
+ public synchronized double getRate() {
+ switch (m_yaw_axis) {
+ case kX:
+ return m_gyro_x;
+ case kY:
+ return m_gyro_y;
+ case kZ:
+ return m_gyro_z;
+ }
+ return 0.0;
+ }
+
+ /** @return Yaw Axis */
+ public IMUAxis getYawAxis() {
+ return m_yaw_axis;
+ }
+
+ /** @return current gyro angle in the X direction */
+ public synchronized double getGyroInstantX() {
+ return m_gyro_x;
+ }
+
+ /** @return current gyro angle in the Y axis */
+ public synchronized double getGyroInstantY() {
+ return m_gyro_y;
+ }
+
+ /** @return current gyro angle in the Z axis */
+ public synchronized double getGyroInstantZ() {
+ return m_gyro_z;
+ }
+
+ /** @return current acceleration in the X axis */
+ public synchronized double getAccelInstantX() {
+ return m_accel_x;
+ }
+
+ /** @return current acceleration in the Y axis */
+ public synchronized double getAccelInstantY() {
+ return m_accel_y;
+ }
+
+ /** @return current acceleration in the Z axis */
+ public synchronized double getAccelInstantZ() {
+ return m_accel_z;
+ }
+
+ /** @return X axis complementary angle */
+ public synchronized double getXComplementaryAngle() {
+ return m_compAngleX;
+ }
+
+ /** @return Y axis complementary angle */
+ public synchronized double getYComplementaryAngle() {
+ return m_compAngleY;
+ }
+
+ /** @return X axis filtered acceleration angle */
+ public synchronized double getXFilteredAccelAngle() {
+ return m_accelAngleX;
+ }
+
+ /** @return Y axis filtered acceleration angle */
+ public synchronized double getYFilteredAccelAngle() {
+ return m_accelAngleY;
+ }
+
+ @Override
+ public void initSendable(NTSendableBuilder builder) {
+ builder.setSmartDashboardType("Gyro");
+ builder.addDoubleProperty("Value", this::getAngle, null);
+ }
+}