From 1f59ff72f9da8b9f07820537b9075a9b7db0058d Mon Sep 17 00:00:00 2001 From: Thad House Date: Thu, 30 Dec 2021 19:43:53 -0800 Subject: [PATCH] [wpilib] Add ADIS IMUs (#3777) Co-authored-by: Tyler Veness Co-authored-by: Matteo Kimura --- styleguide/spotbugs-exclude.xml | 8 + wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp | 857 ++++++++++++++ .../src/main/native/cpp/ADIS_16470_IMU.cpp | 795 +++++++++++++ .../main/native/include/frc/ADIS16448_IMU.h | 378 ++++++ .../main/native/include/frc/ADIS16470_IMU.h | 390 +++++++ .../edu/wpi/first/wpilibj/ADIS16448_IMU.java | 1021 +++++++++++++++++ .../edu/wpi/first/wpilibj/ADIS16470_IMU.java | 969 ++++++++++++++++ 7 files changed, 4418 insertions(+) create mode 100644 wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp create mode 100644 wpilibc/src/main/native/cpp/ADIS_16470_IMU.cpp create mode 100644 wpilibc/src/main/native/include/frc/ADIS16448_IMU.h create mode 100644 wpilibc/src/main/native/include/frc/ADIS16470_IMU.h create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java 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); + } +}