diff --git a/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp index a9950ae2dbe..8ba4a96c404 100644 --- a/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp +++ b/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp @@ -113,23 +113,86 @@ ADIS16448_IMU::ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port, return; } - // Set IMU internal decimation to 819.2 SPS - WriteRegister(SMPL_PRD, 0x0001); - // Enable Data Ready (LOW = Good Data) on DIO1 (PWM0 on MXP) - WriteRegister(MSC_CTRL, 0x0016); - // Disable IMU internal Bartlett filter - WriteRegister(SENS_AVG, 0x0400); + // Set up flash state variable + bool m_needs_flash = 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; + REPORT_WARNING( + "ADIS16448: SMPL_PRD register configuration inconsistent! Scheduling " + "flash update."); + } + + // 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; + REPORT_WARNING( + "ADIS16448: MSC_CTRL register configuration inconsistent! Scheduling " + "flash update."); + } + + // Disable IMU internal Bartlett filter (204Hz bandwidth is sufficient) and + // set IMU scale factor (range) + if (ReadRegister(SENS_AVG) != 0x0400) { + WriteRegister(SENS_AVG, 0x0400); + m_needs_flash = true; + REPORT_WARNING( + "ADIS16448: SENS_AVG register configuration inconsistent! Scheduling " + "flash update."); + } // Clear offset registers - WriteRegister(XGYRO_OFF, 0x0000); - WriteRegister(YGYRO_OFF, 0x0000); - WriteRegister(ZGYRO_OFF, 0x0000); + if (ReadRegister(XGYRO_OFF) != 0x0000) { + WriteRegister(XGYRO_OFF, 0x0000); + m_needs_flash = true; + REPORT_WARNING( + "ADIS16448: XGYRO_OFF register configuration inconsistent! " + "Scheduling flash update."); + } + + if (ReadRegister(YGYRO_OFF) != 0x0000) { + WriteRegister(YGYRO_OFF, 0x0000); + m_needs_flash = true; + REPORT_WARNING( + "ADIS16448: YGYRO_OFF register configuration inconsistent! " + "Scheduling flash update."); + } + + if (ReadRegister(ZGYRO_OFF) != 0x0000) { + WriteRegister(ZGYRO_OFF, 0x0000); + m_needs_flash = true; + REPORT_WARNING( + "ADIS16448: ZGYRO_OFF register configuration inconsistent! " + "Scheduling flash update."); + } + + // If any registers on the IMU don't match the config, trigger a flash + // update + if (m_needs_flash) { + 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 + // 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!"); + } + // Configure and enable auto SPI if (!SwitchToAutoSPI()) { return; } // Notify DS that IMU calibration delay is active - REPORT_WARNING( - "ADIS16448 IMU Detected. Starting initial calibration delay."); + REPORT_WARNING("ADIS16448: Starting initial calibration delay."); // Wait for whatever time the user set as the start-up delay Wait(static_cast(m_calibration_time) * 1.2_s); // Execute calibration routine diff --git a/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp index 6f562e1b6ed..0f8da84a21f 100644 --- a/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp +++ b/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp @@ -63,12 +63,12 @@ inline void ADISReportError(int32_t status, const char* file, int line, * Constructor. */ ADIS16470_IMU::ADIS16470_IMU() - : ADIS16470_IMU(kZ, kY, kX, SPI::Port::kOnboardCS0, CalibrationTime::_4s) {} + : ADIS16470_IMU(kZ, kY, kX, SPI::Port::kOnboardCS0, CalibrationTime::_1s) {} ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, IMUAxis pitch_axis, IMUAxis roll_axis) : ADIS16470_IMU(yaw_axis, pitch_axis, roll_axis, SPI::Port::kOnboardCS0, - CalibrationTime::_4s) {} + CalibrationTime::_1s) {} ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, IMUAxis pitch_axis, IMUAxis roll_axis, SPI::Port port, @@ -132,20 +132,60 @@ ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, IMUAxis pitch_axis, return; } + // Set up flash state variable + bool m_needs_flash = false; + // Set IMU internal decimation to 4 (output data rate of 2000 SPS / (4 + 1) // = 400Hz) - WriteRegister(DEC_RATE, 0x0004); + if (ReadRegister(DEC_RATE) != 0x0004) { + WriteRegister(DEC_RATE, 0x0004); + m_needs_flash = true; + REPORT_WARNING( + "ADIS16470: DEC_RATE register configuration inconsistent! Scheduling " + "flash update."); + } // Set data ready polarity (HIGH = Good Data), Disable gSense Compensation // and PoP - WriteRegister(MSC_CTRL, 0x0001); - // Configure IMU internal Bartlett filter - WriteRegister(FILT_CTRL, 0x0000); + if (ReadRegister(MSC_CTRL) != 0x0001) { + WriteRegister(MSC_CTRL, 0x0001); + m_needs_flash = true; + REPORT_WARNING( + "ADIS16470: MSC_CTRL register configuration inconsistent! Scheduling " + "flash update."); + } + + // Disable IMU internal Bartlett filter (200Hz bandwidth is sufficient) + if (ReadRegister(FILT_CTRL) != 0x0000) { + WriteRegister(FILT_CTRL, 0x0000); + m_needs_flash = true; + REPORT_WARNING( + "ADIS16470: FILT_CTRL register configuration inconsistent! " + "Scheduling flash update."); + } + + // If any registers on the IMU don't match the config, trigger a flash + // update + if (m_needs_flash) { + REPORT_WARNING( + "ADIS16470: Register configuration changed! Starting IMU flash " + "update."); + WriteRegister(GLOB_CMD, 0x0008); + // Wait long enough for the flash update to finish (72ms minimum as per + // 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 " + "required!"); + } + // Configure continuous bias calibration time based on user setting WriteRegister(NULL_CNFG, m_calibration_time | 0x700); // Notify DS that IMU calibration delay is active - REPORT_WARNING( - "ADIS16470 IMU Detected. Starting initial calibration delay."); + REPORT_WARNING("ADIS16470: Starting initial calibration delay."); // Wait for samples to accumulate internal to the IMU (110% of user-defined // time) diff --git a/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h b/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h index 0c030c7a9e3..f4f4145e177 100644 --- a/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h +++ b/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h @@ -106,16 +106,16 @@ class ADIS16470_IMU : public wpi::Sendable, /** * Creates a new ADIS16740 IMU object. * - * The default setup is the onboard SPI port with a calibration time of 4 - * seconds. Yaw, pitch, and roll are kZ, kX, and kY respectively. + * The default setup is the onboard SPI port with a calibration time of 1 + * second. Yaw, pitch, and roll are kZ, kX, and kY respectively. */ ADIS16470_IMU(); /** * Creates a new ADIS16740 IMU object. * - * The default setup is the onboard SPI port with a calibration time of 4 - * seconds. + * The default setup is the onboard SPI port with a calibration time of 1 + * second. * * Input axes limited to kX, kY and kZ. Specifying kYaw, kPitch,or kRoll * will result in an error. 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 93260c0f2d2..6117b8816a7 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 @@ -192,6 +192,7 @@ public enum IMUAxis { 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 */ private SPI m_spi; @@ -312,24 +313,77 @@ public ADIS16448_IMU(final IMUAxis yaw_axis, SPI.Port port, CalibrationTime cal_ return; } - // Set IMU internal decimation to 819.2 SPS - writeRegister(SMPL_PRD, 0x0001); - // Enable Data Ready (LOW = Good Data) on DIO1 (PWM0 on MXP) - writeRegister(MSC_CTRL, 0x0016); - // Disable IMU internal Bartlett filter - writeRegister(SENS_AVG, 0x0400); + // 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; + DriverStation.reportWarning( + "ADIS16448: SMPL_PRD register configuration inconsistent! Scheduling flash update.", + false); + } + + // 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; + DriverStation.reportWarning( + "ADIS16448: MSC_CTRL register configuration inconsistent! Scheduling flash update.", + false); + } + + // 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; + DriverStation.reportWarning( + "ADIS16448: SENS_AVG register configuration inconsistent! Scheduling flash update.", + false); + } // Clear offset registers - writeRegister(XGYRO_OFF, 0x0000); - writeRegister(YGYRO_OFF, 0x0000); - writeRegister(ZGYRO_OFF, 0x0000); + if (readRegister(XGYRO_OFF) != 0x0000) { + writeRegister(XGYRO_OFF, 0x0000); + m_needs_flash = true; + DriverStation.reportWarning( + "ADIS16448: XGYRO_OFF register configuration inconsistent! Scheduling flash update.", + false); + } + + if (readRegister(YGYRO_OFF) != 0x0000) { + writeRegister(YGYRO_OFF, 0x0000); + m_needs_flash = true; + DriverStation.reportWarning( + "ADIS16448: YGYRO_OFF register configuration inconsistent! Scheduling flash update.", + false); + } + + if (readRegister(ZGYRO_OFF) != 0x0000) { + writeRegister(ZGYRO_OFF, 0x0000); + m_needs_flash = 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) { + 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); + } // Configure standard SPI if (!switchToAutoSPI()) { return; } // Notify DS that IMU calibration delay is active - DriverStation.reportWarning( - "ADIS16448 IMU Detected. Starting initial calibration delay.", false); + DriverStation.reportWarning("ADIS16448: Starting initial calibration delay.", false); // Wait for whatever time the user set as the start-up delay try { Thread.sleep((long) (m_calibration_time.value * 1.2 * 1000)); 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 23b6538c3c7..c51d209cea7 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 @@ -253,6 +253,7 @@ public enum IMUAxis { 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; @@ -292,17 +293,17 @@ public void run() { /** * Creates a new ADIS16740 IMU object. * - *

The default setup is the onboard SPI port with a calibration time of 4 seconds. Yaw, pitch, + *

The default setup is the onboard SPI port with a calibration time of 1 second. Yaw, pitch, * and roll are kZ, kX, and kY respectively. */ public ADIS16470_IMU() { - this(IMUAxis.kZ, IMUAxis.kX, IMUAxis.kY, SPI.Port.kOnboardCS0, CalibrationTime._4s); + this(IMUAxis.kZ, IMUAxis.kX, IMUAxis.kY, SPI.Port.kOnboardCS0, CalibrationTime._1s); } /** * Creates a new ADIS16740 IMU object. * - *

The default setup is the onboard SPI port with a calibration time of 4 seconds. + *

The default setup is the onboard SPI port with a calibration time of 1 second. * *

Input axes limited to kX, kY and kZ. Specifying kYaw, kPitch,or kRoll will result in * an error. @@ -312,7 +313,7 @@ public ADIS16470_IMU() { * @param roll_axis The axis that measures the roll */ public ADIS16470_IMU(IMUAxis yaw_axis, IMUAxis pitch_axis, IMUAxis roll_axis) { - this(yaw_axis, pitch_axis, roll_axis, SPI.Port.kOnboardCS0, CalibrationTime._4s); + this(yaw_axis, pitch_axis, roll_axis, SPI.Port.kOnboardCS0, CalibrationTime._1s); } /** @@ -391,23 +392,52 @@ public ADIS16470_IMU( return; } - // Set IMU internal decimation to 4 (output data rate of 2000 SPS / (4 + 1) = - // 400Hz) - writeRegister(DEC_RATE, 4); + // 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; + DriverStation.reportWarning( + "ADIS16470: DEC_RATE register configuration inconsistent! Scheduling flash update.", + false); + } + + // 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; + DriverStation.reportWarning( + "ADIS16470: MSC_CTRL register configuration inconsistent! Scheduling flash update.", + false); + } - // Set data ready polarity (HIGH = Good Data), Disable gSense Compensation and - // PoP - writeRegister(MSC_CTRL, 1); + // Disable IMU internal Bartlett filter (200Hz bandwidth is sufficient) + if (readRegister(FILT_CTRL) != 0x0000) { + writeRegister(FILT_CTRL, 0x0000); + m_needs_flash = true; + DriverStation.reportWarning( + "ADIS16470: FILT_CTRL register configuration inconsistent! Scheduling flash update.", + false); + } - // Configure IMU internal Bartlett filter - writeRegister(FILT_CTRL, 0); + // If any registers on the IMU don't match the config, trigger a flash update + if (m_needs_flash) { + 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)); // Notify DS that IMU calibration delay is active - DriverStation.reportWarning( - "ADIS16470 IMU Detected. Starting initial calibration delay.", false); + DriverStation.reportWarning("ADIS16470: Starting initial calibration delay.", false); // Wait for samples to accumulate internal to the IMU (110% of user-defined // time)