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