Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41: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

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)