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) FIRST 2016. 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 012package edu.wpi.first.wpilibj; 013 014// import java.lang.FdLibm.Pow; 015import edu.wpi.first.hal.FRCNetComm.tResourceType; 016import edu.wpi.first.hal.HAL; 017import edu.wpi.first.hal.SimDevice; 018import edu.wpi.first.hal.SimDouble; 019import edu.wpi.first.networktables.NTSendable; 020import edu.wpi.first.networktables.NTSendableBuilder; 021import java.nio.ByteBuffer; 022import java.nio.ByteOrder; 023 024// CHECKSTYLE.OFF: TypeName 025// CHECKSTYLE.OFF: MemberName 026// CHECKSTYLE.OFF: SummaryJavadoc 027// CHECKSTYLE.OFF: UnnecessaryParentheses 028// CHECKSTYLE.OFF: OverloadMethodsDeclarationOrder 029// CHECKSTYLE.OFF: NonEmptyAtclauseDescription 030// CHECKSTYLE.OFF: MissingOverride 031// CHECKSTYLE.OFF: AtclauseOrder 032// CHECKSTYLE.OFF: LocalVariableName 033// CHECKSTYLE.OFF: RedundantModifier 034// CHECKSTYLE.OFF: AbbreviationAsWordInName 035// CHECKSTYLE.OFF: ParameterName 036// CHECKSTYLE.OFF: EmptyCatchBlock 037// CHECKSTYLE.OFF: MissingJavadocMethod 038// CHECKSTYLE.OFF: MissingSwitchDefault 039// CHECKSTYLE.OFF: VariableDeclarationUsageDistance 040// CHECKSTYLE.OFF: ArrayTypeStyle 041 042/** This class is for the ADIS16470 IMU that connects to the RoboRIO SPI port. */ 043@SuppressWarnings({ 044 "unused", 045 "PMD.RedundantFieldInitializer", 046 "PMD.ImmutableField", 047 "PMD.SingularField", 048 "PMD.CollapsibleIfStatements", 049 "PMD.MissingOverride", 050 "PMD.EmptyIfStmt", 051 "PMD.EmptyStatementNotInLoop" 052}) 053public class ADIS16470_IMU implements AutoCloseable, NTSendable { 054 /* ADIS16470 Register Map Declaration */ 055 private static final int FLASH_CNT = 0x00; // Flash memory write count 056 private static final int DIAG_STAT = 0x02; // Diagnostic and operational status 057 private static final int X_GYRO_LOW = 0x04; // X-axis gyroscope output, lower word 058 private static final int X_GYRO_OUT = 0x06; // X-axis gyroscope output, upper word 059 private static final int Y_GYRO_LOW = 0x08; // Y-axis gyroscope output, lower word 060 private static final int Y_GYRO_OUT = 0x0A; // Y-axis gyroscope output, upper word 061 private static final int Z_GYRO_LOW = 0x0C; // Z-axis gyroscope output, lower word 062 private static final int Z_GYRO_OUT = 0x0E; // Z-axis gyroscope output, upper word 063 private static final int X_ACCL_LOW = 0x10; // X-axis accelerometer output, lower word 064 private static final int X_ACCL_OUT = 0x12; // X-axis accelerometer output, upper word 065 private static final int Y_ACCL_LOW = 0x14; // Y-axis accelerometer output, lower word 066 private static final int Y_ACCL_OUT = 0x16; // Y-axis accelerometer output, upper word 067 private static final int Z_ACCL_LOW = 0x18; // Z-axis accelerometer output, lower word 068 private static final int Z_ACCL_OUT = 0x1A; // Z-axis accelerometer output, upper word 069 private static final int TEMP_OUT = 0x1C; // Temperature output (internal, not calibrated) 070 private static final int TIME_STAMP = 0x1E; // PPS mode time stamp 071 private static final int X_DELTANG_LOW = 0x24; // X-axis delta angle output, lower word 072 private static final int X_DELTANG_OUT = 0x26; // X-axis delta angle output, upper word 073 private static final int Y_DELTANG_LOW = 0x28; // Y-axis delta angle output, lower word 074 private static final int Y_DELTANG_OUT = 0x2A; // Y-axis delta angle output, upper word 075 private static final int Z_DELTANG_LOW = 0x2C; // Z-axis delta angle output, lower word 076 private static final int Z_DELTANG_OUT = 0x2E; // Z-axis delta angle output, upper word 077 private static final int X_DELTVEL_LOW = 0x30; // X-axis delta velocity output, lower word 078 private static final int X_DELTVEL_OUT = 0x32; // X-axis delta velocity output, upper word 079 private static final int Y_DELTVEL_LOW = 0x34; // Y-axis delta velocity output, lower word 080 private static final int Y_DELTVEL_OUT = 0x36; // Y-axis delta velocity output, upper word 081 private static final int Z_DELTVEL_LOW = 0x38; // Z-axis delta velocity output, lower word 082 private static final int Z_DELTVEL_OUT = 0x3A; // Z-axis delta velocity output, upper word 083 private static final int XG_BIAS_LOW = 084 0x40; // X-axis gyroscope bias offset correction, lower word 085 private static final int XG_BIAS_HIGH = 086 0x42; // X-axis gyroscope bias offset correction, upper word 087 private static final int YG_BIAS_LOW = 088 0x44; // Y-axis gyroscope bias offset correction, lower word 089 private static final int YG_BIAS_HIGH = 090 0x46; // Y-axis gyroscope bias offset correction, upper word 091 private static final int ZG_BIAS_LOW = 092 0x48; // Z-axis gyroscope bias offset correction, lower word 093 private static final int ZG_BIAS_HIGH = 094 0x4A; // Z-axis gyroscope bias offset correction, upper word 095 private static final int XA_BIAS_LOW = 096 0x4C; // X-axis accelerometer bias offset correction, lower word 097 private static final int XA_BIAS_HIGH = 098 0x4E; // X-axis accelerometer bias offset correction, upper word 099 private static final int YA_BIAS_LOW = 100 0x50; // Y-axis accelerometer bias offset correction, lower word 101 private static final int YA_BIAS_HIGH = 102 0x52; // Y-axis accelerometer bias offset correction, upper word 103 private static final int ZA_BIAS_LOW = 104 0x54; // Z-axis accelerometer bias offset correction, lower word 105 private static final int ZA_BIAS_HIGH = 106 0x56; // Z-axis accelerometer bias offset correction, upper word 107 private static final int FILT_CTRL = 0x5C; // Filter control 108 private static final int MSC_CTRL = 0x60; // Miscellaneous control 109 private static final int UP_SCALE = 0x62; // Clock scale factor, PPS mode 110 private static final int DEC_RATE = 0x64; // Decimation rate control (output data rate) 111 private static final int NULL_CNFG = 0x66; // Auto-null configuration control 112 private static final int GLOB_CMD = 0x68; // Global commands 113 private static final int FIRM_REV = 0x6C; // Firmware revision 114 private static final int FIRM_DM = 0x6E; // Firmware revision date, month and day 115 private static final int FIRM_Y = 0x70; // Firmware revision date, year 116 private static final int PROD_ID = 0x72; // Product identification 117 private static final int SERIAL_NUM = 0x74; // Serial number (relative to assembly lot) 118 private static final int USER_SCR1 = 0x76; // User scratch register 1 119 private static final int USER_SCR2 = 0x78; // User scratch register 2 120 private static final int USER_SCR3 = 0x7A; // User scratch register 3 121 private static final int FLSHCNT_LOW = 0x7C; // Flash update count, lower word 122 private static final int FLSHCNT_HIGH = 0x7E; // Flash update count, upper word 123 124 private static final byte[] m_autospi_x_packet = { 125 X_DELTANG_OUT, 126 FLASH_CNT, 127 X_DELTANG_LOW, 128 FLASH_CNT, 129 X_GYRO_OUT, 130 FLASH_CNT, 131 Y_GYRO_OUT, 132 FLASH_CNT, 133 Z_GYRO_OUT, 134 FLASH_CNT, 135 X_ACCL_OUT, 136 FLASH_CNT, 137 Y_ACCL_OUT, 138 FLASH_CNT, 139 Z_ACCL_OUT, 140 FLASH_CNT 141 }; 142 143 private static final byte[] m_autospi_y_packet = { 144 Y_DELTANG_OUT, 145 FLASH_CNT, 146 Y_DELTANG_LOW, 147 FLASH_CNT, 148 X_GYRO_OUT, 149 FLASH_CNT, 150 Y_GYRO_OUT, 151 FLASH_CNT, 152 Z_GYRO_OUT, 153 FLASH_CNT, 154 X_ACCL_OUT, 155 FLASH_CNT, 156 Y_ACCL_OUT, 157 FLASH_CNT, 158 Z_ACCL_OUT, 159 FLASH_CNT 160 }; 161 162 private static final byte[] m_autospi_z_packet = { 163 Z_DELTANG_OUT, 164 FLASH_CNT, 165 Z_DELTANG_LOW, 166 FLASH_CNT, 167 X_GYRO_OUT, 168 FLASH_CNT, 169 Y_GYRO_OUT, 170 FLASH_CNT, 171 Z_GYRO_OUT, 172 FLASH_CNT, 173 X_ACCL_OUT, 174 FLASH_CNT, 175 Y_ACCL_OUT, 176 FLASH_CNT, 177 Z_ACCL_OUT, 178 FLASH_CNT 179 }; 180 181 public enum CalibrationTime { 182 _32ms(0), 183 _64ms(1), 184 _128ms(2), 185 _256ms(3), 186 _512ms(4), 187 _1s(5), 188 _2s(6), 189 _4s(7), 190 _8s(8), 191 _16s(9), 192 _32s(10), 193 _64s(11); 194 195 private int value; 196 197 private CalibrationTime(int value) { 198 this.value = value; 199 } 200 } 201 202 public enum IMUAxis { 203 kX, 204 kY, 205 kZ 206 } 207 208 // Static Constants 209 private static final double delta_angle_sf = 2160.0 / 2147483648.0; /* 2160 / (2^31) */ 210 private static final double rad_to_deg = 57.2957795; 211 private static final double deg_to_rad = 0.0174532; 212 private static final double grav = 9.81; 213 214 // User-specified yaw axis 215 private IMUAxis m_yaw_axis; 216 217 // Instant raw outputs 218 private double m_gyro_rate_x = 0.0; 219 private double m_gyro_rate_y = 0.0; 220 private double m_gyro_rate_z = 0.0; 221 private double m_accel_x = 0.0; 222 private double m_accel_y = 0.0; 223 private double m_accel_z = 0.0; 224 225 // Integrated gyro angle 226 private double m_integ_angle = 0.0; 227 228 // Complementary filter variables 229 private double m_dt = 0.0; 230 private double m_alpha = 0.0; 231 private double m_tau = 1.0; 232 private double m_compAngleX = 0.0; 233 private double m_compAngleY = 0.0; 234 private double m_accelAngleX = 0.0; 235 private double m_accelAngleY = 0.0; 236 237 // State variables 238 private volatile boolean m_thread_active = false; 239 private int m_calibration_time = 0; 240 private volatile boolean m_first_run = true; 241 private volatile boolean m_thread_idle = false; 242 private boolean m_auto_configured = false; 243 private double m_scaled_sample_rate = 2500.0; 244 245 // Resources 246 private SPI m_spi; 247 private SPI.Port m_spi_port; 248 private DigitalInput m_auto_interrupt; 249 private DigitalOutput m_reset_out; 250 private DigitalInput m_reset_in; 251 private DigitalOutput m_status_led; 252 private Thread m_acquire_task; 253 254 private SimDevice m_simDevice; 255 private SimDouble m_simGyroAngleX; 256 private SimDouble m_simGyroAngleY; 257 private SimDouble m_simGyroAngleZ; 258 private SimDouble m_simGyroRateX; 259 private SimDouble m_simGyroRateY; 260 private SimDouble m_simGyroRateZ; 261 private SimDouble m_simAccelX; 262 private SimDouble m_simAccelY; 263 private SimDouble m_simAccelZ; 264 265 private static class AcquireTask implements Runnable { 266 private ADIS16470_IMU imu; 267 268 public AcquireTask(ADIS16470_IMU imu) { 269 this.imu = imu; 270 } 271 272 @Override 273 public void run() { 274 imu.acquire(); 275 } 276 } 277 278 public ADIS16470_IMU() { 279 this(IMUAxis.kZ, SPI.Port.kOnboardCS0, CalibrationTime._4s); 280 } 281 282 /** 283 * @param yaw_axis The axis that measures the yaw 284 * @param port The SPI Port the gyro is plugged into 285 * @param cal_time Calibration time 286 */ 287 public ADIS16470_IMU(IMUAxis yaw_axis, SPI.Port port, CalibrationTime cal_time) { 288 m_yaw_axis = yaw_axis; 289 m_calibration_time = cal_time.value; 290 m_spi_port = port; 291 292 m_acquire_task = new Thread(new AcquireTask(this)); 293 294 m_simDevice = SimDevice.create("Gyro:ADIS16470", port.value); 295 if (m_simDevice != null) { 296 m_simGyroAngleX = m_simDevice.createDouble("gyro_angle_x", SimDevice.Direction.kInput, 0.0); 297 m_simGyroAngleY = m_simDevice.createDouble("gyro_angle_y", SimDevice.Direction.kInput, 0.0); 298 m_simGyroAngleZ = m_simDevice.createDouble("gyro_angle_z", SimDevice.Direction.kInput, 0.0); 299 m_simGyroRateX = m_simDevice.createDouble("gyro_rate_x", SimDevice.Direction.kInput, 0.0); 300 m_simGyroRateY = m_simDevice.createDouble("gyro_rate_y", SimDevice.Direction.kInput, 0.0); 301 m_simGyroRateZ = m_simDevice.createDouble("gyro_rate_z", SimDevice.Direction.kInput, 0.0); 302 m_simAccelX = m_simDevice.createDouble("accel_x", SimDevice.Direction.kInput, 0.0); 303 m_simAccelY = m_simDevice.createDouble("accel_y", SimDevice.Direction.kInput, 0.0); 304 m_simAccelZ = m_simDevice.createDouble("accel_z", SimDevice.Direction.kInput, 0.0); 305 } 306 307 if (m_simDevice == null) { 308 // Force the IMU reset pin to toggle on startup (doesn't require DS enable) 309 // Relies on the RIO hardware by default configuring an output as low 310 // and configuring an input as high Z. The 10k pull-up resistor internal to the 311 // IMU then forces the reset line high for normal operation. 312 m_reset_out = new DigitalOutput(27); // Drive SPI CS2 (IMU RST) low 313 Timer.delay(0.01); // Wait 10ms 314 m_reset_out.close(); 315 m_reset_in = new DigitalInput(27); // Set SPI CS2 (IMU RST) high 316 Timer.delay(0.25); // Wait 250ms for reset to complete 317 318 if (!switchToStandardSPI()) { 319 return; 320 } 321 322 // Set IMU internal decimation to 4 (output data rate of 2000 SPS / (4 + 1) = 323 // 400Hz) 324 writeRegister(DEC_RATE, 4); 325 326 // Set data ready polarity (HIGH = Good Data), Disable gSense Compensation and 327 // PoP 328 writeRegister(MSC_CTRL, 1); 329 330 // Configure IMU internal Bartlett filter 331 writeRegister(FILT_CTRL, 0); 332 333 // Configure continuous bias calibration time based on user setting 334 writeRegister(NULL_CNFG, (m_calibration_time | 0x0700)); 335 336 // Notify DS that IMU calibration delay is active 337 DriverStation.reportWarning( 338 "ADIS16470 IMU Detected. Starting initial calibration delay.", false); 339 340 // Wait for samples to accumulate internal to the IMU (110% of user-defined 341 // time) 342 try { 343 Thread.sleep((long) (Math.pow(2.0, m_calibration_time) / 2000 * 64 * 1.1 * 1000)); 344 } catch (InterruptedException e) { 345 } 346 347 // Write offset calibration command to IMU 348 writeRegister(GLOB_CMD, 0x0001); 349 350 // Configure and enable auto SPI 351 if (!switchToAutoSPI()) { 352 return; 353 } 354 355 // Let the user know the IMU was initiallized successfully 356 DriverStation.reportWarning("ADIS16470 IMU Successfully Initialized!", false); 357 358 // Drive "Ready" LED low 359 m_status_led = new DigitalOutput(28); // Set SPI CS3 (IMU Ready LED) low 360 } 361 362 // Report usage and post data to DS 363 HAL.report(tResourceType.kResourceType_ADIS16470, 0); 364 } 365 366 /** 367 * @param buf 368 * @return 369 */ 370 private static int toUShort(ByteBuffer buf) { 371 return (buf.getShort(0)) & 0xFFFF; 372 } 373 374 /** 375 * @param sint 376 * @return 377 */ 378 private static long toULong(int sint) { 379 return sint & 0x00000000FFFFFFFFL; 380 } 381 382 /** 383 * @param buf 384 * @return 385 */ 386 private static int toShort(int... buf) { 387 return (short) (((buf[0] & 0xFF) << 8) + ((buf[1] & 0xFF) << 0)); 388 } 389 390 /** 391 * @param buf 392 * @return 393 */ 394 private static int toInt(int... buf) { 395 return (int) 396 ((buf[0] & 0xFF) << 24 | (buf[1] & 0xFF) << 16 | (buf[2] & 0xFF) << 8 | (buf[3] & 0xFF)); 397 } 398 399 /** 400 * Switch to standard SPI mode. 401 * 402 * @return 403 */ 404 private boolean switchToStandardSPI() { 405 // Check to see whether the acquire thread is active. If so, wait for it to stop 406 // producing data. 407 if (m_thread_active) { 408 m_thread_active = false; 409 while (!m_thread_idle) { 410 try { 411 Thread.sleep(10); 412 } catch (InterruptedException e) { 413 } 414 } 415 System.out.println("Paused the IMU processing thread successfully!"); 416 // Maybe we're in auto SPI mode? If so, kill auto SPI, and then SPI. 417 if (m_spi != null && m_auto_configured) { 418 m_spi.stopAuto(); 419 // We need to get rid of all the garbage left in the auto SPI buffer after 420 // stopping it. 421 // Sometimes data magically reappears, so we have to check the buffer size a 422 // couple of times 423 // to be sure we got it all. Yuck. 424 int[] trashBuffer = new int[200]; 425 try { 426 Thread.sleep(100); 427 } catch (InterruptedException e) { 428 } 429 int data_count = m_spi.readAutoReceivedData(trashBuffer, 0, 0); 430 while (data_count > 0) { 431 m_spi.readAutoReceivedData(trashBuffer, Math.min(data_count, 200), 0); 432 data_count = m_spi.readAutoReceivedData(trashBuffer, 0, 0); 433 } 434 System.out.println("Paused auto SPI successfully."); 435 } 436 } 437 // There doesn't seem to be a SPI port active. Let's try to set one up 438 if (m_spi == null) { 439 System.out.println("Setting up a new SPI port."); 440 m_spi = new SPI(m_spi_port); 441 m_spi.setClockRate(2000000); 442 m_spi.setMSBFirst(); 443 m_spi.setSampleDataOnTrailingEdge(); 444 m_spi.setClockActiveLow(); 445 m_spi.setChipSelectActiveLow(); 446 readRegister(PROD_ID); // Dummy read 447 448 // Validate the product ID 449 if (readRegister(PROD_ID) != 16982) { 450 DriverStation.reportError("Could not find ADIS16470", false); 451 close(); 452 return false; 453 } 454 return true; 455 } else { 456 // Maybe the SPI port is active, but not in auto SPI mode? Try to read the 457 // product ID. 458 readRegister(PROD_ID); // dummy read 459 if (readRegister(PROD_ID) != 16982) { 460 DriverStation.reportError("Could not find an ADIS16470", false); 461 close(); 462 return false; 463 } else { 464 return true; 465 } 466 } 467 } 468 469 /** @return */ 470 boolean switchToAutoSPI() { 471 // No SPI port has been set up. Go set one up first. 472 if (m_spi == null) { 473 if (!switchToStandardSPI()) { 474 DriverStation.reportError("Failed to start/restart auto SPI", false); 475 return false; 476 } 477 } 478 // Only set up the interrupt if needed. 479 if (m_auto_interrupt == null) { 480 // Configure interrupt on SPI CS1 481 m_auto_interrupt = new DigitalInput(26); 482 } 483 // The auto SPI controller gets angry if you try to set up two instances on one 484 // bus. 485 if (!m_auto_configured) { 486 m_spi.initAuto(8200); 487 m_auto_configured = true; 488 } 489 // Do we need to change auto SPI settings? 490 switch (m_yaw_axis) { 491 case kX: 492 m_spi.setAutoTransmitData(m_autospi_x_packet, 2); 493 break; 494 case kY: 495 m_spi.setAutoTransmitData(m_autospi_y_packet, 2); 496 break; 497 default: 498 m_spi.setAutoTransmitData(m_autospi_z_packet, 2); 499 break; 500 } 501 // Configure auto stall time 502 m_spi.configureAutoStall(5, 1000, 1); 503 // Kick off auto SPI (Note: Device configration impossible after auto SPI is 504 // activated) 505 // DR High = Data good (data capture should be triggered on the rising edge) 506 m_spi.startAutoTrigger(m_auto_interrupt, true, false); 507 508 // Check to see if the acquire thread is running. If not, kick one off. 509 if (!m_acquire_task.isAlive()) { 510 m_first_run = true; 511 m_thread_active = true; 512 m_acquire_task.start(); 513 System.out.println("Processing thread activated!"); 514 } else { 515 // The thread was running, re-init run variables and start it up again. 516 m_first_run = true; 517 m_thread_active = true; 518 System.out.println("Processing thread activated!"); 519 } 520 // Looks like the thread didn't start for some reason. Abort. 521 if (!m_acquire_task.isAlive()) { 522 DriverStation.reportError("Failed to start/restart the acquire() thread.", false); 523 close(); 524 return false; 525 } 526 return true; 527 } 528 529 /** 530 * Configures calibration time 531 * 532 * @param new_cal_time New calibration time 533 * @return 1 if the new calibration time is the same as the current one else 0 534 */ 535 public int configCalTime(CalibrationTime new_cal_time) { 536 if (m_calibration_time == new_cal_time.value) { 537 return 1; 538 } 539 if (!switchToStandardSPI()) { 540 DriverStation.reportError("Failed to configure/reconfigure standard SPI.", false); 541 return 2; 542 } 543 m_calibration_time = new_cal_time.value; 544 writeRegister(NULL_CNFG, (m_calibration_time | 0x700)); 545 if (!switchToAutoSPI()) { 546 DriverStation.reportError("Failed to configure/reconfigure auto SPI.", false); 547 return 2; 548 } 549 return 0; 550 } 551 552 public int configDecRate(int reg) { 553 int m_reg = reg; 554 if (!switchToStandardSPI()) { 555 DriverStation.reportError("Failed to configure/reconfigure standard SPI.", false); 556 return 2; 557 } 558 if (m_reg > 1999) { 559 DriverStation.reportError("Attemted to write an invalid deimation value.", false); 560 m_reg = 1999; 561 } 562 m_scaled_sample_rate = (((m_reg + 1.0) / 2000.0) * 1000000.0); 563 writeRegister(DEC_RATE, m_reg); 564 System.out.println("Decimation register: " + readRegister(DEC_RATE)); 565 if (!switchToAutoSPI()) { 566 DriverStation.reportError("Failed to configure/reconfigure auto SPI.", false); 567 return 2; 568 } 569 return 0; 570 } 571 572 /** 573 * Calibrate the gyro. It's important to make sure that the robot is not moving while the 574 * calibration is in progress, this is typically done when the robot is first turned on while it's 575 * sitting at rest before the match starts. 576 */ 577 public void calibrate() { 578 if (!switchToStandardSPI()) { 579 DriverStation.reportError("Failed to configure/reconfigure standard SPI.", false); 580 } 581 writeRegister(GLOB_CMD, 0x0001); 582 if (!switchToAutoSPI()) { 583 DriverStation.reportError("Failed to configure/reconfigure auto SPI.", false); 584 } 585 ; 586 } 587 588 /** 589 * Sets the yaw axis 590 * 591 * @param yaw_axis The new yaw axis to use 592 * @return 1 if the new yaw axis is the same as the current one, 2 if the switch to Standard SPI 593 * failed, else 0. 594 */ 595 public int setYawAxis(IMUAxis yaw_axis) { 596 if (m_yaw_axis == yaw_axis) { 597 return 1; 598 } 599 if (!switchToStandardSPI()) { 600 DriverStation.reportError("Failed to configure/reconfigure standard SPI.", false); 601 return 2; 602 } 603 m_yaw_axis = yaw_axis; 604 if (!switchToAutoSPI()) { 605 DriverStation.reportError("Failed to configure/reconfigure auto SPI.", false); 606 } 607 return 0; 608 } 609 610 /** 611 * @param reg 612 * @return 613 */ 614 private int readRegister(int reg) { 615 ByteBuffer buf = ByteBuffer.allocateDirect(2); 616 buf.order(ByteOrder.BIG_ENDIAN); 617 buf.put(0, (byte) (reg & 0x7f)); 618 buf.put(1, (byte) 0); 619 620 m_spi.write(buf, 2); 621 m_spi.read(false, buf, 2); 622 623 return toUShort(buf); 624 } 625 626 /** 627 * @param reg 628 * @param val 629 */ 630 private void writeRegister(int reg, int val) { 631 ByteBuffer buf = ByteBuffer.allocateDirect(2); 632 // low byte 633 buf.put(0, (byte) ((0x80 | reg))); 634 buf.put(1, (byte) (val & 0xff)); 635 m_spi.write(buf, 2); 636 // high byte 637 buf.put(0, (byte) (0x81 | reg)); 638 buf.put(1, (byte) (val >> 8)); 639 m_spi.write(buf, 2); 640 } 641 642 /** {@inheritDoc} */ 643 public void reset() { 644 synchronized (this) { 645 m_integ_angle = 0.0; 646 } 647 } 648 649 /** Delete (free) the spi port used for the IMU. */ 650 @Override 651 public void close() { 652 if (m_thread_active) { 653 m_thread_active = false; 654 try { 655 if (m_acquire_task != null) { 656 m_acquire_task.join(); 657 m_acquire_task = null; 658 } 659 } catch (InterruptedException e) { 660 } 661 if (m_spi != null) { 662 if (m_auto_configured) { 663 m_spi.stopAuto(); 664 } 665 m_spi.close(); 666 m_auto_configured = false; 667 if (m_auto_interrupt != null) { 668 m_auto_interrupt.close(); 669 m_auto_interrupt = null; 670 } 671 m_spi = null; 672 } 673 } 674 System.out.println("Finished cleaning up after the IMU driver."); 675 } 676 677 /** */ 678 private void acquire() { 679 // Set data packet length 680 final int dataset_len = 19; // 18 data points + timestamp 681 final int BUFFER_SIZE = 4000; 682 683 // Set up buffers and variables 684 int[] buffer = new int[BUFFER_SIZE]; 685 int data_count = 0; 686 int data_remainder = 0; 687 int data_to_read = 0; 688 double previous_timestamp = 0.0; 689 double delta_angle = 0.0; 690 double gyro_rate_x = 0.0; 691 double gyro_rate_y = 0.0; 692 double gyro_rate_z = 0.0; 693 double accel_x = 0.0; 694 double accel_y = 0.0; 695 double accel_z = 0.0; 696 double gyro_rate_x_si = 0.0; 697 double gyro_rate_y_si = 0.0; 698 double gyro_rate_z_si = 0.0; 699 double accel_x_si = 0.0; 700 double accel_y_si = 0.0; 701 double accel_z_si = 0.0; 702 double compAngleX = 0.0; 703 double compAngleY = 0.0; 704 double accelAngleX = 0.0; 705 double accelAngleY = 0.0; 706 707 while (true) { 708 // Sleep loop for 10ms 709 try { 710 Thread.sleep(10); 711 } catch (InterruptedException e) { 712 } 713 714 if (m_thread_active) { 715 m_thread_idle = false; 716 717 data_count = 718 m_spi.readAutoReceivedData( 719 buffer, 0, 0); // Read number of bytes currently stored in the 720 // buffer 721 data_remainder = 722 data_count % dataset_len; // Check if frame is incomplete. Add 1 because of timestamp 723 data_to_read = data_count - data_remainder; // Remove incomplete data from read count 724 /* Want to cap the data to read in a single read at the buffer size */ 725 if (data_to_read > BUFFER_SIZE) { 726 DriverStation.reportWarning( 727 "ADIS16470 data processing thread overrun has occurred!", false); 728 data_to_read = BUFFER_SIZE - (BUFFER_SIZE % dataset_len); 729 } 730 m_spi.readAutoReceivedData( 731 buffer, data_to_read, 0); // Read data from DMA buffer (only complete sets) 732 733 // Could be multiple data sets in the buffer. Handle each one. 734 for (int i = 0; i < data_to_read; i += dataset_len) { 735 // Timestamp is at buffer[i] 736 m_dt = ((double) buffer[i] - previous_timestamp) / 1000000.0; 737 738 /* 739 * System.out.println(((toInt(buffer[i + 3], buffer[i + 4], buffer[i + 5], 740 * buffer[i + 6]))*delta_angle_sf) / ((10000.0 / (buffer[i] - 741 * previous_timestamp)) / 100.0)); 742 * // DEBUG: Plot Sub-Array Data in Terminal 743 * for (int j = 0; j < data_to_read; j++) { 744 * System.out.print(buffer[j]); 745 * System.out.print(" ,"); 746 * } 747 * System.out.println(" "); 748 * //System.out.println(((toInt(buffer[i + 3], buffer[i + 4], buffer[i + 5], 749 * buffer[i + 6]))*delta_angle_sf) / ((10000.0 / (buffer[i] - 750 * previous_timestamp)) / 100.0) + "," + buffer[3] + "," + buffer[4] + "," + 751 * buffer[5] + "," + buffer[6] 752 * /*toShort(buffer[7], buffer[8]) + "," + 753 * toShort(buffer[9], buffer[10]) + "," + 754 * toShort(buffer[11], buffer[12]) + "," + 755 * toShort(buffer[13], buffer[14]) + "," + 756 * toShort(buffer[15], buffer[16]) + "," 757 * + toShort(buffer[17], buffer[18])); 758 */ 759 760 /* 761 * Get delta angle value for selected yaw axis and scale by the elapsed time 762 * (based on timestamp) 763 */ 764 delta_angle = 765 (toInt(buffer[i + 3], buffer[i + 4], buffer[i + 5], buffer[i + 6]) * delta_angle_sf) 766 / (m_scaled_sample_rate / (buffer[i] - previous_timestamp)); 767 gyro_rate_x = (toShort(buffer[i + 7], buffer[i + 8]) / 10.0); 768 gyro_rate_y = (toShort(buffer[i + 9], buffer[i + 10]) / 10.0); 769 gyro_rate_z = (toShort(buffer[i + 11], buffer[i + 12]) / 10.0); 770 accel_x = (toShort(buffer[i + 13], buffer[i + 14]) / 800.0); 771 accel_y = (toShort(buffer[i + 15], buffer[i + 16]) / 800.0); 772 accel_z = (toShort(buffer[i + 17], buffer[i + 18]) / 800.0); 773 774 // Convert scaled sensor data to SI units (for tilt calculations) 775 // TODO: Should the unit outputs be selectable? 776 gyro_rate_x_si = gyro_rate_x * deg_to_rad; 777 gyro_rate_y_si = gyro_rate_y * deg_to_rad; 778 gyro_rate_z_si = gyro_rate_z * deg_to_rad; 779 accel_x_si = accel_x * grav; 780 accel_y_si = accel_y * grav; 781 accel_z_si = accel_z * grav; 782 783 // Store timestamp for next iteration 784 previous_timestamp = buffer[i]; 785 786 m_alpha = m_tau / (m_tau + m_dt); 787 788 if (m_first_run) { 789 // Set up inclinometer calculations for first run 790 accelAngleX = 791 Math.atan2( 792 accel_x_si, Math.sqrt((accel_y_si * accel_y_si) + (accel_z_si * accel_z_si))); 793 accelAngleY = 794 Math.atan2( 795 accel_y_si, Math.sqrt((accel_x_si * accel_x_si) + (accel_z_si * accel_z_si))); 796 compAngleX = accelAngleX; 797 compAngleY = accelAngleY; 798 } else { 799 // Run inclinometer calculations 800 accelAngleX = 801 Math.atan2( 802 accel_x_si, Math.sqrt((accel_y_si * accel_y_si) + (accel_z_si * accel_z_si))); 803 accelAngleY = 804 Math.atan2( 805 accel_y_si, Math.sqrt((accel_x_si * accel_x_si) + (accel_z_si * accel_z_si))); 806 accelAngleX = formatAccelRange(accelAngleX, accel_z_si); 807 accelAngleY = formatAccelRange(accelAngleY, accel_z_si); 808 compAngleX = compFilterProcess(compAngleX, accelAngleX, -gyro_rate_y_si); 809 compAngleY = compFilterProcess(compAngleY, accelAngleY, gyro_rate_x_si); 810 } 811 812 synchronized (this) { 813 /* Push data to global variables */ 814 if (m_first_run) { 815 /* 816 * Don't accumulate first run. previous_timestamp will be "very" old and the 817 * integration will end up way off 818 */ 819 m_integ_angle = 0.0; 820 } else { 821 m_integ_angle += delta_angle; 822 } 823 m_gyro_rate_x = gyro_rate_x; 824 m_gyro_rate_y = gyro_rate_y; 825 m_gyro_rate_z = gyro_rate_z; 826 m_accel_x = accel_x; 827 m_accel_y = accel_y; 828 m_accel_z = accel_z; 829 m_compAngleX = compAngleX * rad_to_deg; 830 m_compAngleY = compAngleY * rad_to_deg; 831 m_accelAngleX = accelAngleX * rad_to_deg; 832 m_accelAngleY = accelAngleY * rad_to_deg; 833 } 834 m_first_run = false; 835 } 836 } else { 837 m_thread_idle = true; 838 data_count = 0; 839 data_remainder = 0; 840 data_to_read = 0; 841 previous_timestamp = 0.0; 842 delta_angle = 0.0; 843 gyro_rate_x = 0.0; 844 gyro_rate_y = 0.0; 845 gyro_rate_z = 0.0; 846 accel_x = 0.0; 847 accel_y = 0.0; 848 accel_z = 0.0; 849 gyro_rate_x_si = 0.0; 850 gyro_rate_y_si = 0.0; 851 gyro_rate_z_si = 0.0; 852 accel_x_si = 0.0; 853 accel_y_si = 0.0; 854 accel_z_si = 0.0; 855 compAngleX = 0.0; 856 compAngleY = 0.0; 857 accelAngleX = 0.0; 858 accelAngleY = 0.0; 859 } 860 } 861 } 862 863 /** 864 * @param compAngle 865 * @param accAngle 866 * @return 867 */ 868 private double formatFastConverge(double compAngle, double accAngle) { 869 if (compAngle > accAngle + Math.PI) { 870 compAngle = compAngle - 2.0 * Math.PI; 871 } else if (accAngle > compAngle + Math.PI) { 872 compAngle = compAngle + 2.0 * Math.PI; 873 } 874 return compAngle; 875 } 876 877 /** 878 * @param compAngle 879 * @return 880 */ 881 private double formatRange0to2PI(double compAngle) { 882 while (compAngle >= 2 * Math.PI) { 883 compAngle = compAngle - 2.0 * Math.PI; 884 } 885 while (compAngle < 0.0) { 886 compAngle = compAngle + 2.0 * Math.PI; 887 } 888 return compAngle; 889 } 890 891 /** 892 * @param accelAngle 893 * @param accelZ 894 * @return 895 */ 896 private double formatAccelRange(double accelAngle, double accelZ) { 897 if (accelZ < 0.0) { 898 accelAngle = Math.PI - accelAngle; 899 } else if (accelZ > 0.0 && accelAngle < 0.0) { 900 accelAngle = 2.0 * Math.PI + accelAngle; 901 } 902 return accelAngle; 903 } 904 905 /** 906 * @param compAngle 907 * @param accelAngle 908 * @param omega 909 * @return 910 */ 911 private double compFilterProcess(double compAngle, double accelAngle, double omega) { 912 compAngle = formatFastConverge(compAngle, accelAngle); 913 compAngle = m_alpha * (compAngle + omega * m_dt) + (1.0 - m_alpha) * accelAngle; 914 compAngle = formatRange0to2PI(compAngle); 915 if (compAngle > Math.PI) { 916 compAngle = compAngle - 2.0 * Math.PI; 917 } 918 return compAngle; 919 } 920 921 /** @return Yaw axis angle in degrees (CCW positive) */ 922 public synchronized double getAngle() { 923 switch (m_yaw_axis) { 924 case kX: 925 if (m_simGyroAngleX != null) { 926 return m_simGyroAngleX.get(); 927 } 928 break; 929 case kY: 930 if (m_simGyroAngleY != null) { 931 return m_simGyroAngleY.get(); 932 } 933 break; 934 case kZ: 935 if (m_simGyroAngleZ != null) { 936 return m_simGyroAngleZ.get(); 937 } 938 break; 939 } 940 return m_integ_angle; 941 } 942 943 /** @return Yaw axis angular rate in degrees per second (CCW positive) */ 944 public synchronized double getRate() { 945 if (m_yaw_axis == IMUAxis.kX) { 946 if (m_simGyroRateX != null) { 947 return m_simGyroRateX.get(); 948 } 949 return m_gyro_rate_x; 950 } else if (m_yaw_axis == IMUAxis.kY) { 951 if (m_simGyroRateY != null) { 952 return m_simGyroRateY.get(); 953 } 954 return m_gyro_rate_y; 955 } else if (m_yaw_axis == IMUAxis.kZ) { 956 if (m_simGyroRateZ != null) { 957 return m_simGyroRateZ.get(); 958 } 959 return m_gyro_rate_z; 960 } else { 961 return 0.0; 962 } 963 } 964 965 /** @return Yaw Axis */ 966 public IMUAxis getYawAxis() { 967 return m_yaw_axis; 968 } 969 970 /** @return current acceleration in the X axis */ 971 public synchronized double getAccelX() { 972 return m_accel_x * 9.81; 973 } 974 975 /** @return current acceleration in the Y axis */ 976 public synchronized double getAccelY() { 977 return m_accel_y * 9.81; 978 } 979 980 /** @return current acceleration in the Z axis */ 981 public synchronized double getAccelZ() { 982 return m_accel_z * 9.81; 983 } 984 985 /** @return X axis complementary angle */ 986 public synchronized double getXComplementaryAngle() { 987 return m_compAngleX; 988 } 989 990 /** @return Y axis complementary angle */ 991 public synchronized double getYComplementaryAngle() { 992 return m_compAngleY; 993 } 994 995 /** @return X axis filtered acceleration angle */ 996 public synchronized double getXFilteredAccelAngle() { 997 return m_accelAngleX; 998 } 999 1000 /** @return Y axis filtered acceleration angle */ 1001 public synchronized double getYFilteredAccelAngle() { 1002 return m_accelAngleY; 1003 } 1004 1005 /** 1006 * Get the SPI port number. 1007 * 1008 * @return The SPI port number. 1009 */ 1010 public int getPort() { 1011 return m_spi_port.value; 1012 } 1013 1014 @Override 1015 public void initSendable(NTSendableBuilder builder) { 1016 builder.setSmartDashboardType("Gyro"); 1017 builder.addDoubleProperty("Value", this::getAngle, null); 1018 } 1019}