Skip to content

Commit

Permalink
[wpilib] Add flash update capability to ADI IMUs (#6450)
Browse files Browse the repository at this point in the history
  • Loading branch information
@juchong
juchong authored Apr 22, 2024
1 parent 3e5187f commit abfe248
Show file tree
Hide file tree
Showing 5 changed files with 235 additions and 48 deletions.
85 changes: 74 additions & 11 deletions wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<double>(m_calibration_time) * 1.2_s);
// Execute calibration routine
Expand Down
56 changes: 48 additions & 8 deletions wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -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)
Expand Down
8 changes: 4 additions & 4 deletions wpilibc/src/main/native/include/frc/ADIS16470_IMU.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
*
* <b><i>Input axes limited to kX, kY and kZ. Specifying kYaw, kPitch,or kRoll
* will result in an error.</i></b>
Expand Down
76 changes: 65 additions & 11 deletions wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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));
Expand Down
58 changes: 44 additions & 14 deletions wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -292,17 +293,17 @@ public void run() {
/**
* Creates a new ADIS16740 IMU object.
*
* <p>The default setup is the onboard SPI port with a calibration time of 4 seconds. Yaw, pitch,
* <p>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.
*
* <p>The default setup is the onboard SPI port with a calibration time of 4 seconds.
* <p>The default setup is the onboard SPI port with a calibration time of 1 second.
*
* <p><b><i>Input axes limited to kX, kY and kZ. Specifying kYaw, kPitch,or kRoll will result in
* an error.</i></b>
Expand All @@ -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);
}

/**
Expand Down Expand Up @@ -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)
Expand Down

0 comments on commit abfe248

Please sign in to comment.