diff --git a/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp index bf90157094..ef6cdb87ed 100644 --- a/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp +++ b/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp @@ -628,10 +628,10 @@ void ADIS16448_IMU::Acquire() { 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 & 0x00FF) ^ byte]; + 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 & 0x00FF) ^ byte]; + calc_crc = (calc_crc >> 8) ^ m_adiscrc[(calc_crc & 0xFF) ^ byte]; } // Complement calc_crc = ~calc_crc; 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 1519cd6c8b..0c375be361 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 @@ -714,13 +714,13 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable { // 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 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 & 0x000000FF) ^ buffer[i + k + 1]]; + 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 & 0x000000FF) ^ buffer[i + k]]; + calc_crc = (calc_crc >>> 8) ^ m_adiscrc[(calc_crc & 0xFF) ^ buffer[i + k]]; } // Complement calc_crc = ~calc_crc & 0xFFFF; @@ -808,7 +808,6 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable { 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; }