Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-22 01:11:42 +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

85
wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp
View File

@@ -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

56
wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp
View File

@@ -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)

8
wpilibc/src/main/native/include/frc/ADIS16470_IMU.h
View File

@@ -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>

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)