001// Copyright (c) FIRST and other WPILib contributors. 002// Open Source Software; you can modify and/or share it under the terms of 003// the WPILib BSD license file in the root directory of this project. 004 005/*----------------------------------------------------------------------------*/ 006/* Copyright (c) 2016-2020 Analog Devices Inc. All Rights Reserved. */ 007/* Open Source Software - may be modified and shared by FRC teams. The code */ 008/* must be accompanied by the FIRST BSD license file in the root directory of */ 009/* the project. */ 010/* */ 011/* Modified by Juan Chong - frcsupport@analog.com */ 012/*----------------------------------------------------------------------------*/ 013 014package edu.wpi.first.wpilibj; 015 016import edu.wpi.first.hal.FRCNetComm.tResourceType; 017import edu.wpi.first.hal.HAL; 018import edu.wpi.first.hal.SimDevice; 019import edu.wpi.first.hal.SimDouble; 020import edu.wpi.first.networktables.NTSendable; 021import edu.wpi.first.networktables.NTSendableBuilder; 022 023// CHECKSTYLE.OFF: TypeName 024// CHECKSTYLE.OFF: MemberName 025// CHECKSTYLE.OFF: SummaryJavadoc 026// CHECKSTYLE.OFF: UnnecessaryParentheses 027// CHECKSTYLE.OFF: OverloadMethodsDeclarationOrder 028// CHECKSTYLE.OFF: NonEmptyAtclauseDescription 029// CHECKSTYLE.OFF: MissingOverride 030// CHECKSTYLE.OFF: AtclauseOrder 031// CHECKSTYLE.OFF: LocalVariableName 032// CHECKSTYLE.OFF: RedundantModifier 033// CHECKSTYLE.OFF: AbbreviationAsWordInName 034// CHECKSTYLE.OFF: ParameterName 035// CHECKSTYLE.OFF: EmptyCatchBlock 036// CHECKSTYLE.OFF: MissingJavadocMethod 037// CHECKSTYLE.OFF: MissingSwitchDefault 038// CHECKSTYLE.OFF: VariableDeclarationUsageDistance 039// CHECKSTYLE.OFF: ArrayTypeStyle 040 041/** This class is for the ADIS16448 IMU that connects to the RoboRIO MXP port. */ 042@SuppressWarnings({ 043 "unused", 044 "PMD.RedundantFieldInitializer", 045 "PMD.ImmutableField", 046 "PMD.SingularField", 047 "PMD.CollapsibleIfStatements", 048 "PMD.MissingOverride", 049 "PMD.EmptyIfStmt", 050 "PMD.EmptyStatementNotInLoop" 051}) 052public class ADIS16448_IMU implements AutoCloseable, NTSendable { 053 /** ADIS16448 Register Map Declaration */ 054 private static final int FLASH_CNT = 0x00; // Flash memory write count 055 056 private static final int XGYRO_OUT = 0x04; // X-axis gyroscope output 057 private static final int YGYRO_OUT = 0x06; // Y-axis gyroscope output 058 private static final int ZGYRO_OUT = 0x08; // Z-axis gyroscope output 059 private static final int XACCL_OUT = 0x0A; // X-axis accelerometer output 060 private static final int YACCL_OUT = 0x0C; // Y-axis accelerometer output 061 private static final int ZACCL_OUT = 0x0E; // Z-axis accelerometer output 062 private static final int XMAGN_OUT = 0x10; // X-axis magnetometer output 063 private static final int YMAGN_OUT = 0x12; // Y-axis magnetometer output 064 private static final int ZMAGN_OUT = 0x14; // Z-axis magnetometer output 065 private static final int BARO_OUT = 0x16; // Barometer pressure measurement, high word 066 private static final int TEMP_OUT = 0x18; // Temperature output 067 private static final int XGYRO_OFF = 0x1A; // X-axis gyroscope bias offset factor 068 private static final int YGYRO_OFF = 0x1C; // Y-axis gyroscope bias offset factor 069 private static final int ZGYRO_OFF = 0x1E; // Z-axis gyroscope bias offset factor 070 private static final int XACCL_OFF = 0x20; // X-axis acceleration bias offset factor 071 private static final int YACCL_OFF = 0x22; // Y-axis acceleration bias offset factor 072 private static final int ZACCL_OFF = 0x24; // Z-axis acceleration bias offset factor 073 private static final int XMAGN_HIC = 0x26; // X-axis magnetometer, hard iron factor 074 private static final int YMAGN_HIC = 0x28; // Y-axis magnetometer, hard iron factor 075 private static final int ZMAGN_HIC = 0x2A; // Z-axis magnetometer, hard iron factor 076 private static final int XMAGN_SIC = 0x2C; // X-axis magnetometer, soft iron factor 077 private static final int YMAGN_SIC = 0x2E; // Y-axis magnetometer, soft iron factor 078 private static final int ZMAGN_SIC = 0x30; // Z-axis magnetometer, soft iron factor 079 private static final int GPIO_CTRL = 0x32; // GPIO control 080 private static final int MSC_CTRL = 0x34; // MISC control 081 private static final int SMPL_PRD = 0x36; // Sample clock/Decimation filter control 082 private static final int SENS_AVG = 0x38; // Digital filter control 083 private static final int SEQ_CNT = 0x3A; // MAGN_OUT and BARO_OUT counter 084 private static final int DIAG_STAT = 0x3C; // System status 085 private static final int GLOB_CMD = 0x3E; // System command 086 private static final int ALM_MAG1 = 0x40; // Alarm 1 amplitude threshold 087 private static final int ALM_MAG2 = 0x42; // Alarm 2 amplitude threshold 088 private static final int ALM_SMPL1 = 0x44; // Alarm 1 sample size 089 private static final int ALM_SMPL2 = 0x46; // Alarm 2 sample size 090 private static final int ALM_CTRL = 0x48; // Alarm control 091 private static final int LOT_ID1 = 0x52; // Lot identification number 092 private static final int LOT_ID2 = 0x54; // Lot identification number 093 private static final int PROD_ID = 0x56; // Product identifier 094 private static final int SERIAL_NUM = 0x58; // Lot-specific serial number 095 096 public enum CalibrationTime { 097 _32ms(0), 098 _64ms(1), 099 _128ms(2), 100 _256ms(3), 101 _512ms(4), 102 _1s(5), 103 _2s(6), 104 _4s(7), 105 _8s(8), 106 _16s(9), 107 _32s(10), 108 _64s(11); 109 110 private int value; 111 112 private CalibrationTime(int value) { 113 this.value = value; 114 } 115 } 116 117 public enum IMUAxis { 118 kX, 119 kY, 120 kZ 121 } 122 123 // * Static Constants */ 124 private static final double rad_to_deg = 57.2957795; 125 private static final double deg_to_rad = 0.0174532; 126 private static final double grav = 9.81; 127 128 /* User-specified yaw axis */ 129 private IMUAxis m_yaw_axis; 130 131 /* Offset data storage */ 132 private double m_offset_data_gyro_rate_x[]; 133 private double m_offset_data_gyro_rate_y[]; 134 private double m_offset_data_gyro_rate_z[]; 135 136 /* Instant raw output variables */ 137 private double m_gyro_rate_x = 0.0; 138 private double m_gyro_rate_y = 0.0; 139 private double m_gyro_rate_z = 0.0; 140 private double m_accel_x = 0.0; 141 private double m_accel_y = 0.0; 142 private double m_accel_z = 0.0; 143 private double m_mag_x = 0.0; 144 private double m_mag_y = 0.0; 145 private double m_mag_z = 0.0; 146 private double m_baro = 0.0; 147 private double m_temp = 0.0; 148 149 /* IMU gyro offset variables */ 150 private double m_gyro_rate_offset_x = 0.0; 151 private double m_gyro_rate_offset_y = 0.0; 152 private double m_gyro_rate_offset_z = 0.0; 153 private int m_avg_size = 0; 154 private int m_accum_count = 0; 155 156 /* Integrated gyro angle variables */ 157 private double m_integ_gyro_angle_x = 0.0; 158 private double m_integ_gyro_angle_y = 0.0; 159 private double m_integ_gyro_angle_z = 0.0; 160 161 /* Complementary filter variables */ 162 private double m_dt = 0.0; 163 private double m_alpha = 0.0; 164 private double m_tau = 1.0; 165 private double m_compAngleX = 0.0; 166 private double m_compAngleY = 0.0; 167 private double m_accelAngleX = 0.0; 168 private double m_accelAngleY = 0.0; 169 170 /* State variables */ 171 private volatile boolean m_thread_active = false; 172 private CalibrationTime m_calibration_time = CalibrationTime._512ms; 173 private volatile boolean m_first_run = true; 174 private volatile boolean m_thread_idle = false; 175 private boolean m_auto_configured = false; 176 private boolean m_start_up_mode = true; 177 178 /* Resources */ 179 private SPI m_spi; 180 private SPI.Port m_spi_port; 181 private DigitalInput m_auto_interrupt; 182 private DigitalOutput m_reset_out; 183 private DigitalInput m_reset_in; 184 private DigitalOutput m_status_led; 185 private Thread m_acquire_task; 186 187 private SimDevice m_simDevice; 188 private SimDouble m_simGyroAngleX; 189 private SimDouble m_simGyroAngleY; 190 private SimDouble m_simGyroAngleZ; 191 private SimDouble m_simGyroRateX; 192 private SimDouble m_simGyroRateY; 193 private SimDouble m_simGyroRateZ; 194 private SimDouble m_simAccelX; 195 private SimDouble m_simAccelY; 196 private SimDouble m_simAccelZ; 197 198 /* CRC-16 Look-Up Table */ 199 int adiscrc[] = 200 new int[] { 201 0x0000, 0x17CE, 0x0FDF, 0x1811, 0x1FBE, 0x0870, 0x1061, 0x07AF, 202 0x1F3F, 0x08F1, 0x10E0, 0x072E, 0x0081, 0x174F, 0x0F5E, 0x1890, 203 0x1E3D, 0x09F3, 0x11E2, 0x062C, 0x0183, 0x164D, 0x0E5C, 0x1992, 204 0x0102, 0x16CC, 0x0EDD, 0x1913, 0x1EBC, 0x0972, 0x1163, 0x06AD, 205 0x1C39, 0x0BF7, 0x13E6, 0x0428, 0x0387, 0x1449, 0x0C58, 0x1B96, 206 0x0306, 0x14C8, 0x0CD9, 0x1B17, 0x1CB8, 0x0B76, 0x1367, 0x04A9, 207 0x0204, 0x15CA, 0x0DDB, 0x1A15, 0x1DBA, 0x0A74, 0x1265, 0x05AB, 208 0x1D3B, 0x0AF5, 0x12E4, 0x052A, 0x0285, 0x154B, 0x0D5A, 0x1A94, 209 0x1831, 0x0FFF, 0x17EE, 0x0020, 0x078F, 0x1041, 0x0850, 0x1F9E, 210 0x070E, 0x10C0, 0x08D1, 0x1F1F, 0x18B0, 0x0F7E, 0x176F, 0x00A1, 211 0x060C, 0x11C2, 0x09D3, 0x1E1D, 0x19B2, 0x0E7C, 0x166D, 0x01A3, 212 0x1933, 0x0EFD, 0x16EC, 0x0122, 0x068D, 0x1143, 0x0952, 0x1E9C, 213 0x0408, 0x13C6, 0x0BD7, 0x1C19, 0x1BB6, 0x0C78, 0x1469, 0x03A7, 214 0x1B37, 0x0CF9, 0x14E8, 0x0326, 0x0489, 0x1347, 0x0B56, 0x1C98, 215 0x1A35, 0x0DFB, 0x15EA, 0x0224, 0x058B, 0x1245, 0x0A54, 0x1D9A, 216 0x050A, 0x12C4, 0x0AD5, 0x1D1B, 0x1AB4, 0x0D7A, 0x156B, 0x02A5, 217 0x1021, 0x07EF, 0x1FFE, 0x0830, 0x0F9F, 0x1851, 0x0040, 0x178E, 218 0x0F1E, 0x18D0, 0x00C1, 0x170F, 0x10A0, 0x076E, 0x1F7F, 0x08B1, 219 0x0E1C, 0x19D2, 0x01C3, 0x160D, 0x11A2, 0x066C, 0x1E7D, 0x09B3, 220 0x1123, 0x06ED, 0x1EFC, 0x0932, 0x0E9D, 0x1953, 0x0142, 0x168C, 221 0x0C18, 0x1BD6, 0x03C7, 0x1409, 0x13A6, 0x0468, 0x1C79, 0x0BB7, 222 0x1327, 0x04E9, 0x1CF8, 0x0B36, 0x0C99, 0x1B57, 0x0346, 0x1488, 223 0x1225, 0x05EB, 0x1DFA, 0x0A34, 0x0D9B, 0x1A55, 0x0244, 0x158A, 224 0x0D1A, 0x1AD4, 0x02C5, 0x150B, 0x12A4, 0x056A, 0x1D7B, 0x0AB5, 225 0x0810, 0x1FDE, 0x07CF, 0x1001, 0x17AE, 0x0060, 0x1871, 0x0FBF, 226 0x172F, 0x00E1, 0x18F0, 0x0F3E, 0x0891, 0x1F5F, 0x074E, 0x1080, 227 0x162D, 0x01E3, 0x19F2, 0x0E3C, 0x0993, 0x1E5D, 0x064C, 0x1182, 228 0x0912, 0x1EDC, 0x06CD, 0x1103, 0x16AC, 0x0162, 0x1973, 0x0EBD, 229 0x1429, 0x03E7, 0x1BF6, 0x0C38, 0x0B97, 0x1C59, 0x0448, 0x1386, 230 0x0B16, 0x1CD8, 0x04C9, 0x1307, 0x14A8, 0x0366, 0x1B77, 0x0CB9, 231 0x0A14, 0x1DDA, 0x05CB, 0x1205, 0x15AA, 0x0264, 0x1A75, 0x0DBB, 232 0x152B, 0x02E5, 0x1AF4, 0x0D3A, 0x0A95, 0x1D5B, 0x054A, 0x1284 233 }; 234 235 private static class AcquireTask implements Runnable { 236 private final ADIS16448_IMU imu; 237 238 public AcquireTask(final ADIS16448_IMU imu) { 239 this.imu = imu; 240 } 241 242 @Override 243 public void run() { 244 imu.acquire(); 245 } 246 } 247 248 public ADIS16448_IMU() { 249 this(IMUAxis.kZ, SPI.Port.kMXP, CalibrationTime._512ms); 250 } 251 252 /** 253 * @param yaw_axis The axis that measures the yaw 254 * @param port The SPI Port the gyro is plugged into 255 * @param cal_time Calibration time 256 */ 257 public ADIS16448_IMU(final IMUAxis yaw_axis, SPI.Port port, CalibrationTime cal_time) { 258 m_yaw_axis = yaw_axis; 259 m_spi_port = port; 260 261 m_acquire_task = new Thread(new AcquireTask(this)); 262 263 m_simDevice = SimDevice.create("Gyro:ADIS16448", port.value); 264 if (m_simDevice != null) { 265 m_simGyroAngleX = m_simDevice.createDouble("gyro_angle_x", SimDevice.Direction.kInput, 0.0); 266 m_simGyroAngleY = m_simDevice.createDouble("gyro_angle_y", SimDevice.Direction.kInput, 0.0); 267 m_simGyroAngleZ = m_simDevice.createDouble("gyro_angle_z", SimDevice.Direction.kInput, 0.0); 268 m_simGyroRateX = m_simDevice.createDouble("gyro_rate_x", SimDevice.Direction.kInput, 0.0); 269 m_simGyroRateY = m_simDevice.createDouble("gyro_rate_y", SimDevice.Direction.kInput, 0.0); 270 m_simGyroRateZ = m_simDevice.createDouble("gyro_rate_z", SimDevice.Direction.kInput, 0.0); 271 m_simAccelX = m_simDevice.createDouble("accel_x", SimDevice.Direction.kInput, 0.0); 272 m_simAccelY = m_simDevice.createDouble("accel_y", SimDevice.Direction.kInput, 0.0); 273 m_simAccelZ = m_simDevice.createDouble("accel_z", SimDevice.Direction.kInput, 0.0); 274 } 275 276 if (m_simDevice == null) { 277 // Force the IMU reset pin to toggle on startup (doesn't require DS enable) 278 m_reset_out = new DigitalOutput(18); // Drive MXP DIO8 low 279 Timer.delay(0.01); // Wait 10ms 280 m_reset_out.close(); 281 m_reset_in = new DigitalInput(18); // Set MXP DIO8 high 282 Timer.delay(0.25); // Wait 250ms 283 284 configCalTime(cal_time); 285 286 if (!switchToStandardSPI()) { 287 return; 288 } 289 290 // Set IMU internal decimation to 819.2 SPS 291 writeRegister(SMPL_PRD, 0x0001); 292 // Enable Data Ready (LOW = Good Data) on DIO1 (PWM0 on MXP) 293 writeRegister(MSC_CTRL, 0x0016); 294 // Disable IMU internal Bartlett filter 295 writeRegister(SENS_AVG, 0x0400); 296 // Clear offset registers 297 writeRegister(XGYRO_OFF, 0x0000); 298 writeRegister(YGYRO_OFF, 0x0000); 299 writeRegister(ZGYRO_OFF, 0x0000); 300 301 // Configure standard SPI 302 if (!switchToAutoSPI()) { 303 return; 304 } 305 // Notify DS that IMU calibration delay is active 306 DriverStation.reportWarning( 307 "ADIS16448 IMU Detected. Starting initial calibration delay.", false); 308 // Wait for whatever time the user set as the start-up delay 309 try { 310 Thread.sleep((long) (m_calibration_time.value * 1.2 * 1000)); 311 } catch (InterruptedException e) { 312 } 313 // Execute calibration routine 314 calibrate(); 315 // Reset accumulated offsets 316 reset(); 317 // Tell the acquire loop that we're done starting up 318 m_start_up_mode = false; 319 // Let the user know the IMU was initiallized successfully 320 DriverStation.reportWarning("ADIS16448 IMU Successfully Initialized!", false); 321 // Drive MXP PWM5 (IMU ready LED) low (active low) 322 m_status_led = new DigitalOutput(19); 323 } 324 325 // Report usage and post data to DS 326 HAL.report(tResourceType.kResourceType_ADIS16448, 0); 327 } 328 329 /** */ 330 private static int toUShort(byte[] buf) { 331 return (((buf[0] & 0xFF) << 8) + ((buf[1] & 0xFF) << 0)); 332 } 333 334 private static int toUByte(int... buf) { 335 return (buf[0] & 0xFF); 336 } 337 338 public static int toUShort(int... buf) { 339 return (((buf[0] & 0xFF) << 8) + (buf[1] & 0xFF)); 340 } 341 342 /** */ 343 private static long toULong(int sint) { 344 return sint & 0x00000000FFFFFFFFL; 345 } 346 347 /** */ 348 private static int toShort(int... buf) { 349 return (short) (((buf[0] & 0xFF) << 8) + ((buf[1] & 0xFF) << 0)); 350 } 351 352 /** */ 353 private static int toInt(int... buf) { 354 return (int) 355 ((buf[0] & 0xFF) << 24 | (buf[1] & 0xFF) << 16 | (buf[2] & 0xFF) << 8 | (buf[3] & 0xFF)); 356 } 357 358 /** */ 359 private boolean switchToStandardSPI() { 360 // Check to see whether the acquire thread is active. If so, wait for it to stop 361 // producing data. 362 if (m_thread_active) { 363 m_thread_active = false; 364 while (!m_thread_idle) { 365 try { 366 Thread.sleep(10); 367 } catch (InterruptedException e) { 368 } 369 } 370 System.out.println("Paused the IMU processing thread successfully!"); 371 // Maybe we're in auto SPI mode? If so, kill auto SPI, and then SPI. 372 if (m_spi != null && m_auto_configured) { 373 m_spi.stopAuto(); 374 // We need to get rid of all the garbage left in the auto SPI buffer after 375 // stopping it. 376 // Sometimes data magically reappears, so we have to check the buffer size a 377 // couple of times 378 // to be sure we got it all. Yuck. 379 int[] trashBuffer = new int[200]; 380 try { 381 Thread.sleep(100); 382 } catch (InterruptedException e) { 383 } 384 int data_count = m_spi.readAutoReceivedData(trashBuffer, 0, 0); 385 while (data_count > 0) { 386 /* Dequeue 200 at a time, or the remainder of the buffer if less than 200 */ 387 m_spi.readAutoReceivedData(trashBuffer, Math.min(200, data_count), 0); 388 /* Update remaining buffer count */ 389 data_count = m_spi.readAutoReceivedData(trashBuffer, 0, 0); 390 } 391 System.out.println("Paused auto SPI successfully."); 392 } 393 } 394 // There doesn't seem to be a SPI port active. Let's try to set one up 395 if (m_spi == null) { 396 System.out.println("Setting up a new SPI port."); 397 m_spi = new SPI(m_spi_port); 398 m_spi.setClockRate(1000000); 399 m_spi.setMSBFirst(); 400 m_spi.setSampleDataOnTrailingEdge(); 401 m_spi.setClockActiveLow(); 402 m_spi.setChipSelectActiveLow(); 403 readRegister(PROD_ID); // Dummy read 404 405 // Validate the product ID 406 if (readRegister(PROD_ID) != 16448) { 407 DriverStation.reportError("Could not find ADIS16448", false); 408 close(); 409 return false; 410 } 411 return true; 412 } else { 413 // Maybe the SPI port is active, but not in auto SPI mode? Try to read the 414 // product ID. 415 readRegister(PROD_ID); // dummy read 416 if (readRegister(PROD_ID) != 16448) { 417 DriverStation.reportError("Could not find an ADIS16448", false); 418 close(); 419 return false; 420 } else { 421 return true; 422 } 423 } 424 } 425 426 /** */ 427 boolean switchToAutoSPI() { 428 // No SPI port has been set up. Go set one up first. 429 if (m_spi == null) { 430 if (!switchToStandardSPI()) { 431 DriverStation.reportError("Failed to start/restart auto SPI", false); 432 return false; 433 } 434 } 435 // Only set up the interrupt if needed. 436 if (m_auto_interrupt == null) { 437 m_auto_interrupt = new DigitalInput(10); // MXP DIO0 438 } 439 // The auto SPI controller gets angry if you try to set up two instances on one 440 // bus. 441 if (!m_auto_configured) { 442 m_spi.initAuto(8200); 443 m_auto_configured = true; 444 } 445 // Set auto SPI packet data and size 446 m_spi.setAutoTransmitData(new byte[] {GLOB_CMD}, 27); 447 // Configure auto stall time 448 m_spi.configureAutoStall(100, 1000, 255); 449 // Kick off auto SPI (Note: Device configration impossible after auto SPI is 450 // activated) 451 m_spi.startAutoTrigger(m_auto_interrupt, true, false); 452 453 // Check to see if the acquire thread is running. If not, kick one off. 454 if (!m_acquire_task.isAlive()) { 455 m_first_run = true; 456 m_thread_active = true; 457 m_acquire_task.start(); 458 System.out.println("New IMU Processing thread activated!"); 459 } else { 460 // The thread was running, re-init run variables and start it up again. 461 m_first_run = true; 462 m_thread_active = true; 463 System.out.println("Old IMU Processing thread re-activated!"); 464 } 465 // Looks like the thread didn't start for some reason. Abort. 466 if (!m_acquire_task.isAlive()) { 467 DriverStation.reportError("Failed to start/restart the acquire() thread.", false); 468 close(); 469 return false; 470 } 471 return true; 472 } 473 474 public int configDecRate(int m_decRate) { 475 int writeValue = m_decRate; 476 int readbackValue; 477 if (!switchToStandardSPI()) { 478 DriverStation.reportError("Failed to configure/reconfigure standard SPI.", false); 479 return 2; 480 } 481 482 /* Check max */ 483 if (m_decRate > 9) { 484 DriverStation.reportError( 485 "Attemted to write an invalid decimation value. Capping at 9", false); 486 m_decRate = 9; 487 } 488 if (m_decRate < 0) { 489 DriverStation.reportError( 490 "Attemted to write an invalid decimation value. Capping at 0", false); 491 m_decRate = 0; 492 } 493 494 /* Shift decimation setting to correct position and select internal sync */ 495 writeValue = (m_decRate << 8) | 0x1; 496 497 /* Apply to IMU */ 498 writeRegister(SMPL_PRD, writeValue); 499 500 /* Perform read back to verify write */ 501 readbackValue = readRegister(SMPL_PRD); 502 503 /* Throw error for invalid write */ 504 if (readbackValue != writeValue) { 505 DriverStation.reportError("ADIS16448 SMPL_PRD write failed.", false); 506 } 507 508 if (!switchToAutoSPI()) { 509 DriverStation.reportError("Failed to configure/reconfigure auto SPI.", false); 510 return 2; 511 } 512 return 0; 513 } 514 515 /** 516 * Configures calibration time 517 * 518 * @param new_cal_time New calibration time 519 * @return 1 if the new calibration time is the same as the current one else 0 520 */ 521 public int configCalTime(CalibrationTime new_cal_time) { 522 if (m_calibration_time == new_cal_time) { 523 return 1; 524 } else { 525 m_calibration_time = new_cal_time; 526 m_avg_size = m_calibration_time.value * 819; 527 initOffsetBuffer(m_avg_size); 528 return 0; 529 } 530 } 531 532 private void initOffsetBuffer(int size) { 533 // Avoid exceptions in the case of bad arguments 534 if (size < 1) { 535 size = 1; 536 } 537 // Set average size to size (correct bad values) 538 m_avg_size = size; 539 synchronized (this) { 540 // Resize vector 541 m_offset_data_gyro_rate_x = new double[size]; 542 m_offset_data_gyro_rate_y = new double[size]; 543 m_offset_data_gyro_rate_z = new double[size]; 544 // Set acculumate count to 0 545 m_accum_count = 0; 546 } 547 } 548 549 /** 550 * Calibrate the gyro. It's important to make sure that the robot is not moving while the 551 * calibration is in progress, this is typically done when the robot is first turned on while it's 552 * sitting at rest before the match starts. 553 */ 554 public void calibrate() { 555 synchronized (this) { 556 int gyroAverageSize = Math.min(m_accum_count, m_avg_size); 557 double accum_gyro_rate_x = 0.0; 558 double accum_gyro_rate_y = 0.0; 559 double accum_gyro_rate_z = 0.0; 560 for (int i = 0; i < gyroAverageSize; i++) { 561 accum_gyro_rate_x += m_offset_data_gyro_rate_x[i]; 562 accum_gyro_rate_y += m_offset_data_gyro_rate_y[i]; 563 accum_gyro_rate_z += m_offset_data_gyro_rate_z[i]; 564 } 565 m_gyro_rate_offset_x = accum_gyro_rate_x / gyroAverageSize; 566 m_gyro_rate_offset_y = accum_gyro_rate_y / gyroAverageSize; 567 m_gyro_rate_offset_z = accum_gyro_rate_z / gyroAverageSize; 568 m_integ_gyro_angle_x = 0.0; 569 m_integ_gyro_angle_y = 0.0; 570 m_integ_gyro_angle_z = 0.0; 571 // System.out.println("Avg Size: " + gyroAverageSize + "X Off: " + 572 // m_gyro_rate_offset_x + "Y Off: " + m_gyro_rate_offset_y + "Z Off: " + 573 // m_gyro_rate_offset_z); 574 } 575 } 576 577 /** 578 * Sets the yaw axis 579 * 580 * @param yaw_axis The new yaw axis to use 581 * @return 1 if the new yaw axis is the same as the current one else 0. 582 */ 583 public int setYawAxis(IMUAxis yaw_axis) { 584 if (m_yaw_axis == yaw_axis) { 585 return 1; 586 } 587 m_yaw_axis = yaw_axis; 588 reset(); 589 return 0; 590 } 591 592 private int readRegister(final int reg) { 593 // ByteBuffer buf = ByteBuffer.allocateDirect(2); 594 final byte[] buf = new byte[2]; 595 // buf.order(ByteOrder.BIG_ENDIAN); 596 buf[0] = (byte) (reg & 0x7f); 597 buf[1] = (byte) 0; 598 599 m_spi.write(buf, 2); 600 m_spi.read(false, buf, 2); 601 602 return toUShort(buf); 603 } 604 605 private void writeRegister(final int reg, final int val) { 606 final byte[] buf = new byte[2]; 607 // low byte 608 buf[0] = (byte) (0x80 | reg); 609 buf[1] = (byte) (val & 0xff); 610 m_spi.write(buf, 2); 611 // high byte 612 buf[0] = (byte) (0x81 | reg); 613 buf[1] = (byte) (val >> 8); 614 m_spi.write(buf, 2); 615 } 616 617 /** {@inheritDoc} */ 618 public void reset() { 619 synchronized (this) { 620 m_integ_gyro_angle_x = 0.0; 621 m_integ_gyro_angle_y = 0.0; 622 m_integ_gyro_angle_z = 0.0; 623 } 624 } 625 626 /** Delete (free) the spi port used for the IMU. */ 627 @Override 628 public void close() { 629 if (m_thread_active) { 630 m_thread_active = false; 631 try { 632 if (m_acquire_task != null) { 633 m_acquire_task.join(); 634 m_acquire_task = null; 635 } 636 } catch (InterruptedException e) { 637 } 638 if (m_spi != null) { 639 if (m_auto_configured) { 640 m_spi.stopAuto(); 641 } 642 m_spi.close(); 643 m_auto_configured = false; 644 if (m_auto_interrupt != null) { 645 m_auto_interrupt.close(); 646 m_auto_interrupt = null; 647 } 648 m_spi = null; 649 } 650 } 651 System.out.println("Finished cleaning up after the IMU driver."); 652 } 653 654 /** */ 655 private void acquire() { 656 // Set data packet length 657 final int dataset_len = 29; // 18 data points + timestamp 658 final int BUFFER_SIZE = 4000; 659 660 // Set up buffers and variables 661 int[] buffer = new int[BUFFER_SIZE]; 662 int data_count = 0; 663 int data_remainder = 0; 664 int data_to_read = 0; 665 int bufferAvgIndex = 0; 666 double previous_timestamp = 0.0; 667 double delta_angle = 0.0; 668 double gyro_rate_x = 0.0; 669 double gyro_rate_y = 0.0; 670 double gyro_rate_z = 0.0; 671 double accel_x = 0.0; 672 double accel_y = 0.0; 673 double accel_z = 0.0; 674 double mag_x = 0.0; 675 double mag_y = 0.0; 676 double mag_z = 0.0; 677 double baro = 0.0; 678 double temp = 0.0; 679 double gyro_rate_x_si = 0.0; 680 double gyro_rate_y_si = 0.0; 681 double gyro_rate_z_si = 0.0; 682 double accel_x_si = 0.0; 683 double accel_y_si = 0.0; 684 double accel_z_si = 0.0; 685 double compAngleX = 0.0; 686 double compAngleY = 0.0; 687 double accelAngleX = 0.0; 688 double accelAngleY = 0.0; 689 690 while (true) { 691 // Sleep loop for 5ms 692 try { 693 Thread.sleep(5); 694 } catch (InterruptedException e) { 695 } 696 697 if (m_thread_active) { 698 m_thread_idle = false; 699 700 data_count = 701 m_spi.readAutoReceivedData( 702 buffer, 0, 0); // Read number of bytes currently stored in the buffer 703 data_remainder = 704 data_count % dataset_len; // Check if frame is incomplete. Add 1 because of timestamp 705 data_to_read = data_count - data_remainder; // Remove incomplete data from read count 706 if (data_to_read > BUFFER_SIZE) { 707 DriverStation.reportWarning( 708 "ADIS16448 data processing thread overrun has occurred!", false); 709 data_to_read = BUFFER_SIZE - (BUFFER_SIZE % dataset_len); 710 } 711 m_spi.readAutoReceivedData( 712 buffer, data_to_read, 0); // Read data from DMA buffer (only complete sets) 713 714 // Could be multiple data sets in the buffer. Handle each one. 715 for (int i = 0; i < data_to_read; i += dataset_len) { 716 // Calculate CRC-16 on each data packet 717 int calc_crc = 0x0000FFFF; // Starting word 718 int read_byte = 0; 719 int imu_crc = 0; 720 for (int k = 5; 721 k < 27; 722 k += 2) { // Cycle through XYZ GYRO, XYZ ACCEL, XYZ MAG, BARO, TEMP (Ignore Status & 723 // CRC) 724 read_byte = buffer[i + k + 1]; // Process LSB 725 calc_crc = (calc_crc >>> 8) ^ adiscrc[(calc_crc & 0x000000FF) ^ read_byte]; 726 read_byte = buffer[i + k]; // Process MSB 727 calc_crc = (calc_crc >>> 8) ^ adiscrc[(calc_crc & 0x000000FF) ^ read_byte]; 728 } 729 calc_crc = ~calc_crc & 0xFFFF; // Complement 730 calc_crc = ((calc_crc << 8) | (calc_crc >> 8)) & 0xFFFF; // Flip LSB & MSB 731 imu_crc = toUShort(buffer[i + 27], buffer[i + 28]); // Extract DUT CRC from data buffer 732 733 if (calc_crc == imu_crc) { 734 // Timestamp is at buffer[i] 735 m_dt = ((double) buffer[i] - previous_timestamp) / 1000000.0; 736 737 // Scale sensor data 738 gyro_rate_x = (toShort(buffer[i + 5], buffer[i + 6]) * 0.04); 739 gyro_rate_y = (toShort(buffer[i + 7], buffer[i + 8]) * 0.04); 740 gyro_rate_z = (toShort(buffer[i + 9], buffer[i + 10]) * 0.04); 741 accel_x = (toShort(buffer[i + 11], buffer[i + 12]) * 0.833); 742 accel_y = (toShort(buffer[i + 13], buffer[i + 14]) * 0.833); 743 accel_z = (toShort(buffer[i + 15], buffer[i + 16]) * 0.833); 744 mag_x = (toShort(buffer[i + 17], buffer[i + 18]) * 0.1429); 745 mag_y = (toShort(buffer[i + 19], buffer[i + 20]) * 0.1429); 746 mag_z = (toShort(buffer[i + 21], buffer[i + 22]) * 0.1429); 747 baro = (toShort(buffer[i + 23], buffer[i + 24]) * 0.02); 748 temp = (toShort(buffer[i + 25], buffer[i + 26]) * 0.07386 + 31.0); 749 750 // Convert scaled sensor data to SI units (for tilt calculations) 751 // TODO: Should the unit outputs be selectable? 752 gyro_rate_x_si = gyro_rate_x * deg_to_rad; 753 gyro_rate_y_si = gyro_rate_y * deg_to_rad; 754 gyro_rate_z_si = gyro_rate_z * deg_to_rad; 755 accel_x_si = accel_x * grav; 756 accel_y_si = accel_y * grav; 757 accel_z_si = accel_z * grav; 758 // Store timestamp for next iteration 759 previous_timestamp = buffer[i]; 760 // Calculate alpha for use with the complementary filter 761 m_alpha = m_tau / (m_tau + m_dt); 762 // Calculate complementary filter 763 if (m_first_run) { 764 // Set up inclinometer calculations for first run 765 accelAngleX = 766 Math.atan2( 767 -accel_x_si, 768 Math.sqrt((accel_y_si * accel_y_si) + (-accel_z_si * -accel_z_si))); 769 accelAngleY = 770 Math.atan2( 771 accel_y_si, 772 Math.sqrt((-accel_x_si * -accel_x_si) + (-accel_z_si * -accel_z_si))); 773 compAngleX = accelAngleX; 774 compAngleY = accelAngleY; 775 } else { 776 // Run inclinometer calculations 777 accelAngleX = 778 Math.atan2( 779 -accel_x_si, 780 Math.sqrt((accel_y_si * accel_y_si) + (-accel_z_si * -accel_z_si))); 781 accelAngleY = 782 Math.atan2( 783 accel_y_si, 784 Math.sqrt((-accel_x_si * -accel_x_si) + (-accel_z_si * -accel_z_si))); 785 accelAngleX = formatAccelRange(accelAngleX, -accel_z_si); 786 accelAngleY = formatAccelRange(accelAngleY, -accel_z_si); 787 compAngleX = compFilterProcess(compAngleX, accelAngleX, -gyro_rate_y_si); 788 compAngleY = compFilterProcess(compAngleY, accelAngleY, -gyro_rate_x_si); 789 } 790 791 // Update global variables and state 792 synchronized (this) { 793 // Ignore first, integrated sample 794 if (m_first_run) { 795 m_integ_gyro_angle_x = 0.0; 796 m_integ_gyro_angle_y = 0.0; 797 m_integ_gyro_angle_z = 0.0; 798 } else { 799 // Accumulate gyro for offset calibration 800 // Add to buffer 801 bufferAvgIndex = m_accum_count % m_avg_size; 802 m_offset_data_gyro_rate_x[bufferAvgIndex] = gyro_rate_x; 803 m_offset_data_gyro_rate_y[bufferAvgIndex] = gyro_rate_y; 804 m_offset_data_gyro_rate_z[bufferAvgIndex] = gyro_rate_z; 805 // Increment counter 806 m_accum_count++; 807 } 808 if (!m_start_up_mode) { 809 m_gyro_rate_x = gyro_rate_x; 810 m_gyro_rate_y = gyro_rate_y; 811 m_gyro_rate_z = gyro_rate_z; 812 m_accel_x = accel_x; 813 m_accel_y = accel_y; 814 m_accel_z = accel_z; 815 m_mag_x = mag_x; 816 m_mag_y = mag_y; 817 m_mag_z = mag_z; 818 m_baro = baro; 819 m_temp = temp; 820 m_compAngleX = compAngleX * rad_to_deg; 821 m_compAngleY = compAngleY * rad_to_deg; 822 m_accelAngleX = accelAngleX * rad_to_deg; 823 m_accelAngleY = accelAngleY * rad_to_deg; 824 // Accumulate gyro for angle integration and publish to global variables 825 m_integ_gyro_angle_x += (gyro_rate_x - m_gyro_rate_offset_x) * m_dt; 826 m_integ_gyro_angle_y += (gyro_rate_y - m_gyro_rate_offset_y) * m_dt; 827 m_integ_gyro_angle_z += (gyro_rate_z - m_gyro_rate_offset_z) * m_dt; 828 } 829 // System.out.println("Good CRC"); 830 } 831 m_first_run = false; 832 } else { 833 // System.out.println("Bad CRC"); 834 /* 835 * System.out.println("Calc CRC: " + calc_crc); 836 * System.out.println("IMU CRC: " + imu_crc); 837 * System.out.println( 838 * buffer[i] + " " + 839 * (buffer[i + 1]) + " " + (buffer[i + 2]) + " " + 840 * (buffer[i + 3]) + " " + (buffer[i + 4]) + " " + 841 * (buffer[i + 5]) + " " + (buffer[i + 6]) + " " + 842 * (buffer[i + 7]) + " " + (buffer[i + 8]) + " " + 843 * (buffer[i + 9]) + " " + (buffer[i + 10]) + " " + 844 * (buffer[i + 11]) + " " + (buffer[i + 12]) + " " + 845 * (buffer[i + 13]) + " " + (buffer[i + 14]) + " " + 846 * (buffer[i + 15]) + " " + (buffer[i + 16]) + " " + 847 * (buffer[i + 17]) + " " + (buffer[i + 18]) + " " + 848 * (buffer[i + 19]) + " " + (buffer[i + 20]) + " " + 849 * (buffer[i + 21]) + " " + (buffer[i + 22]) + " " + 850 * (buffer[i + 23]) + " " + (buffer[i + 24]) + " " + 851 * (buffer[i + 25]) + " " + (buffer[i + 26]) + " " + 852 * (buffer[i + 27]) + " " + (buffer[i + 28])); 853 */ 854 } 855 } 856 } else { 857 m_thread_idle = true; 858 data_count = 0; 859 data_remainder = 0; 860 data_to_read = 0; 861 previous_timestamp = 0.0; 862 delta_angle = 0.0; 863 gyro_rate_x = 0.0; 864 gyro_rate_y = 0.0; 865 gyro_rate_z = 0.0; 866 accel_x = 0.0; 867 accel_y = 0.0; 868 accel_z = 0.0; 869 mag_x = 0.0; 870 mag_y = 0.0; 871 mag_z = 0.0; 872 baro = 0.0; 873 temp = 0.0; 874 gyro_rate_x_si = 0.0; 875 gyro_rate_y_si = 0.0; 876 gyro_rate_z_si = 0.0; 877 accel_x_si = 0.0; 878 accel_y_si = 0.0; 879 accel_z_si = 0.0; 880 compAngleX = 0.0; 881 compAngleY = 0.0; 882 accelAngleX = 0.0; 883 accelAngleY = 0.0; 884 } 885 } 886 } 887 888 /** 889 * @param compAngle 890 * @param accAngle 891 * @return 892 */ 893 private double formatFastConverge(double compAngle, double accAngle) { 894 if (compAngle > accAngle + Math.PI) { 895 compAngle = compAngle - 2.0 * Math.PI; 896 } else if (accAngle > compAngle + Math.PI) { 897 compAngle = compAngle + 2.0 * Math.PI; 898 } 899 return compAngle; 900 } 901 902 /** 903 * @param compAngle 904 * @return 905 */ 906 private double formatRange0to2PI(double compAngle) { 907 while (compAngle >= 2 * Math.PI) { 908 compAngle = compAngle - 2.0 * Math.PI; 909 } 910 while (compAngle < 0.0) { 911 compAngle = compAngle + 2.0 * Math.PI; 912 } 913 return compAngle; 914 } 915 916 /** 917 * @param accelAngle 918 * @param accelZ 919 * @return 920 */ 921 private double formatAccelRange(double accelAngle, double accelZ) { 922 if (accelZ < 0.0) { 923 accelAngle = Math.PI - accelAngle; 924 } else if (accelZ > 0.0 && accelAngle < 0.0) { 925 accelAngle = 2.0 * Math.PI + accelAngle; 926 } 927 return accelAngle; 928 } 929 930 /** 931 * @param compAngle 932 * @param accelAngle 933 * @param omega 934 * @return 935 */ 936 private double compFilterProcess(double compAngle, double accelAngle, double omega) { 937 compAngle = formatFastConverge(compAngle, accelAngle); 938 compAngle = m_alpha * (compAngle + omega * m_dt) + (1.0 - m_alpha) * accelAngle; 939 compAngle = formatRange0to2PI(compAngle); 940 if (compAngle > Math.PI) { 941 compAngle = compAngle - 2.0 * Math.PI; 942 } 943 return compAngle; 944 } 945 946 /** @return Yaw axis angle in degrees (CCW positive) */ 947 public synchronized double getAngle() { 948 switch (m_yaw_axis) { 949 case kX: 950 return getGyroAngleX(); 951 case kY: 952 return getGyroAngleY(); 953 case kZ: 954 return getGyroAngleZ(); 955 default: 956 return 0.0; 957 } 958 } 959 960 /** @return Yaw axis angular rate in degrees per second (CCW positive) */ 961 public synchronized double getRate() { 962 switch (m_yaw_axis) { 963 case kX: 964 return getGyroAngleX(); 965 case kY: 966 return getGyroAngleY(); 967 case kZ: 968 return getGyroAngleZ(); 969 default: 970 return 0.0; 971 } 972 } 973 974 /** @return Yaw Axis */ 975 public IMUAxis getYawAxis() { 976 return m_yaw_axis; 977 } 978 979 /** @return accumulated gyro angle in the X axis in degrees */ 980 public synchronized double getGyroAngleX() { 981 if (m_simGyroAngleX != null) { 982 return m_simGyroAngleX.get(); 983 } 984 return m_integ_gyro_angle_x; 985 } 986 987 /** @return accumulated gyro angle in the Y axis in degrees */ 988 public synchronized double getGyroAngleY() { 989 if (m_simGyroAngleY != null) { 990 return m_simGyroAngleY.get(); 991 } 992 return m_integ_gyro_angle_y; 993 } 994 995 /** @return accumulated gyro angle in the Z axis in degrees */ 996 public synchronized double getGyroAngleZ() { 997 if (m_simGyroAngleZ != null) { 998 return m_simGyroAngleZ.get(); 999 } 1000 return m_integ_gyro_angle_z; 1001 } 1002 1003 /** @return gyro angular rate in the X axis in degrees per second */ 1004 public synchronized double getGyroRateX() { 1005 if (m_simGyroRateX != null) { 1006 return m_simGyroRateX.get(); 1007 } 1008 return m_gyro_rate_x; 1009 } 1010 1011 /** @return gyro angular rate in the Y axis in degrees per second */ 1012 public synchronized double getGyroRateY() { 1013 if (m_simGyroRateY != null) { 1014 return m_simGyroRateY.get(); 1015 } 1016 return m_gyro_rate_y; 1017 } 1018 1019 /** @return gyro angular rate in the Z axis in degrees per second */ 1020 public synchronized double getGyroRateZ() { 1021 if (m_simGyroRateZ != null) { 1022 return m_simGyroRateZ.get(); 1023 } 1024 return m_gyro_rate_z; 1025 } 1026 1027 /** @return urrent acceleration in the X axis in meters per second squared */ 1028 public synchronized double getAccelX() { 1029 if (m_simAccelX != null) { 1030 return m_simAccelX.get(); 1031 } 1032 return m_accel_x * 9.81; 1033 } 1034 1035 /** @return current acceleration in the Y axis in meters per second squared */ 1036 public synchronized double getAccelY() { 1037 if (m_simAccelY != null) { 1038 return m_simAccelY.get(); 1039 } 1040 return m_accel_y * 9.81; 1041 } 1042 1043 /** @return current acceleration in the Z axis in meters per second squared */ 1044 public synchronized double getAccelZ() { 1045 if (m_simAccelZ != null) { 1046 return m_simAccelZ.get(); 1047 } 1048 return m_accel_z * 9.81; 1049 } 1050 1051 /** @return Magnetic field strength in the X axis in Tesla */ 1052 public synchronized double getMagneticFieldX() { 1053 // mG to T 1054 return m_mag_x * 1e-7; 1055 } 1056 1057 /** @return Magnetic field strength in the Y axis in Tesla */ 1058 public synchronized double getMagneticFieldY() { 1059 // mG to T 1060 return m_mag_y * 1e-7; 1061 } 1062 1063 /** @return Magnetic field strength in the Z axis in Tesla */ 1064 public synchronized double getMagneticFieldZ() { 1065 // mG to T 1066 return m_mag_z * 1e-7; 1067 } 1068 1069 /** @return X axis complementary angle in degrees */ 1070 public synchronized double getXComplementaryAngle() { 1071 return m_compAngleX; 1072 } 1073 1074 /** @return Y axis complementary angle in degrees */ 1075 public synchronized double getYComplementaryAngle() { 1076 return m_compAngleY; 1077 } 1078 1079 /** @return X axis filtered acceleration angle in degrees */ 1080 public synchronized double getXFilteredAccelAngle() { 1081 return m_accelAngleX; 1082 } 1083 1084 /** @return Y axis filtered acceleration angle in degrees */ 1085 public synchronized double getYFilteredAccelAngle() { 1086 return m_accelAngleY; 1087 } 1088 1089 /** @return Barometric Pressure in PSI */ 1090 public synchronized double getBarometricPressure() { 1091 // mbar to PSI 1092 return m_baro * 0.0145; 1093 } 1094 1095 /** @return Temperature in degrees Celsius */ 1096 public synchronized double getTemperature() { 1097 return m_temp; 1098 } 1099 1100 /** 1101 * Get the SPI port number. 1102 * 1103 * @return The SPI port number. 1104 */ 1105 public int getPort() { 1106 return m_spi_port.value; 1107 } 1108 1109 @Override 1110 public void initSendable(NTSendableBuilder builder) { 1111 builder.setSmartDashboardType("Gyro"); 1112 builder.addDoubleProperty("Value", this::getAngle, null); 1113 } 1114}