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