Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpilib] Add flash update capability to ADI IMUs (#6450)

Browse Source
This commit is contained in:
Juan Jose Chong
2024-04-21 20:39:53 -07:00
committed by GitHub
parent 3e5187ff32
commit abfe2488ff
5 changed files with 235 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files

76
wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java
View File

@@ -192,6 +192,7 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
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 class ADIS16448_IMU implements AutoCloseable, Sendable {
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));

58
wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java
View File

@@ -253,6 +253,7 @@ 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;
@@ -292,17 +293,17 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
/**
* 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>
@@ -312,7 +313,7 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
* @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 class ADIS16470_IMU implements AutoCloseable, Sendable {
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
writeRegister(MSC_CTRL, 1);
// 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);
}
// Configure IMU internal Bartlett filter
writeRegister(FILT_CTRL, 0);
// 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);
}
// 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)