diff --git a/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp index 8ba4a96c40..74a3e42066 100644 --- a/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp +++ b/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp @@ -29,24 +29,17 @@ #include #include "frc/Errors.h" +#include "frc/MathUtil.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 { @@ -100,27 +93,28 @@ ADIS16448_IMU::ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port, // 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; + DigitalOutput* reset_out = new DigitalOutput(18); // Drive MXP DIO8 low + Wait(10_ms); + delete reset_out; m_reset_in = new DigitalInput(18); // Set MXP DIO8 high - Wait(500_ms); // Wait 500ms for reset to complete + Wait(500_ms); // Wait for reset to complete ConfigCalTime(cal_time); + m_spi = new SPI(m_spi_port); + m_spi->SetClockRate(1000000); + m_spi->SetMode(frc::SPI::Mode::kMode3); + m_spi->SetChipSelectActiveLow(); // Configure standard SPI if (!SwitchToStandardSPI()) { return; } - - // Set up flash state variable - bool m_needs_flash = false; - + bool needsFlash = false; // Set IMU internal decimation to 1 (output data rate of 819.2 SPS / (1 + 1) // = 409.6Hz), output bandwidth = 204.8Hz if (ReadRegister(SMPL_PRD) != 0x0001) { WriteRegister(SMPL_PRD, 0x0001); - m_needs_flash = true; + needsFlash = true; REPORT_WARNING( "ADIS16448: SMPL_PRD register configuration inconsistent! Scheduling " "flash update."); @@ -129,7 +123,7 @@ ADIS16448_IMU::ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port, // Set data ready polarity (LOW = Good Data) on DIO1 (PWM0 on MXP) if (ReadRegister(MSC_CTRL) != 0x0016) { WriteRegister(MSC_CTRL, 0x0016); - m_needs_flash = true; + needsFlash = true; REPORT_WARNING( "ADIS16448: MSC_CTRL register configuration inconsistent! Scheduling " "flash update."); @@ -139,7 +133,7 @@ ADIS16448_IMU::ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port, // set IMU scale factor (range) if (ReadRegister(SENS_AVG) != 0x0400) { WriteRegister(SENS_AVG, 0x0400); - m_needs_flash = true; + needsFlash = true; REPORT_WARNING( "ADIS16448: SENS_AVG register configuration inconsistent! Scheduling " "flash update."); @@ -147,7 +141,7 @@ ADIS16448_IMU::ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port, // Clear offset registers if (ReadRegister(XGYRO_OFF) != 0x0000) { WriteRegister(XGYRO_OFF, 0x0000); - m_needs_flash = true; + needsFlash = true; REPORT_WARNING( "ADIS16448: XGYRO_OFF register configuration inconsistent! " "Scheduling flash update."); @@ -155,7 +149,7 @@ ADIS16448_IMU::ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port, if (ReadRegister(YGYRO_OFF) != 0x0000) { WriteRegister(YGYRO_OFF, 0x0000); - m_needs_flash = true; + needsFlash = true; REPORT_WARNING( "ADIS16448: YGYRO_OFF register configuration inconsistent! " "Scheduling flash update."); @@ -163,7 +157,7 @@ ADIS16448_IMU::ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port, if (ReadRegister(ZGYRO_OFF) != 0x0000) { WriteRegister(ZGYRO_OFF, 0x0000); - m_needs_flash = true; + needsFlash = true; REPORT_WARNING( "ADIS16448: ZGYRO_OFF register configuration inconsistent! " "Scheduling flash update."); @@ -171,22 +165,22 @@ ADIS16448_IMU::ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port, // If any registers on the IMU don't match the config, trigger a flash // update - if (m_needs_flash) { + if (needsFlash) { REPORT_WARNING( "ADIS16448: Register configuration changed! Starting IMU flash " "update."); WriteRegister(GLOB_CMD, 0x0008); - // Wait long enough for the flash update to finish (72ms minimum as per + // Wait long enough for the flash update to finish (75ms minimum as per // the datasheet) Wait(0.5_s); REPORT_WARNING("ADIS16448: Flash update finished!"); - m_needs_flash = false; } else { REPORT_WARNING( "ADIS16448: Flash and RAM configuration consistent. No flash update " "required!"); } + m_auto_interrupt = new DigitalInput(10); // Configure and enable auto SPI if (!SwitchToAutoSPI()) { return; @@ -202,11 +196,11 @@ ADIS16448_IMU::ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port, // Tell the acquire loop that we're done starting up m_start_up_mode = false; - // Let the user know the IMU was initiallized successfully + // Let the user know the IMU was initialized 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) + // Drive MXP PWM5 (IMU ready LED) low (active low) m_status_led = new DigitalOutput(19); } @@ -232,7 +226,6 @@ ADIS16448_IMU::ADIS16448_IMU(ADIS16448_IMU&& other) m_mag_z{std::move(other.m_mag_z)}, m_baro{std::move(other.m_baro)}, m_temp{std::move(other.m_temp)}, - m_tau{std::move(other.m_tau)}, m_dt{std::move(other.m_dt)}, m_alpha{std::move(other.m_alpha)}, m_compAngleX{std::move(other.m_compAngleX)}, @@ -291,7 +284,6 @@ ADIS16448_IMU& ADIS16448_IMU::operator=(ADIS16448_IMU&& other) { std::swap(this->m_mag_z, other.m_mag_z); std::swap(this->m_baro, other.m_baro); std::swap(this->m_temp, other.m_temp); - std::swap(this->m_tau, other.m_tau); std::swap(this->m_dt, other.m_dt); std::swap(this->m_alpha, other.m_alpha); std::swap(this->m_compAngleX, other.m_compAngleX); @@ -362,54 +354,34 @@ bool ADIS16448_IMU::SwitchToStandardSPI() { Wait(10_ms); } // Maybe we're in auto SPI mode? If so, kill auto SPI, and then SPI. - if (m_spi != nullptr && m_auto_configured) { + if (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. + // 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 */ + // 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 */ + // Update remaining buffer count data_count = m_spi->ReadAutoReceivedData(trashBuffer, 0, 0_s); } } } - // There doesn't seem to be a SPI port active. Let's try to set one up - if (m_spi == nullptr) { - m_spi = new SPI(m_spi_port); - m_spi->SetClockRate(1000000); - m_spi->SetMode(frc::SPI::Mode::kMode3); - 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; - } + 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; } void ADIS16448_IMU::InitOffsetBuffer(int size) { @@ -442,17 +414,6 @@ void ADIS16448_IMU::InitOffsetBuffer(int size) { *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) { @@ -489,9 +450,6 @@ bool ADIS16448_IMU::SwitchToAutoSPI() { return true; } -/** - * - **/ int ADIS16448_IMU::ConfigCalTime(CalibrationTime new_cal_time) { if (m_calibration_time == new_cal_time) { return 1; @@ -503,9 +461,6 @@ int ADIS16448_IMU::ConfigCalTime(CalibrationTime new_cal_time) { } } -/** - * - **/ void ADIS16448_IMU::Calibrate() { std::scoped_lock sync(m_mutex); // Calculate the running average @@ -526,13 +481,6 @@ void ADIS16448_IMU::Calibrate() { m_integ_gyro_angle_z = 0.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; @@ -541,15 +489,15 @@ uint16_t ADIS16448_IMU::ReadRegister(uint8_t reg) { m_spi->Write(buf, 2); m_spi->Read(false, buf, 2); - return ToUShort(buf); + return (static_cast(buf[0]) << 8) | buf[1]; } /** * 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. + * via SPI. The upper and lower bytes that make up the 16-bit value are split + * into two unsigned, 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]; @@ -561,10 +509,6 @@ void ADIS16448_IMU::WriteRegister(uint8_t reg, uint16_t val) { 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_angle_x = 0.0; @@ -609,126 +553,93 @@ ADIS16448_IMU::~ADIS16448_IMU() { void ADIS16448_IMU::Acquire() { // Set data packet length const int dataset_len = 29; // 18 data points + timestamp - 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; - int bufferAvgIndex = 0; uint32_t previous_timestamp = 0; - double gyro_rate_x = 0.0; - double gyro_rate_y = 0.0; - double gyro_rate_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_rate_x_si = 0.0; - double gyro_rate_y_si = 0.0; - // double gyro_rate_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 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 */ + // Read number of bytes currently stored in the buffer + int data_count = m_spi->ReadAutoReceivedData(buffer, 0, 0_s); + // Check if frame is incomplete + int data_remainder = data_count % dataset_len; + // Remove incomplete data from read count + int data_to_read = data_count - data_remainder; + // 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 + // Read data from DMA buffer + m_spi->ReadAutoReceivedData(buffer, data_to_read, 0_s); // 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]; + // Cycle through XYZ GYRO, XYZ ACCEL, XYZ MAG, BARO, TEMP (Ignore Status + // & CRC) + for (int k = 5; k < 27; k += 2) { + // Process LSB + uint8_t byte = static_cast(buffer[i + k + 1]); + calc_crc = (calc_crc >> 8) ^ m_adiscrc[(calc_crc & 0xFF) ^ byte]; + // Process MSB + byte = static_cast(buffer[i + k]); + calc_crc = (calc_crc >> 8) ^ m_adiscrc[(calc_crc & 0xFF) ^ 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 + // Complement + calc_crc = ~calc_crc; + // Flip LSB & MSB + calc_crc = static_cast((calc_crc << 8) | (calc_crc >> 8)); + // Extract DUT CRC from data buffer + uint16_t imu_crc = BuffToUShort(&buffer[i + 27]); // 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_rate_x = BuffToShort(&buffer[i + 5]) * 0.04; - gyro_rate_y = BuffToShort(&buffer[i + 7]) * 0.04; - gyro_rate_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; + // Scale sensor data + double gyro_rate_x = BuffToShort(&buffer[i + 5]) * 0.04; + double gyro_rate_y = BuffToShort(&buffer[i + 7]) * 0.04; + double gyro_rate_z = BuffToShort(&buffer[i + 9]) * 0.04; + double accel_x = BuffToShort(&buffer[i + 11]) * 0.833; + double accel_y = BuffToShort(&buffer[i + 13]) * 0.833; + double accel_z = BuffToShort(&buffer[i + 15]) * 0.833; + double mag_x = BuffToShort(&buffer[i + 17]) * 0.1429; + double mag_y = BuffToShort(&buffer[i + 19]) * 0.1429; + double mag_z = BuffToShort(&buffer[i + 21]) * 0.1429; + double baro = BuffToShort(&buffer[i + 23]) * 0.02; + double temp = BuffToShort(&buffer[i + 25]) * 0.07386 + 31.0; // Convert scaled sensor data to SI units - gyro_rate_x_si = gyro_rate_x * deg_to_rad; - gyro_rate_y_si = gyro_rate_y * deg_to_rad; - // gyro_rate_z_si = gyro_rate_z * deg_to_rad; - accel_x_si = accel_x * grav; - accel_y_si = accel_y * grav; - accel_z_si = accel_z * grav; + double gyro_rate_x_si = gyro_rate_x * kDegToRad; + double gyro_rate_y_si = gyro_rate_y * kDegToRad; + // double gyro_rate_z_si = gyro_rate_z * kDegToRad; + double accel_x_si = accel_x * kGrav; + double accel_y_si = accel_y * kGrav; + double accel_z_si = accel_z * kGrav; // 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); + m_alpha = kTau / (kTau + m_dt); + // Run inclinometer calculations + double accelAngleX = + atan2f(-accel_x_si, std::hypotf(accel_y_si, -accel_z_si)); + double accelAngleY = + atan2f(accel_y_si, std::hypotf(-accel_x_si, -accel_z_si)); // 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 = @@ -748,7 +659,7 @@ void ADIS16448_IMU::Acquire() { } else { // Accumulate gyro for offset calibration // Add most recent sample data to buffer - bufferAvgIndex = m_accum_count % m_avg_size; + int bufferAvgIndex = m_accum_count % m_avg_size; m_offset_buffer[bufferAvgIndex] = OffsetData{gyro_rate_x, gyro_rate_y, gyro_rate_z}; // Increment counter @@ -768,10 +679,10 @@ void ADIS16448_IMU::Acquire() { 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; + m_compAngleX = compAngleX * kRadToDeg; + m_compAngleY = compAngleY * kRadToDeg; + m_accelAngleX = accelAngleX * kRadToDeg; + m_accelAngleY = accelAngleY * kRadToDeg; // Accumulate gyro for angle integration and publish to global // variables m_integ_gyro_angle_x += @@ -787,31 +698,9 @@ void ADIS16448_IMU::Acquire() { } } else { m_thread_idle = true; - data_count = 0; - data_remainder = 0; - data_to_read = 0; previous_timestamp = 0.0; - gyro_rate_x = 0.0; - gyro_rate_y = 0.0; - gyro_rate_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_rate_x_si = 0.0; - gyro_rate_y_si = 0.0; - // gyro_rate_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; } } } @@ -826,16 +715,6 @@ double ADIS16448_IMU::FormatFastConverge(double compAngle, double accAngle) { return compAngle; } -double ADIS16448_IMU::FormatRange0to2PI(double compAngle) { - while (compAngle >= 2 * std::numbers::pi) { - compAngle = compAngle - 2.0 * std::numbers::pi; - } - while (compAngle < 0.0) { - compAngle = compAngle + 2.0 * std::numbers::pi; - } - return compAngle; -} - double ADIS16448_IMU::FormatAccelRange(double accelAngle, double accelZ) { if (accelZ < 0.0) { accelAngle = std::numbers::pi - accelAngle; @@ -850,43 +729,35 @@ double ADIS16448_IMU::CompFilterProcess(double compAngle, double accelAngle, compAngle = FormatFastConverge(compAngle, accelAngle); compAngle = m_alpha * (compAngle + omega * m_dt) + (1.0 - m_alpha) * accelAngle; - compAngle = FormatRange0to2PI(compAngle); - if (compAngle > std::numbers::pi) { - compAngle = compAngle - 2.0 * std::numbers::pi; - } - return compAngle; + return frc::InputModulus(compAngle, -std::numbers::pi, std::numbers::pi); } int ADIS16448_IMU::ConfigDecRate(uint16_t decimationRate) { // 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. - // - // 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. - uint16_t writeValue = decimationRate; - uint16_t readbackValue; + // the DECIMATE register in the IMU, adjusts the sample scale factor, and + // re-enables auto SPI. if (!SwitchToStandardSPI()) { REPORT_ERROR("Failed to configure/reconfigure standard SPI."); return 2; } - /* Check max */ + // Check max if (decimationRate > 9) { REPORT_ERROR( "Attempted to write an invalid decimation value. Capping at 9"); decimationRate = 9; } - /* Shift decimation setting to correct position and select internal sync */ - writeValue = (decimationRate << 8) | 0x1; + // Shift decimation setting to correct position and select internal sync + uint16_t writeValue = (decimationRate << 8) | 0x1; - /* Apply to IMU */ + // Apply to IMU WriteRegister(SMPL_PRD, writeValue); - /* Perform read back to verify write */ - readbackValue = ReadRegister(SMPL_PRD); + // Perform read back to verify write + uint16_t readbackValue = ReadRegister(SMPL_PRD); - /* Throw error for invalid write */ + // Throw error for invalid write if (readbackValue != writeValue) { REPORT_ERROR("ADIS16448 SMPL_PRD write failed."); } @@ -1059,12 +930,6 @@ int ADIS16448_IMU::GetPort() const { return m_spi_port; } -/** - * @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(wpi::SendableBuilder& builder) { builder.SetSmartDashboardType("ADIS16448 IMU"); builder.AddDoubleProperty( diff --git a/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp index 0f8da84a21..5af75b8a6d 100644 --- a/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp +++ b/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp @@ -27,6 +27,7 @@ #include #include "frc/Errors.h" +#include "frc/MathUtil.h" /* Helpful conversion functions */ static inline int32_t ToInt(const uint32_t* buf) { @@ -38,10 +39,6 @@ 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 { @@ -59,9 +56,6 @@ inline void ADISReportError(int32_t status, const char* file, int line, #define REPORT_ERROR(msg) \ ADISReportError(err::Error, __FILE__, __LINE__, __FUNCTION__, msg) -/** - * Constructor. - */ ADIS16470_IMU::ADIS16470_IMU() : ADIS16470_IMU(kZ, kY, kX, SPI::Port::kOnboardCS0, CalibrationTime::_1s) {} @@ -120,35 +114,37 @@ ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, IMUAxis pitch_axis, // 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 = + DigitalOutput* reset_out = new DigitalOutput(27); // Drive SPI CS2 (IMU RST) low - Wait(10_ms); // Wait 10ms - delete m_reset_out; + Wait(10_ms); + delete reset_out; m_reset_in = new DigitalInput(27); // Set SPI CS2 (IMU RST) high - Wait(500_ms); // Wait 500ms for reset to complete + Wait(500_ms); // Wait for reset to complete + m_spi = new SPI(m_spi_port); + m_spi->SetClockRate(2000000); + m_spi->SetMode(frc::SPI::Mode::kMode3); + m_spi->SetChipSelectActiveLow(); // Configure standard SPI if (!SwitchToStandardSPI()) { return; } - - // Set up flash state variable - bool m_needs_flash = false; - + bool needsFlash = false; // Set IMU internal decimation to 4 (output data rate of 2000 SPS / (4 + 1) // = 400Hz) if (ReadRegister(DEC_RATE) != 0x0004) { WriteRegister(DEC_RATE, 0x0004); - m_needs_flash = true; + needsFlash = true; REPORT_WARNING( "ADIS16470: DEC_RATE register configuration inconsistent! Scheduling " "flash update."); } + // Set data ready polarity (HIGH = Good Data), Disable gSense Compensation // and PoP if (ReadRegister(MSC_CTRL) != 0x0001) { WriteRegister(MSC_CTRL, 0x0001); - m_needs_flash = true; + needsFlash = true; REPORT_WARNING( "ADIS16470: MSC_CTRL register configuration inconsistent! Scheduling " "flash update."); @@ -157,7 +153,7 @@ ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, IMUAxis pitch_axis, // Disable IMU internal Bartlett filter (200Hz bandwidth is sufficient) if (ReadRegister(FILT_CTRL) != 0x0000) { WriteRegister(FILT_CTRL, 0x0000); - m_needs_flash = true; + needsFlash = true; REPORT_WARNING( "ADIS16470: FILT_CTRL register configuration inconsistent! " "Scheduling flash update."); @@ -165,7 +161,7 @@ ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, IMUAxis pitch_axis, // If any registers on the IMU don't match the config, trigger a flash // update - if (m_needs_flash) { + if (needsFlash) { REPORT_WARNING( "ADIS16470: Register configuration changed! Starting IMU flash " "update."); @@ -174,7 +170,6 @@ ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, IMUAxis pitch_axis, // the datasheet) Wait(0.3_s); REPORT_WARNING("ADIS16470: Flash update finished!"); - m_needs_flash = false; } else { REPORT_WARNING( "ADIS16470: Flash and RAM configuration consistent. No flash update " @@ -194,12 +189,13 @@ ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, IMUAxis pitch_axis, // Write offset calibration command to IMU WriteRegister(GLOB_CMD, 0x0001); + m_auto_interrupt = new DigitalInput(26); // Configure and enable auto SPI if (!SwitchToAutoSPI()) { return; } - // Let the user know the IMU was initiallized successfully + // Let the user know the IMU was initialized successfully REPORT_WARNING("ADIS16470 IMU Successfully Initialized!"); // Drive SPI CS3 (IMU ready LED) low (active low) @@ -228,7 +224,6 @@ ADIS16470_IMU::ADIS16470_IMU(ADIS16470_IMU&& other) m_accel_x{std::move(other.m_accel_x)}, m_accel_y{std::move(other.m_accel_y)}, m_accel_z{std::move(other.m_accel_z)}, - m_tau{std::move(other.m_tau)}, m_dt{std::move(other.m_dt)}, m_alpha{std::move(other.m_alpha)}, m_compAngleX{std::move(other.m_compAngleX)}, @@ -278,7 +273,6 @@ ADIS16470_IMU& ADIS16470_IMU::operator=(ADIS16470_IMU&& other) { std::swap(this->m_accel_x, other.m_accel_x); std::swap(this->m_accel_y, other.m_accel_y); std::swap(this->m_accel_z, other.m_accel_z); - std::swap(this->m_tau, other.m_tau); std::swap(this->m_dt, other.m_dt); std::swap(this->m_alpha, other.m_alpha); std::swap(this->m_compAngleX, other.m_compAngleX); @@ -340,54 +334,33 @@ bool ADIS16470_IMU::SwitchToStandardSPI() { Wait(10_ms); } // Maybe we're in auto SPI mode? If so, kill auto SPI, and then SPI. - if (m_spi != nullptr && m_auto_configured) { + if (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. + // 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) - */ + // 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 remaining data count */ + // Get the remaining data count data_count = m_spi->ReadAutoReceivedData(trashBuffer, 0, 0_s); } } } - // There doesn't seem to be a SPI port active. Let's try to set one up - if (m_spi == nullptr) { - m_spi = new SPI(m_spi_port); - m_spi->SetClockRate(2000000); - m_spi->SetMode(frc::SPI::Mode::kMode3); - 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; - } + 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; } /** @@ -407,17 +380,6 @@ bool ADIS16470_IMU::SwitchToStandardSPI() { *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) { @@ -426,12 +388,11 @@ bool ADIS16470_IMU::SwitchToAutoSPI() { } // Do we need to change auto SPI settings? m_spi->SetAutoTransmitData(m_autospi_allangle_packet, 2); - // Configure auto stall time m_spi->ConfigureAutoStall(HAL_SPI_kOnboardCS0, 5, 1000, 1); // Kick off DMA SPI (Note: Device configuration impossible after SPI DMA is - // activated) DR High = Data good (data capture should be triggered on the - // rising edge) + // 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) { @@ -488,10 +449,8 @@ int ADIS16470_IMU::ConfigCalTime(CalibrationTime new_cal_time) { int ADIS16470_IMU::ConfigDecRate(uint16_t decimationRate) { // 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. - // - // 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. + // the DECIMATE register in the IMU, adjusts the sample scale factor, and + // re-enables auto SPI. if (!SwitchToStandardSPI()) { REPORT_ERROR("Failed to configure/reconfigure standard SPI."); return 2; @@ -500,7 +459,7 @@ int ADIS16470_IMU::ConfigDecRate(uint16_t decimationRate) { REPORT_ERROR("Attempted to write an invalid decimation value."); decimationRate = 1999; } - m_scaled_sample_rate = (((decimationRate + 1.0) / 2000.0) * 1000000.0); + m_scaled_sample_rate = (decimationRate + 1.0) / 2000.0 * 1000000.0; WriteRegister(DEC_RATE, decimationRate); if (!SwitchToAutoSPI()) { REPORT_ERROR("Failed to configure/reconfigure auto SPI."); @@ -528,20 +487,6 @@ void ADIS16470_IMU::Calibrate() { } } -/** - * @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; @@ -550,24 +495,24 @@ uint16_t ADIS16470_IMU::ReadRegister(uint8_t reg) { m_spi->Write(buf, 2); m_spi->Read(false, buf, 2); - return ToUShort(buf); + return (static_cast(buf[0]) << 8) | buf[1]; } /** * @brief Writes an unsigned, 16-bit value to two adjacent, 8-bit register - *locations over SPI. + * 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. + * 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. - **/ + * via SPI. The upper and lower bytes that make up the 16-bit value are split + * into two unsigned, 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; @@ -578,12 +523,6 @@ void ADIS16470_IMU::WriteRegister(uint8_t reg, uint16_t val) { 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_x = 0.0; @@ -646,110 +585,79 @@ ADIS16470_IMU::~ADIS16470_IMU() { void ADIS16470_IMU::Acquire() { // Set data packet length const int dataset_len = 27; // 26 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_x = 0.0; - double delta_angle_y = 0.0; - double delta_angle_z = 0.0; - double gyro_rate_x = 0.0; - double gyro_rate_y = 0.0; - double gyro_rate_z = 0.0; - double accel_x = 0.0; - double accel_y = 0.0; - double accel_z = 0.0; - double gyro_rate_x_si = 0.0; - double gyro_rate_y_si = 0.0; - // double gyro_rate_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 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 */ + // Read number of bytes currently stored in the buffer + int data_count = m_spi->ReadAutoReceivedData(buffer, 0, 0_s); + // Check if frame is incomplete + int data_remainder = data_count % dataset_len; + // Remove incomplete data from read count + int data_to_read = data_count - data_remainder; + // 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) + // Read data from DMA buffer (only complete sets) + m_spi->ReadAutoReceivedData(buffer, data_to_read, 0_s); // 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_x = - (ToInt(&buffer[i + 3]) * delta_angle_sf) / - (m_scaled_sample_rate / (buffer[i] - previous_timestamp)); - delta_angle_y = - (ToInt(&buffer[i + 7]) * delta_angle_sf) / - (m_scaled_sample_rate / (buffer[i] - previous_timestamp)); - delta_angle_z = - (ToInt(&buffer[i + 11]) * delta_angle_sf) / - (m_scaled_sample_rate / (buffer[i] - previous_timestamp)); + // Get delta angle value for selected yaw axis and scale by the elapsed + // time (based on timestamp) + double elapsed_time = + m_scaled_sample_rate / (buffer[i] - previous_timestamp); + double delta_angle_x = + ToInt(&buffer[i + 3]) * delta_angle_sf / elapsed_time; + double delta_angle_y = + ToInt(&buffer[i + 7]) * delta_angle_sf / elapsed_time; + double delta_angle_z = + ToInt(&buffer[i + 11]) * delta_angle_sf / elapsed_time; - gyro_rate_x = (BuffToShort(&buffer[i + 15]) / 10.0); - gyro_rate_y = (BuffToShort(&buffer[i + 17]) / 10.0); - gyro_rate_z = (BuffToShort(&buffer[i + 19]) / 10.0); - accel_x = (BuffToShort(&buffer[i + 21]) / 800.0); - accel_y = (BuffToShort(&buffer[i + 23]) / 800.0); - accel_z = (BuffToShort(&buffer[i + 25]) / 800.0); + double gyro_rate_x = BuffToShort(&buffer[i + 15]) / 10.0; + double gyro_rate_y = BuffToShort(&buffer[i + 17]) / 10.0; + double gyro_rate_z = BuffToShort(&buffer[i + 19]) / 10.0; + double accel_x = BuffToShort(&buffer[i + 21]) / 800.0; + double accel_y = BuffToShort(&buffer[i + 23]) / 800.0; + double accel_z = BuffToShort(&buffer[i + 25]) / 800.0; // Convert scaled sensor data to SI units - gyro_rate_x_si = gyro_rate_x * deg_to_rad; - gyro_rate_y_si = gyro_rate_y * deg_to_rad; - // gyro_rate_z_si = gyro_rate_z * deg_to_rad; - accel_x_si = accel_x * grav; - accel_y_si = accel_y * grav; - accel_z_si = accel_z * grav; + double gyro_rate_x_si = gyro_rate_x * kDegToRad; + double gyro_rate_y_si = gyro_rate_y * kDegToRad; + // double gyro_rate_z_si = gyro_rate_z * kDegToRad; + double accel_x_si = accel_x * kGrav; + double accel_y_si = accel_y * kGrav; + double accel_z_si = accel_z * kGrav; // Store timestamp for next iteration previous_timestamp = buffer[i]; - m_alpha = m_tau / (m_tau + m_dt); + m_alpha = kTau / (kTau + m_dt); + // Run inclinometer calculations + double accelAngleX = + atan2f(accel_x_si, std::hypotf(accel_y_si, accel_z_si)); + double accelAngleY = + atan2f(accel_y_si, std::hypotf(accel_x_si, accel_z_si)); 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 = @@ -760,10 +668,10 @@ void ADIS16470_IMU::Acquire() { { std::scoped_lock sync(m_mutex); - /* Push data to global variables */ + // 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 */ + // Don't accumulate first run. previous_timestamp will be "very" old + // and the integration will end up way off m_integ_angle_x = 0.0; m_integ_angle_y = 0.0; m_integ_angle_z = 0.0; @@ -778,38 +686,18 @@ void ADIS16470_IMU::Acquire() { 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_compAngleX = compAngleX * kRadToDeg; + m_compAngleY = compAngleY * kRadToDeg; + m_accelAngleX = accelAngleX * kRadToDeg; + m_accelAngleY = accelAngleY * kRadToDeg; } 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_x = 0.0; - delta_angle_y = 0.0; - delta_angle_z = 0.0; - gyro_rate_x = 0.0; - gyro_rate_y = 0.0; - gyro_rate_z = 0.0; - accel_x = 0.0; - accel_y = 0.0; - accel_z = 0.0; - gyro_rate_x_si = 0.0; - gyro_rate_y_si = 0.0; - // gyro_rate_z_si = 0.0; - accel_x_si = 0.0; - accel_y_si = 0.0; - accel_z_si = 0.0; + previous_timestamp = 0; compAngleX = 0.0; compAngleY = 0.0; - accelAngleX = 0.0; - accelAngleY = 0.0; } } } @@ -824,16 +712,6 @@ double ADIS16470_IMU::FormatFastConverge(double compAngle, double accAngle) { return compAngle; } -double ADIS16470_IMU::FormatRange0to2PI(double compAngle) { - while (compAngle >= 2 * std::numbers::pi) { - compAngle = compAngle - 2.0 * std::numbers::pi; - } - while (compAngle < 0.0) { - compAngle = compAngle + 2.0 * std::numbers::pi; - } - return compAngle; -} - double ADIS16470_IMU::FormatAccelRange(double accelAngle, double accelZ) { if (accelZ < 0.0) { accelAngle = std::numbers::pi - accelAngle; @@ -848,11 +726,7 @@ double ADIS16470_IMU::CompFilterProcess(double compAngle, double accelAngle, compAngle = FormatFastConverge(compAngle, accelAngle); compAngle = m_alpha * (compAngle + omega * m_dt) + (1.0 - m_alpha) * accelAngle; - compAngle = FormatRange0to2PI(compAngle); - if (compAngle > std::numbers::pi) { - compAngle = compAngle - 2.0 * std::numbers::pi; - } - return compAngle; + return frc::InputModulus(compAngle, -std::numbers::pi, std::numbers::pi); } void ADIS16470_IMU::SetGyroAngle(IMUAxis axis, units::degree_t angle) { @@ -1054,12 +928,6 @@ int ADIS16470_IMU::GetPort() const { return m_spi_port; } -/** - * @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(wpi::SendableBuilder& builder) { builder.SetSmartDashboardType("ADIS16470 IMU"); builder.AddDoubleProperty( diff --git a/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h b/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h index 0b16782b8a..71d52b8160 100644 --- a/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h +++ b/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h @@ -281,7 +281,7 @@ class ADIS16448_IMU : public wpi::Sendable, void InitSendable(wpi::SendableBuilder& builder) override; private: - /** @brief ADIS16448 Register Map Declaration */ + // 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 @@ -338,9 +338,9 @@ class ADIS16448_IMU : public wpi::Sendable, static constexpr uint8_t SERIAL_NUM = 0x58; // Lot-specific serial number /** @brief ADIS16448 Static Constants */ - static constexpr double rad_to_deg = 57.2957795; - static constexpr double deg_to_rad = 0.0174532; - static constexpr double grav = 9.81; + static constexpr double kRadToDeg = 57.2957795; + static constexpr double kDegToRad = 0.0174532; + static constexpr double kGrav = 9.81; /** @brief struct to store offset data */ struct OffsetData { @@ -382,8 +382,8 @@ class ADIS16448_IMU : public wpi::Sendable, double m_temp = 0.0; // Complementary filter variables - double m_tau = 0.5; double m_dt, m_alpha = 0.0; + static constexpr double kTau = 0.5; double m_compAngleX, m_compAngleY, m_accelAngleX, m_accelAngleY = 0.0; // vector for storing most recent imu values @@ -408,8 +408,6 @@ class ADIS16448_IMU : public wpi::Sendable, // 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); @@ -463,7 +461,7 @@ class ADIS16448_IMU : public wpi::Sendable, mutable NonMovableMutexWrapper m_mutex; // CRC-16 Look-Up Table - static constexpr uint16_t adiscrc[256] = { + static constexpr uint16_t m_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, diff --git a/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h b/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h index f4f4145e17..a55800c416 100644 --- a/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h +++ b/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h @@ -162,8 +162,10 @@ class ADIS16470_IMU : public wpi::Sendable, 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. + * Configures calibration time. + * + * @param new_cal_time New calibration time + * @return 0 if success, 1 if no change, 2 if error. */ int ConfigCalTime(CalibrationTime new_cal_time); @@ -418,11 +420,11 @@ class ADIS16470_IMU : public wpi::Sendable, Y_ACCL_OUT, FLASH_CNT, Z_ACCL_OUT, FLASH_CNT}; static constexpr double delta_angle_sf = 2160.0 / 2147483648.0; - static constexpr double rad_to_deg = 57.2957795; - static constexpr double deg_to_rad = 0.0174532; - static constexpr double grav = 9.81; + static constexpr double kRadToDeg = 57.2957795; + static constexpr double kDegToRad = 0.0174532; + static constexpr double kGrav = 9.81; - /** @brief Resources **/ + // Resources DigitalInput* m_reset_in = nullptr; DigitalOutput* m_status_led = nullptr; @@ -487,15 +489,13 @@ class ADIS16470_IMU : public wpi::Sendable, double m_accel_z = 0.0; // Complementary filter variables - double m_tau = 1.0; double m_dt, m_alpha = 0.0; + static constexpr double kTau = 1.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); 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 index 1d79d0d669..8843a9b1e3 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java @@ -18,40 +18,20 @@ import edu.wpi.first.hal.HAL; import edu.wpi.first.hal.SimBoolean; import edu.wpi.first.hal.SimDevice; import edu.wpi.first.hal.SimDouble; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.util.sendable.SendableBuilder; // 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" -}) +@SuppressWarnings({"PMD.RedundantFieldInitializer", "PMD.ImmutableField"}) public class ADIS16448_IMU implements AutoCloseable, Sendable { - /** ADIS16448 Register Map Declaration */ + // 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 @@ -96,29 +76,29 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable { /** ADIS16448 calibration times. */ public enum CalibrationTime { - /** 32 ms calibration time */ + /** 32 ms calibration time. */ _32ms(0), - /** 64 ms calibration time */ + /** 64 ms calibration time. */ _64ms(1), - /** 128 ms calibration time */ + /** 128 ms calibration time. */ _128ms(2), - /** 256 ms calibration time */ + /** 256 ms calibration time. */ _256ms(3), - /** 512 ms calibration time */ + /** 512 ms calibration time. */ _512ms(4), - /** 1 s calibration time */ + /** 1 s calibration time. */ _1s(5), - /** 2 s calibration time */ + /** 2 s calibration time. */ _2s(6), - /** 4 s calibration time */ + /** 4 s calibration time. */ _4s(7), - /** 8 s calibration time */ + /** 8 s calibration time. */ _8s(8), - /** 16 s calibration time */ + /** 16 s calibration time. */ _16s(9), - /** 32 s calibration time */ + /** 32 s calibration time. */ _32s(10), - /** 64 s calibration time */ + /** 64 s calibration time. */ _64s(11); private final int value; @@ -138,20 +118,17 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable { 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; + private static final double kGrav = 9.81; - /* User-specified yaw axis */ + // User-specified yaw axis private IMUAxis m_yaw_axis; - /* Offset data storage */ + // Offset data storage private double[] m_offset_data_gyro_rate_x; private double[] m_offset_data_gyro_rate_y; private double[] m_offset_data_gyro_rate_z; - /* Instant raw output variables */ + // Instant raw output variables private double m_gyro_rate_x = 0.0; private double m_gyro_rate_y = 0.0; private double m_gyro_rate_z = 0.0; @@ -164,41 +141,39 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable { private double m_baro = 0.0; private double m_temp = 0.0; - /* IMU gyro offset variables */ + // IMU gyro offset variables private double m_gyro_rate_offset_x = 0.0; private double m_gyro_rate_offset_y = 0.0; private double m_gyro_rate_offset_z = 0.0; private int m_avg_size = 0; private int m_accum_count = 0; - /* Integrated gyro angle variables */ + // Integrated gyro angle variables private double m_integ_gyro_angle_x = 0.0; private double m_integ_gyro_angle_y = 0.0; private double m_integ_gyro_angle_z = 0.0; - /* Complementary filter variables */ + // Complementary filter variables private double m_dt = 0.0; private double m_alpha = 0.0; - private double m_tau = 1.0; + private static final double kTau = 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 */ + // State variables private volatile boolean m_thread_active = false; private CalibrationTime m_calibration_time = CalibrationTime._32ms; 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; - private boolean m_needs_flash = false; - /* Resources */ + // 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; @@ -216,55 +191,41 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable { private SimDouble m_simAccelY; private SimDouble m_simAccelZ; - /* 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(); - } - } + // CRC-16 Look-Up Table + private int[] m_adiscrc = { + 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 + }; /** Creates a new ADIS16448_IMU object. */ public ADIS16448_IMU() { @@ -278,12 +239,11 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable { * @param port The SPI Port the gyro is plugged into * @param cal_time Calibration time */ - @SuppressWarnings("this-escape") public ADIS16448_IMU(final IMUAxis yaw_axis, SPI.Port port, CalibrationTime cal_time) { m_yaw_axis = yaw_axis; m_spi_port = port; - m_acquire_task = new Thread(new AcquireTask(this)); + m_acquire_task = new Thread(this::acquire); m_simDevice = SimDevice.create("Gyro:ADIS16448", port.value); if (m_simDevice != null) { @@ -301,22 +261,26 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable { if (m_simDevice == null) { // 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(); + var reset_out = new DigitalOutput(18); // Drive MXP DIO8 low + Timer.delay(0.01); + reset_out.close(); m_reset_in = new DigitalInput(18); // Set MXP DIO8 high - Timer.delay(0.25); // Wait 250ms + Timer.delay(0.25); // Wait for reset to complete configCalTime(cal_time); + m_spi = new SPI(m_spi_port); + m_spi.setClockRate(1000000); + m_spi.setMode(SPI.Mode.kMode3); + m_spi.setChipSelectActiveLow(); if (!switchToStandardSPI()) { return; } - + boolean needsFlash = false; // Set IMU internal decimation to 1 (ODR = 819.2 SPS / (1 + 1) = 409.6Hz), BW = 204.8Hz if (readRegister(SMPL_PRD) != 0x0001) { writeRegister(SMPL_PRD, 0x0001); - m_needs_flash = true; + needsFlash = true; DriverStation.reportWarning( "ADIS16448: SMPL_PRD register configuration inconsistent! Scheduling flash update.", false); @@ -325,7 +289,7 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable { // Set data ready polarity (LOW = Good Data) on DIO1 (PWM0 on MXP) if (readRegister(MSC_CTRL) != 0x0016) { writeRegister(MSC_CTRL, 0x0016); - m_needs_flash = true; + needsFlash = true; DriverStation.reportWarning( "ADIS16448: MSC_CTRL register configuration inconsistent! Scheduling flash update.", false); @@ -334,7 +298,7 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable { // Disable IMU internal Bartlett filter (204Hz BW is sufficient) and set IMU scale factor if (readRegister(SENS_AVG) != 0x0400) { writeRegister(SENS_AVG, 0x0400); - m_needs_flash = true; + needsFlash = true; DriverStation.reportWarning( "ADIS16448: SENS_AVG register configuration inconsistent! Scheduling flash update.", false); @@ -342,7 +306,7 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable { // Clear offset registers if (readRegister(XGYRO_OFF) != 0x0000) { writeRegister(XGYRO_OFF, 0x0000); - m_needs_flash = true; + needsFlash = true; DriverStation.reportWarning( "ADIS16448: XGYRO_OFF register configuration inconsistent! Scheduling flash update.", false); @@ -350,7 +314,7 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable { if (readRegister(YGYRO_OFF) != 0x0000) { writeRegister(YGYRO_OFF, 0x0000); - m_needs_flash = true; + needsFlash = true; DriverStation.reportWarning( "ADIS16448: YGYRO_OFF register configuration inconsistent! Scheduling flash update.", false); @@ -358,26 +322,27 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable { if (readRegister(ZGYRO_OFF) != 0x0000) { writeRegister(ZGYRO_OFF, 0x0000); - m_needs_flash = true; + needsFlash = true; DriverStation.reportWarning( "ADIS16448: ZGYRO_OFF register configuration inconsistent! Scheduling flash update.", false); } // If any registers on the IMU don't match the config, trigger a flash update - if (m_needs_flash) { + if (needsFlash) { DriverStation.reportWarning( "ADIS16448: Register configuration changed! Starting IMU flash update.", false); writeRegister(GLOB_CMD, 0x0008); // Wait long enough for the flash update to finish (75ms minimum as per the datasheet) Timer.delay(0.5); DriverStation.reportWarning("ADIS16448: Flash update finished!", false); - m_needs_flash = false; } else { DriverStation.reportWarning( - "ADIS16448: and RAM configuration consistent. No flash update required!", false); + "ADIS16448: Flash and RAM configuration consistent. No flash update required!", false); } + // Set up the interrupt + m_auto_interrupt = new DigitalInput(10); // MXP DIO0 // Configure standard SPI if (!switchToAutoSPI()) { return; @@ -389,13 +354,11 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable { Thread.sleep((long) (m_calibration_time.value * 1.2 * 1000)); } catch (InterruptedException e) { } - // Execute calibration routine + // Execute calibration routine and reset accumulated offsets calibrate(); - // Reset accumulated offsets - reset(); // Indicate to the acquire loop that we're done starting up m_start_up_mode = false; - // Let the user know the IMU was initiallized successfully + // Let the user know the IMU was initialized successfully DriverStation.reportWarning("ADIS16448 IMU Successfully Initialized!", false); // Drive MXP PWM5 (IMU ready LED) low (active low) m_status_led = new DigitalOutput(19); @@ -418,34 +381,14 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable { return m_connected; } - private static int toUShort(byte[] buf) { - return (((buf[0] & 0xFF) << 8) + ((buf[1] & 0xFF) << 0)); + private static int toUShort(int upper, int lower) { + return ((upper & 0xFF) << 8) + (lower & 0xFF); } - private static int toUByte(int... buf) { - return (buf[0] & 0xFF); + private static int toShort(int upper, int lower) { + return (short) (((upper & 0xFF) << 8) + (lower & 0xFF)); } - private 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)); - } - - /** */ - private static int toInt(int... buf) { - return (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. @@ -459,13 +402,12 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable { } 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) { + if (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. + // couple of times to be sure we got it all. Yuck. int[] trashBuffer = new int[200]; try { Thread.sleep(100); @@ -473,57 +415,25 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable { } 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 */ + // 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 */ + // 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.setMode(SPI.Mode.kMode3); - 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; - } + 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; } - /** */ 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) { @@ -537,7 +447,6 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable { // Kick off auto SPI (Note: Device configuration 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; @@ -566,19 +475,14 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable { * @return 0 if success, 1 if no change, 2 if error. */ public int configDecRate(int decimationRate) { - // 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. - // - // 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 writeValue; - int readbackValue; + // Switches the active SPI port to standard SPI mode, writes a new value to the DECIMATE + // register in the IMU, adjusts the sample scale factor, and re-enables auto SPI. if (!switchToStandardSPI()) { DriverStation.reportError("Failed to configure/reconfigure standard SPI.", false); return 2; } - /* Check max */ + // Check max if (decimationRate > 9) { DriverStation.reportError( "Attempted to write an invalid decimation value. Capping at 9", false); @@ -590,16 +494,16 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable { decimationRate = 0; } - /* Shift decimation setting to correct position and select internal sync */ - writeValue = (decimationRate << 8) | 0x1; + // Shift decimation setting to correct position and select internal sync + int writeValue = (decimationRate << 8) | 0x1; - /* Apply to IMU */ + // Apply to IMU writeRegister(SMPL_PRD, writeValue); - /* Perform read back to verify write */ - readbackValue = readRegister(SMPL_PRD); + // Perform read back to verify write + int readbackValue = readRegister(SMPL_PRD); - /* Throw error for invalid write */ + // Throw error for invalid write if (readbackValue != writeValue) { DriverStation.reportError("ADIS16448 SMPL_PRD write failed.", false); } @@ -612,7 +516,7 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable { } /** - * Configures calibration time + * 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 @@ -664,17 +568,12 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable { m_gyro_rate_offset_x = accum_gyro_rate_x / gyroAverageSize; m_gyro_rate_offset_y = accum_gyro_rate_y / gyroAverageSize; m_gyro_rate_offset_z = accum_gyro_rate_z / gyroAverageSize; - m_integ_gyro_angle_x = 0.0; - m_integ_gyro_angle_y = 0.0; - m_integ_gyro_angle_z = 0.0; - // System.out.println("Avg Size: " + gyroAverageSize + "X Off: " + - // m_gyro_rate_offset_x + "Y Off: " + m_gyro_rate_offset_y + "Z Off: " + - // m_gyro_rate_offset_z); + reset(); } } /** - * Sets the yaw axis + * 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. @@ -689,16 +588,12 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable { } 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; + final byte[] buf = {(byte) (reg & 0x7f), 0}; m_spi.write(buf, 2); m_spi.read(false, buf, 2); - return toUShort(buf); + return toUShort(buf[0], buf[1]); } private void writeRegister(final int reg, final int val) { @@ -759,7 +654,6 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable { 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 @@ -767,36 +661,11 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable { // 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_rate_x = 0.0; - double gyro_rate_y = 0.0; - double gyro_rate_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_rate_x_si = 0.0; - double gyro_rate_y_si = 0.0; - double gyro_rate_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 + // Wait for data try { Thread.sleep(5); } catch (InterruptedException e) { @@ -805,91 +674,75 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable { 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 + // Read number of bytes currently stored in the buffer + int data_count = m_spi.readAutoReceivedData(buffer, 0, 0); + // Check if frame is incomplete + int data_remainder = data_count % dataset_len; + // Remove incomplete data from read count + int data_to_read = data_count - data_remainder; 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) + // Read data from DMA buffer (only complete sets) + m_spi.readAutoReceivedData(buffer, data_to_read, 0); // 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]; + int calc_crc = 0xFFFF; // Starting word + // Cycle through XYZ GYRO, XYZ ACCEL, XYZ MAG, BARO, TEMP (Ignore Status & CRC) + for (int k = 5; k < 27; k += 2) { + // Process LSB + calc_crc = (calc_crc >>> 8) ^ m_adiscrc[(calc_crc & 0xFF) ^ buffer[i + k + 1]]; + // Process MSB + calc_crc = (calc_crc >>> 8) ^ m_adiscrc[(calc_crc & 0xFF) ^ buffer[i + k]]; } - 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 + // Complement + calc_crc = ~calc_crc & 0xFFFF; + // Flip LSB & MSB + calc_crc = ((calc_crc << 8) | (calc_crc >> 8)) & 0xFFFF; + // Extract DUT CRC from data buffer + int imu_crc = toUShort(buffer[i + 27], buffer[i + 28]); if (calc_crc == imu_crc) { // Timestamp is at buffer[i] m_dt = (buffer[i] - previous_timestamp) / 1000000.0; // Scale sensor data - gyro_rate_x = (toShort(buffer[i + 5], buffer[i + 6]) * 0.04); - gyro_rate_y = (toShort(buffer[i + 7], buffer[i + 8]) * 0.04); - gyro_rate_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); + double gyro_rate_x = toShort(buffer[i + 5], buffer[i + 6]) * 0.04; + double gyro_rate_y = toShort(buffer[i + 7], buffer[i + 8]) * 0.04; + double gyro_rate_z = toShort(buffer[i + 9], buffer[i + 10]) * 0.04; + double accel_x = toShort(buffer[i + 11], buffer[i + 12]) * 0.833; + double accel_y = toShort(buffer[i + 13], buffer[i + 14]) * 0.833; + double accel_z = toShort(buffer[i + 15], buffer[i + 16]) * 0.833; + double mag_x = toShort(buffer[i + 17], buffer[i + 18]) * 0.1429; + double mag_y = toShort(buffer[i + 19], buffer[i + 20]) * 0.1429; + double mag_z = toShort(buffer[i + 21], buffer[i + 22]) * 0.1429; + double baro = toShort(buffer[i + 23], buffer[i + 24]) * 0.02; + double 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_rate_x_si = gyro_rate_x * deg_to_rad; - gyro_rate_y_si = gyro_rate_y * deg_to_rad; - gyro_rate_z_si = gyro_rate_z * deg_to_rad; - accel_x_si = accel_x * grav; - accel_y_si = accel_y * grav; - accel_z_si = accel_z * grav; + double gyro_rate_x_si = Math.toRadians(gyro_rate_x); + double gyro_rate_y_si = Math.toRadians(gyro_rate_y); + // double gyro_rate_z_si = Math.toRadians(gyro_rate_z); + double accel_x_si = accel_x * kGrav; + double accel_y_si = accel_y * kGrav; + double accel_z_si = accel_z * kGrav; // 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); + m_alpha = kTau / (kTau + m_dt); + // Run inclinometer calculations + double accelAngleX = Math.atan2(-accel_x_si, Math.hypot(accel_y_si, -accel_z_si)); + double accelAngleY = Math.atan2(accel_y_si, Math.hypot(-accel_x_si, -accel_z_si)); // 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_rate_y_si); @@ -900,13 +753,11 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable { synchronized (this) { // Ignore first, integrated sample if (m_first_run) { - m_integ_gyro_angle_x = 0.0; - m_integ_gyro_angle_y = 0.0; - m_integ_gyro_angle_z = 0.0; + reset(); } else { // Accumulate gyro for offset calibration // Add to buffer - bufferAvgIndex = m_accum_count % m_avg_size; + int bufferAvgIndex = m_accum_count % m_avg_size; m_offset_data_gyro_rate_x[bufferAvgIndex] = gyro_rate_x; m_offset_data_gyro_rate_y[bufferAvgIndex] = gyro_rate_y; m_offset_data_gyro_rate_z[bufferAvgIndex] = gyro_rate_z; @@ -925,79 +776,28 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable { 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; + m_compAngleX = Math.toDegrees(compAngleX); + m_compAngleY = Math.toDegrees(compAngleY); + m_accelAngleX = Math.toDegrees(accelAngleX); + m_accelAngleY = Math.toDegrees(accelAngleY); // Accumulate gyro for angle integration and publish to global variables m_integ_gyro_angle_x += (gyro_rate_x - m_gyro_rate_offset_x) * m_dt; m_integ_gyro_angle_y += (gyro_rate_y - m_gyro_rate_offset_y) * m_dt; m_integ_gyro_angle_z += (gyro_rate_z - m_gyro_rate_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_rate_x = 0.0; - gyro_rate_y = 0.0; - gyro_rate_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_rate_x_si = 0.0; - gyro_rate_y_si = 0.0; - gyro_rate_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; @@ -1007,25 +807,6 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable { 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; @@ -1035,20 +816,10 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable { 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; + return MathUtil.angleModulus(compAngle); } /** @@ -1167,7 +938,7 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable { if (m_simAccelX != null) { return m_simAccelX.get(); } - return m_accel_x * 9.81; + return m_accel_x * kGrav; } /** @@ -1179,7 +950,7 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable { if (m_simAccelY != null) { return m_simAccelY.get(); } - return m_accel_y * 9.81; + return m_accel_y * kGrav; } /** @@ -1191,7 +962,7 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable { if (m_simAccelZ != null) { return m_simAccelZ.get(); } - return m_accel_z * 9.81; + return m_accel_z * kGrav; } /** 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 index d320167c4f..e158cb1369 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java @@ -2,55 +2,25 @@ // 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.hal.SimBoolean; import edu.wpi.first.hal.SimDevice; import edu.wpi.first.hal.SimDouble; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.util.sendable.SendableBuilder; -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" -}) +@SuppressWarnings("PMD.RedundantFieldInitializer") public class ADIS16470_IMU implements AutoCloseable, Sendable { /* ADIS16470 Register Map Declaration */ private static final int FLASH_CNT = 0x00; // Flash memory write count @@ -157,29 +127,29 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable { /** ADIS16470 calibration times. */ public enum CalibrationTime { - /** 32 ms calibration time */ + /** 32 ms calibration time. */ _32ms(0), - /** 64 ms calibration time */ + /** 64 ms calibration time. */ _64ms(1), - /** 128 ms calibration time */ + /** 128 ms calibration time. */ _128ms(2), - /** 256 ms calibration time */ + /** 256 ms calibration time. */ _256ms(3), - /** 512 ms calibration time */ + /** 512 ms calibration time. */ _512ms(4), - /** 1 s calibration time */ + /** 1 s calibration time. */ _1s(5), - /** 2 s calibration time */ + /** 2 s calibration time. */ _2s(6), - /** 4 s calibration time */ + /** 4 s calibration time. */ _4s(7), - /** 8 s calibration time */ + /** 8 s calibration time. */ _8s(8), - /** 16 s calibration time */ + /** 16 s calibration time. */ _16s(9), - /** 32 s calibration time */ + /** 32 s calibration time. */ _32s(10), - /** 64 s calibration time */ + /** 64 s calibration time. */ _64s(11); private final int value; @@ -196,25 +166,23 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable { * configured by the user to refer to an X, Y, or Z axis. */ public enum IMUAxis { - /** The IMU's X axis */ + /** The IMU's X axis. */ kX, - /** The IMU's Y axis */ + /** The IMU's Y axis. */ kY, - /** The IMU's Z axis */ + /** The IMU's Z axis. */ kZ, - /** The user-configured yaw axis */ + /** The user-configured yaw axis. */ kYaw, - /** The user-configured pitch axis */ + /** The user-configured pitch axis. */ kPitch, - /** The user-configured roll axis */ + /** The user-configured roll axis. */ kRoll, } // 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; + private static final double kDeltaAngleSf = 2160.0 / 2147483648.0; // 2160 / (2^31) + private static final double kGrav = 9.81; // User-specified axes private IMUAxis m_yaw_axis; @@ -225,9 +193,6 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable { private double m_gyro_rate_x = 0.0; private double m_gyro_rate_y = 0.0; private double m_gyro_rate_z = 0.0; - private double m_average_gyro_rate_x = 0.0; - private double m_average_gyro_rate_y = 0.0; - private double m_average_gyro_rate_z = 0.0; private double m_accel_x = 0.0; private double m_accel_y = 0.0; private double m_accel_z = 0.0; @@ -240,7 +205,7 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable { // Complementary filter variables private double m_dt = 0.0; private double m_alpha = 0.0; - private double m_tau = 1.0; + private static final double kTau = 1.0; private double m_compAngleX = 0.0; private double m_compAngleY = 0.0; private double m_accelAngleX = 0.0; @@ -253,13 +218,11 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable { private volatile boolean m_thread_idle = false; private boolean m_auto_configured = false; private double m_scaled_sample_rate = 2500.0; - private boolean m_needs_flash = false; // 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; @@ -277,19 +240,6 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable { private SimDouble m_simAccelY; private SimDouble m_simAccelZ; - private static class AcquireTask implements Runnable { - private ADIS16470_IMU imu; - - public AcquireTask(ADIS16470_IMU imu) { - this.imu = imu; - } - - @Override - public void run() { - imu.acquire(); - } - } - /** * Creates a new ADIS16740 IMU object. * @@ -328,7 +278,6 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable { * @param port The SPI Port the gyro is plugged into * @param cal_time Calibration time */ - @SuppressWarnings("this-escape") public ADIS16470_IMU( IMUAxis yaw_axis, IMUAxis pitch_axis, @@ -361,7 +310,7 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable { m_calibration_time = cal_time.value; m_spi_port = port; - m_acquire_task = new Thread(new AcquireTask(this)); + m_acquire_task = new Thread(this::acquire); m_simDevice = SimDevice.create("Gyro:ADIS16470", port.value); if (m_simDevice != null) { @@ -382,20 +331,24 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable { // 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(); + var reset_out = new DigitalOutput(27); // Drive SPI CS2 (IMU RST) low + Timer.delay(0.01); + 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 + Timer.delay(0.25); // Wait for reset to complete + m_spi = new SPI(m_spi_port); + m_spi.setClockRate(2000000); + m_spi.setMode(SPI.Mode.kMode3); + m_spi.setChipSelectActiveLow(); if (!switchToStandardSPI()) { return; } - + boolean needsFlash = false; // Set IMU internal decimation to 4 (ODR = 2000 SPS / (4 + 1) = 400Hz), BW = 200Hz if (readRegister(DEC_RATE) != 0x0004) { writeRegister(DEC_RATE, 0x0004); - m_needs_flash = true; + needsFlash = true; DriverStation.reportWarning( "ADIS16470: DEC_RATE register configuration inconsistent! Scheduling flash update.", false); @@ -404,7 +357,7 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable { // Set data ready polarity (HIGH = Good Data), Disable gSense Compensation and PoP if (readRegister(MSC_CTRL) != 0x0001) { writeRegister(MSC_CTRL, 0x0001); - m_needs_flash = true; + needsFlash = true; DriverStation.reportWarning( "ADIS16470: MSC_CTRL register configuration inconsistent! Scheduling flash update.", false); @@ -413,28 +366,27 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable { // Disable IMU internal Bartlett filter (200Hz bandwidth is sufficient) if (readRegister(FILT_CTRL) != 0x0000) { writeRegister(FILT_CTRL, 0x0000); - m_needs_flash = true; + needsFlash = true; DriverStation.reportWarning( "ADIS16470: FILT_CTRL register configuration inconsistent! Scheduling flash update.", false); } // If any registers on the IMU don't match the config, trigger a flash update - if (m_needs_flash) { + if (needsFlash) { DriverStation.reportWarning( "ADIS16470: Register configuration changed! Starting IMU flash update.", false); writeRegister(GLOB_CMD, 0x0008); // Wait long enough for the flash update to finish (72ms minimum as per the datasheet) Timer.delay(0.3); DriverStation.reportWarning("ADIS16470: Flash update finished!", false); - m_needs_flash = false; } else { DriverStation.reportWarning( "ADIS16470: Flash and RAM configuration consistent. No flash update required!", false); } // Configure continuous bias calibration time based on user setting - writeRegister(NULL_CNFG, (m_calibration_time | 0x0700)); + writeRegister(NULL_CNFG, m_calibration_time | 0x0700); // Notify DS that IMU calibration delay is active DriverStation.reportWarning("ADIS16470: Starting initial calibration delay.", false); @@ -449,12 +401,14 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable { // Write offset calibration command to IMU writeRegister(GLOB_CMD, 0x0001); + // Configure interrupt on SPI CS1 + m_auto_interrupt = new DigitalInput(26); // Configure and enable auto SPI if (!switchToAutoSPI()) { return; } - // Let the user know the IMU was initiallized successfully + // Let the user know the IMU was initialized successfully DriverStation.reportWarning("ADIS16470 IMU Successfully Initialized!", false); // Drive "Ready" LED low @@ -478,36 +432,12 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable { return m_connected; } - /** - * @param buf - * @return - */ - private static int toUShort(ByteBuffer buf) { - return buf.getShort(0) & 0xFFFF; + private static int toShort(int upper, int lower) { + return (short) (((upper & 0xFF) << 8) + (lower & 0xFF)); } - /** - * @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)); - } - - /** - * @param buf - * @return - */ - private static int toInt(int... buf) { - return (buf[0] & 0xFF) << 24 | (buf[1] & 0xFF) << 16 | (buf[2] & 0xFF) << 8 | (buf[3] & 0xFF); + private static int toInt(int msb, int byte2, int byte3, int lsb) { + return (msb & 0xFF) << 24 | (byte2 & 0xFF) << 16 | (byte3 & 0xFF) << 8 | (lsb & 0xFF); } /** @@ -528,13 +458,12 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable { } 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) { + if (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. + // couple of times to be sure we got it all. Yuck. int[] trashBuffer = new int[200]; try { Thread.sleep(100); @@ -548,34 +477,14 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable { 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.setMode(SPI.Mode.kMode3); - 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; - } + readRegister(PROD_ID); // Dummy read + // Validate the product ID + if (readRegister(PROD_ID) != 16982) { + DriverStation.reportError("Could not find an ADIS16470", false); + close(); + return false; } + return true; } /** @@ -584,18 +493,6 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable { * @return True if successful, false otherwise. */ 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) { @@ -610,7 +507,6 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable { // 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; @@ -633,10 +529,10 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable { } /** - * Configures calibration time + * 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 + * @return 0 if success, 1 if no change, 2 if error. */ public int configCalTime(CalibrationTime new_cal_time) { if (m_calibration_time == new_cal_time.value) { @@ -647,7 +543,7 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable { return 2; } m_calibration_time = new_cal_time.value; - writeRegister(NULL_CNFG, (m_calibration_time | 0x700)); + writeRegister(NULL_CNFG, m_calibration_time | 0x700); if (!switchToAutoSPI()) { DriverStation.reportError("Failed to configure/reconfigure auto SPI.", false); return 2; @@ -662,11 +558,8 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable { * @return 0 if success, 1 if no change, 2 if error. */ public int configDecRate(int decimationRate) { - // 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. - // - // 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. + // Switches the active SPI port to standard SPI mode, writes a new value to the DECIMATE + // register in the IMU, adjusts the sample scale factor, and re-enables auto SPI. if (!switchToStandardSPI()) { DriverStation.reportError("Failed to configure/reconfigure standard SPI.", false); return 2; @@ -675,7 +568,7 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable { DriverStation.reportError("Attempted to write an invalid decimation value.", false); decimationRate = 1999; } - m_scaled_sample_rate = (((decimationRate + 1.0) / 2000.0) * 1000000.0); + m_scaled_sample_rate = (decimationRate + 1.0) / 2000.0 * 1000000.0; writeRegister(DEC_RATE, decimationRate); System.out.println("Decimation register: " + readRegister(DEC_RATE)); if (!switchToAutoSPI()) { @@ -700,35 +593,22 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable { } } - /** - * @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); + final byte[] buf = {(byte) (reg & 0x7f), 0}; m_spi.write(buf, 2); m_spi.read(false, buf, 2); - return toUShort(buf); + return (buf[0] << 8) & buf[1]; } - /** - * @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)); + final byte[] buf = {(byte) (0x80 | reg), (byte) (val & 0xff)}; m_spi.write(buf, 2); // high byte - buf.put(0, (byte) (0x81 | reg)); - buf.put(1, (byte) (val >> 8)); + buf[0] = (byte) (0x81 | reg); + buf[1] = (byte) (val >> 8); m_spi.write(buf, 2); } @@ -764,7 +644,6 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable { System.out.println("Finished cleaning up after the IMU driver."); } - /** */ private void acquire() { // Set data packet length final int dataset_len = 27; // 26 data points + timestamp @@ -772,32 +651,11 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable { // 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_x = 0.0; - double delta_angle_y = 0.0; - double delta_angle_z = 0.0; - double gyro_rate_x = 0.0; - double gyro_rate_y = 0.0; - double gyro_rate_z = 0.0; - double accel_x = 0.0; - double accel_y = 0.0; - double accel_z = 0.0; - double gyro_rate_x_si = 0.0; - double gyro_rate_y_si = 0.0; - double gyro_rate_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 try { Thread.sleep(10); } catch (InterruptedException e) { @@ -806,108 +664,70 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable { 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 */ + // Read number of bytes currently stored in the buffer + int data_count = m_spi.readAutoReceivedData(buffer, 0, 0); + // Check if frame is incomplete + int data_remainder = data_count % dataset_len; + // Remove incomplete data from read count + int data_to_read = data_count - data_remainder; + // 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) + // Read data from DMA buffer (only complete sets) + m_spi.readAutoReceivedData(buffer, data_to_read, 0); // 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 all 3 axes and scale by the elapsed time + // (based on timestamp) + double elapsed_time = m_scaled_sample_rate / (buffer[i] - previous_timestamp); + double delta_angle_x = + toInt(buffer[i + 3], buffer[i + 4], buffer[i + 5], buffer[i + 6]) + * kDeltaAngleSf + / elapsed_time; + double delta_angle_y = + toInt(buffer[i + 7], buffer[i + 8], buffer[i + 9], buffer[i + 10]) + * kDeltaAngleSf + / elapsed_time; + double delta_angle_z = + toInt(buffer[i + 11], buffer[i + 12], buffer[i + 13], buffer[i + 14]) + * kDeltaAngleSf + / elapsed_time; - /* - * 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])); - */ + double gyro_rate_x = toShort(buffer[i + 15], buffer[i + 16]) / 10.0; + double gyro_rate_y = toShort(buffer[i + 17], buffer[i + 18]) / 10.0; + double gyro_rate_z = toShort(buffer[i + 19], buffer[i + 20]) / 10.0; - /* - * Get delta angle value for all 3 axes and scale by the elapsed time - * (based on timestamp) - */ - delta_angle_x = - (toInt(buffer[i + 3], buffer[i + 4], buffer[i + 5], buffer[i + 6]) * delta_angle_sf) - / (m_scaled_sample_rate / (buffer[i] - previous_timestamp)); - delta_angle_y = - (toInt(buffer[i + 7], buffer[i + 8], buffer[i + 9], buffer[i + 10]) * delta_angle_sf) - / (m_scaled_sample_rate / (buffer[i] - previous_timestamp)); - delta_angle_z = - (toInt(buffer[i + 11], buffer[i + 12], buffer[i + 13], buffer[i + 14]) - * delta_angle_sf) - / (m_scaled_sample_rate / (buffer[i] - previous_timestamp)); - - gyro_rate_x = (toShort(buffer[i + 15], buffer[i + 16]) / 10.0); - gyro_rate_y = (toShort(buffer[i + 17], buffer[i + 18]) / 10.0); - gyro_rate_z = (toShort(buffer[i + 19], buffer[i + 20]) / 10.0); - - accel_x = (toShort(buffer[i + 21], buffer[i + 22]) / 800.0); - accel_y = (toShort(buffer[i + 23], buffer[i + 24]) / 800.0); - accel_z = (toShort(buffer[i + 25], buffer[i + 26]) / 800.0); + double accel_x = toShort(buffer[i + 21], buffer[i + 22]) / 800.0; + double accel_y = toShort(buffer[i + 23], buffer[i + 24]) / 800.0; + double accel_z = toShort(buffer[i + 25], buffer[i + 26]) / 800.0; // Convert scaled sensor data to SI units (for tilt calculations) // TODO: Should the unit outputs be selectable? - gyro_rate_x_si = gyro_rate_x * deg_to_rad; - gyro_rate_y_si = gyro_rate_y * deg_to_rad; - gyro_rate_z_si = gyro_rate_z * deg_to_rad; - accel_x_si = accel_x * grav; - accel_y_si = accel_y * grav; - accel_z_si = accel_z * grav; + double gyro_rate_x_si = Math.toRadians(gyro_rate_x); + double gyro_rate_y_si = Math.toRadians(gyro_rate_y); + // double gyro_rate_z_si = Math.toRadians(gyro_rate_z); + double accel_x_si = accel_x * kGrav; + double accel_y_si = accel_y * kGrav; + double accel_z_si = accel_z * kGrav; // Store timestamp for next iteration previous_timestamp = buffer[i]; - m_alpha = m_tau / (m_tau + m_dt); + m_alpha = kTau / (kTau + m_dt); + // Run inclinometer calculations + double accelAngleX = Math.atan2(accel_x_si, Math.hypot(accel_y_si, accel_z_si)); + double accelAngleY = Math.atan2(accel_y_si, Math.hypot(accel_x_si, accel_z_si)); 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; - - m_average_gyro_rate_x = 0.0; - m_average_gyro_rate_y = 0.0; - m_average_gyro_rate_z = 0.0; } 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_rate_y_si); @@ -915,15 +735,11 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable { } synchronized (this) { - /* Push data to global variables */ + // 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_x = 0.0; - m_integ_angle_y = 0.0; - m_integ_angle_z = 0.0; + // Don't accumulate first run. previous_timestamp will be "very" old and the + // integration will end up way off + reset(); } else { m_integ_angle_x += delta_angle_x; m_integ_angle_y += delta_angle_y; @@ -935,57 +751,22 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable { 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_average_gyro_rate_x += gyro_rate_x; - m_average_gyro_rate_y += gyro_rate_y; - m_average_gyro_rate_z += gyro_rate_z; + m_compAngleX = Math.toDegrees(compAngleX); + m_compAngleY = Math.toDegrees(compAngleY); + m_accelAngleX = Math.toDegrees(accelAngleX); + m_accelAngleY = Math.toDegrees(accelAngleY); } m_first_run = false; } - - // The inverse of data to read divided by dataset length, his is the number of iterations - // of the for loop inverted (so multiplication can be used instead of division) - double invTotalIterations = (double) dataset_len / data_to_read; - m_average_gyro_rate_x *= invTotalIterations; - m_average_gyro_rate_y *= invTotalIterations; - m_average_gyro_rate_z *= invTotalIterations; } else { m_thread_idle = true; - data_count = 0; - data_remainder = 0; - data_to_read = 0; previous_timestamp = 0.0; - delta_angle_x = 0.0; - delta_angle_y = 0.0; - delta_angle_z = 0.0; - gyro_rate_x = 0.0; - gyro_rate_y = 0.0; - gyro_rate_z = 0.0; - accel_x = 0.0; - accel_y = 0.0; - accel_z = 0.0; - gyro_rate_x_si = 0.0; - gyro_rate_y_si = 0.0; - gyro_rate_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; @@ -995,25 +776,6 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable { 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; @@ -1023,20 +785,10 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable { 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; + return MathUtil.angleModulus(compAngle); } /** @@ -1159,27 +911,7 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable { * @return The Yaw axis angle in degrees (CCW positive). */ public synchronized double getAngle() { - return switch (m_yaw_axis) { - case kX -> { - if (m_simGyroAngleX != null) { - yield m_simGyroAngleX.get(); - } - yield m_integ_angle_x; - } - case kY -> { - if (m_simGyroAngleY != null) { - yield m_simGyroAngleY.get(); - } - yield m_integ_angle_y; - } - case kZ -> { - if (m_simGyroAngleZ != null) { - yield m_simGyroAngleZ.get(); - } - yield m_integ_angle_z; - } - default -> 0.0; - }; + return getAngle(m_yaw_axis); } /** @@ -1226,27 +958,7 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable { * @return Yaw axis angular rate in degrees per second (CCW positive). */ public synchronized double getRate() { - return switch (m_yaw_axis) { - case kX -> { - if (m_simGyroRateX != null) { - yield m_simGyroRateX.get(); - } - yield m_gyro_rate_x; - } - case kY -> { - if (m_simGyroRateY != null) { - yield m_simGyroRateY.get(); - } - yield m_gyro_rate_y; - } - case kZ -> { - if (m_simGyroRateZ != null) { - yield m_simGyroRateZ.get(); - } - yield m_gyro_rate_z; - } - default -> 0.0; - }; + return getRate(m_yaw_axis); } /** @@ -1282,7 +994,7 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable { * @return The acceleration in the X axis in meters per second squared. */ public synchronized double getAccelX() { - return m_accel_x * 9.81; + return m_accel_x * kGrav; } /** @@ -1291,7 +1003,7 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable { * @return The acceleration in the Y axis in meters per second squared. */ public synchronized double getAccelY() { - return m_accel_y * 9.81; + return m_accel_y * kGrav; } /** @@ -1300,7 +1012,7 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable { * @return The acceleration in the Z axis in meters per second squared. */ public synchronized double getAccelZ() { - return m_accel_z * 9.81; + return m_accel_z * kGrav; } /**