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.networktables.NTSendable; 021import edu.wpi.first.networktables.NTSendableBuilder; 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, NTSendable { 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 int value; 197 198 private 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 public ADIS16470_IMU(IMUAxis yaw_axis, SPI.Port port, CalibrationTime cal_time) { 291 m_yaw_axis = yaw_axis; 292 m_calibration_time = cal_time.value; 293 m_spi_port = port; 294 295 m_acquire_task = new Thread(new AcquireTask(this)); 296 297 m_simDevice = SimDevice.create("Gyro:ADIS16470", port.value); 298 if (m_simDevice != null) { 299 m_simConnected = m_simDevice.createBoolean("connected", SimDevice.Direction.kInput, true); 300 m_simGyroAngleX = m_simDevice.createDouble("gyro_angle_x", SimDevice.Direction.kInput, 0.0); 301 m_simGyroAngleY = m_simDevice.createDouble("gyro_angle_y", SimDevice.Direction.kInput, 0.0); 302 m_simGyroAngleZ = m_simDevice.createDouble("gyro_angle_z", SimDevice.Direction.kInput, 0.0); 303 m_simGyroRateX = m_simDevice.createDouble("gyro_rate_x", SimDevice.Direction.kInput, 0.0); 304 m_simGyroRateY = m_simDevice.createDouble("gyro_rate_y", SimDevice.Direction.kInput, 0.0); 305 m_simGyroRateZ = m_simDevice.createDouble("gyro_rate_z", SimDevice.Direction.kInput, 0.0); 306 m_simAccelX = m_simDevice.createDouble("accel_x", SimDevice.Direction.kInput, 0.0); 307 m_simAccelY = m_simDevice.createDouble("accel_y", SimDevice.Direction.kInput, 0.0); 308 m_simAccelZ = m_simDevice.createDouble("accel_z", SimDevice.Direction.kInput, 0.0); 309 } 310 311 if (m_simDevice == null) { 312 // Force the IMU reset pin to toggle on startup (doesn't require DS enable) 313 // Relies on the RIO hardware by default configuring an output as low 314 // and configuring an input as high Z. The 10k pull-up resistor internal to the 315 // IMU then forces the reset line high for normal operation. 316 m_reset_out = new DigitalOutput(27); // Drive SPI CS2 (IMU RST) low 317 Timer.delay(0.01); // Wait 10ms 318 m_reset_out.close(); 319 m_reset_in = new DigitalInput(27); // Set SPI CS2 (IMU RST) high 320 Timer.delay(0.25); // Wait 250ms for reset to complete 321 322 if (!switchToStandardSPI()) { 323 return; 324 } 325 326 // Set IMU internal decimation to 4 (output data rate of 2000 SPS / (4 + 1) = 327 // 400Hz) 328 writeRegister(DEC_RATE, 4); 329 330 // Set data ready polarity (HIGH = Good Data), Disable gSense Compensation and 331 // PoP 332 writeRegister(MSC_CTRL, 1); 333 334 // Configure IMU internal Bartlett filter 335 writeRegister(FILT_CTRL, 0); 336 337 // Configure continuous bias calibration time based on user setting 338 writeRegister(NULL_CNFG, (m_calibration_time | 0x0700)); 339 340 // Notify DS that IMU calibration delay is active 341 DriverStation.reportWarning( 342 "ADIS16470 IMU Detected. Starting initial calibration delay.", false); 343 344 // Wait for samples to accumulate internal to the IMU (110% of user-defined 345 // time) 346 try { 347 Thread.sleep((long) (Math.pow(2.0, m_calibration_time) / 2000 * 64 * 1.1 * 1000)); 348 } catch (InterruptedException e) { 349 } 350 351 // Write offset calibration command to IMU 352 writeRegister(GLOB_CMD, 0x0001); 353 354 // Configure and enable auto SPI 355 if (!switchToAutoSPI()) { 356 return; 357 } 358 359 // Let the user know the IMU was initiallized successfully 360 DriverStation.reportWarning("ADIS16470 IMU Successfully Initialized!", false); 361 362 // Drive "Ready" LED low 363 m_status_led = new DigitalOutput(28); // Set SPI CS3 (IMU Ready LED) low 364 } 365 366 // Report usage and post data to DS 367 HAL.report(tResourceType.kResourceType_ADIS16470, 0); 368 m_connected = true; 369 } 370 371 public boolean isConnected() { 372 if (m_simConnected != null) { 373 return m_simConnected.get(); 374 } 375 return m_connected; 376 } 377 378 /** 379 * @param buf 380 * @return 381 */ 382 private static int toUShort(ByteBuffer buf) { 383 return (buf.getShort(0)) & 0xFFFF; 384 } 385 386 /** 387 * @param sint 388 * @return 389 */ 390 private static long toULong(int sint) { 391 return sint & 0x00000000FFFFFFFFL; 392 } 393 394 /** 395 * @param buf 396 * @return 397 */ 398 private static int toShort(int... buf) { 399 return (short) (((buf[0] & 0xFF) << 8) + ((buf[1] & 0xFF) << 0)); 400 } 401 402 /** 403 * @param buf 404 * @return 405 */ 406 private static int toInt(int... buf) { 407 return (buf[0] & 0xFF) << 24 | (buf[1] & 0xFF) << 16 | (buf[2] & 0xFF) << 8 | (buf[3] & 0xFF); 408 } 409 410 /** 411 * Switch to standard SPI mode. 412 * 413 * @return 414 */ 415 private boolean switchToStandardSPI() { 416 // Check to see whether the acquire thread is active. If so, wait for it to stop 417 // producing data. 418 if (m_thread_active) { 419 m_thread_active = false; 420 while (!m_thread_idle) { 421 try { 422 Thread.sleep(10); 423 } catch (InterruptedException e) { 424 } 425 } 426 System.out.println("Paused the IMU processing thread successfully!"); 427 // Maybe we're in auto SPI mode? If so, kill auto SPI, and then SPI. 428 if (m_spi != null && m_auto_configured) { 429 m_spi.stopAuto(); 430 // We need to get rid of all the garbage left in the auto SPI buffer after 431 // stopping it. 432 // Sometimes data magically reappears, so we have to check the buffer size a 433 // couple of times 434 // to be sure we got it all. Yuck. 435 int[] trashBuffer = new int[200]; 436 try { 437 Thread.sleep(100); 438 } catch (InterruptedException e) { 439 } 440 int data_count = m_spi.readAutoReceivedData(trashBuffer, 0, 0); 441 while (data_count > 0) { 442 m_spi.readAutoReceivedData(trashBuffer, Math.min(data_count, 200), 0); 443 data_count = m_spi.readAutoReceivedData(trashBuffer, 0, 0); 444 } 445 System.out.println("Paused auto SPI successfully."); 446 } 447 } 448 // There doesn't seem to be a SPI port active. Let's try to set one up 449 if (m_spi == null) { 450 System.out.println("Setting up a new SPI port."); 451 m_spi = new SPI(m_spi_port); 452 m_spi.setClockRate(2000000); 453 m_spi.setMode(SPI.Mode.kMode3); 454 m_spi.setChipSelectActiveLow(); 455 readRegister(PROD_ID); // Dummy read 456 457 // Validate the product ID 458 if (readRegister(PROD_ID) != 16982) { 459 DriverStation.reportError("Could not find ADIS16470", false); 460 close(); 461 return false; 462 } 463 return true; 464 } else { 465 // Maybe the SPI port is active, but not in auto SPI mode? Try to read the 466 // product ID. 467 readRegister(PROD_ID); // dummy read 468 if (readRegister(PROD_ID) != 16982) { 469 DriverStation.reportError("Could not find an ADIS16470", false); 470 close(); 471 return false; 472 } else { 473 return true; 474 } 475 } 476 } 477 478 /** 479 * @return 480 */ 481 boolean switchToAutoSPI() { 482 // No SPI port has been set up. Go set one up first. 483 if (m_spi == null) { 484 if (!switchToStandardSPI()) { 485 DriverStation.reportError("Failed to start/restart auto SPI", false); 486 return false; 487 } 488 } 489 // Only set up the interrupt if needed. 490 if (m_auto_interrupt == null) { 491 // Configure interrupt on SPI CS1 492 m_auto_interrupt = new DigitalInput(26); 493 } 494 // The auto SPI controller gets angry if you try to set up two instances on one 495 // bus. 496 if (!m_auto_configured) { 497 m_spi.initAuto(8200); 498 m_auto_configured = true; 499 } 500 // Do we need to change auto SPI settings? 501 switch (m_yaw_axis) { 502 case kX: 503 m_spi.setAutoTransmitData(m_autospi_x_packet, 2); 504 break; 505 case kY: 506 m_spi.setAutoTransmitData(m_autospi_y_packet, 2); 507 break; 508 default: 509 m_spi.setAutoTransmitData(m_autospi_z_packet, 2); 510 break; 511 } 512 // Configure auto stall time 513 m_spi.configureAutoStall(5, 1000, 1); 514 // Kick off auto SPI (Note: Device configuration impossible after auto SPI is 515 // activated) 516 // DR High = Data good (data capture should be triggered on the rising edge) 517 m_spi.startAutoTrigger(m_auto_interrupt, true, false); 518 519 // Check to see if the acquire thread is running. If not, kick one off. 520 if (!m_acquire_task.isAlive()) { 521 m_first_run = true; 522 m_thread_active = true; 523 m_acquire_task.start(); 524 System.out.println("Processing thread activated!"); 525 } else { 526 // The thread was running, re-init run variables and start it up again. 527 m_first_run = true; 528 m_thread_active = true; 529 System.out.println("Processing thread activated!"); 530 } 531 // Looks like the thread didn't start for some reason. Abort. 532 if (!m_acquire_task.isAlive()) { 533 DriverStation.reportError("Failed to start/restart the acquire() thread.", false); 534 close(); 535 return false; 536 } 537 return true; 538 } 539 540 /** 541 * Configures calibration time 542 * 543 * @param new_cal_time New calibration time 544 * @return 1 if the new calibration time is the same as the current one else 0 545 */ 546 public int configCalTime(CalibrationTime new_cal_time) { 547 if (m_calibration_time == new_cal_time.value) { 548 return 1; 549 } 550 if (!switchToStandardSPI()) { 551 DriverStation.reportError("Failed to configure/reconfigure standard SPI.", false); 552 return 2; 553 } 554 m_calibration_time = new_cal_time.value; 555 writeRegister(NULL_CNFG, (m_calibration_time | 0x700)); 556 if (!switchToAutoSPI()) { 557 DriverStation.reportError("Failed to configure/reconfigure auto SPI.", false); 558 return 2; 559 } 560 return 0; 561 } 562 563 public int configDecRate(int reg) { 564 int m_reg = reg; 565 if (!switchToStandardSPI()) { 566 DriverStation.reportError("Failed to configure/reconfigure standard SPI.", false); 567 return 2; 568 } 569 if (m_reg > 1999) { 570 DriverStation.reportError("Attempted to write an invalid deimation value.", false); 571 m_reg = 1999; 572 } 573 m_scaled_sample_rate = (((m_reg + 1.0) / 2000.0) * 1000000.0); 574 writeRegister(DEC_RATE, m_reg); 575 System.out.println("Decimation register: " + readRegister(DEC_RATE)); 576 if (!switchToAutoSPI()) { 577 DriverStation.reportError("Failed to configure/reconfigure auto SPI.", false); 578 return 2; 579 } 580 return 0; 581 } 582 583 /** 584 * Calibrate the gyro. It's important to make sure that the robot is not moving while the 585 * calibration is in progress, this is typically done when the robot is first turned on while it's 586 * sitting at rest before the match starts. 587 */ 588 public void calibrate() { 589 if (!switchToStandardSPI()) { 590 DriverStation.reportError("Failed to configure/reconfigure standard SPI.", false); 591 } 592 writeRegister(GLOB_CMD, 0x0001); 593 if (!switchToAutoSPI()) { 594 DriverStation.reportError("Failed to configure/reconfigure auto SPI.", false); 595 } 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 System.out.println("Finished cleaning up after the IMU driver."); 685 } 686 687 /** */ 688 private void acquire() { 689 // Set data packet length 690 final int dataset_len = 19; // 18 data points + timestamp 691 final int BUFFER_SIZE = 4000; 692 693 // Set up buffers and variables 694 int[] buffer = new int[BUFFER_SIZE]; 695 int data_count = 0; 696 int data_remainder = 0; 697 int data_to_read = 0; 698 double previous_timestamp = 0.0; 699 double delta_angle = 0.0; 700 double gyro_rate_x = 0.0; 701 double gyro_rate_y = 0.0; 702 double gyro_rate_z = 0.0; 703 double accel_x = 0.0; 704 double accel_y = 0.0; 705 double accel_z = 0.0; 706 double gyro_rate_x_si = 0.0; 707 double gyro_rate_y_si = 0.0; 708 double gyro_rate_z_si = 0.0; 709 double accel_x_si = 0.0; 710 double accel_y_si = 0.0; 711 double accel_z_si = 0.0; 712 double compAngleX = 0.0; 713 double compAngleY = 0.0; 714 double accelAngleX = 0.0; 715 double accelAngleY = 0.0; 716 717 while (true) { 718 // Sleep loop for 10ms 719 try { 720 Thread.sleep(10); 721 } catch (InterruptedException e) { 722 } 723 724 if (m_thread_active) { 725 m_thread_idle = false; 726 727 data_count = 728 m_spi.readAutoReceivedData( 729 buffer, 0, 0); // Read number of bytes currently stored in the 730 // buffer 731 data_remainder = 732 data_count % dataset_len; // Check if frame is incomplete. Add 1 because of timestamp 733 data_to_read = data_count - data_remainder; // Remove incomplete data from read count 734 /* Want to cap the data to read in a single read at the buffer size */ 735 if (data_to_read > BUFFER_SIZE) { 736 DriverStation.reportWarning( 737 "ADIS16470 data processing thread overrun has occurred!", false); 738 data_to_read = BUFFER_SIZE - (BUFFER_SIZE % dataset_len); 739 } 740 m_spi.readAutoReceivedData( 741 buffer, data_to_read, 0); // Read data from DMA buffer (only complete sets) 742 743 // Could be multiple data sets in the buffer. Handle each one. 744 for (int i = 0; i < data_to_read; i += dataset_len) { 745 // Timestamp is at buffer[i] 746 m_dt = ((double) buffer[i] - previous_timestamp) / 1000000.0; 747 748 /* 749 * System.out.println(((toInt(buffer[i + 3], buffer[i + 4], buffer[i + 5], 750 * buffer[i + 6]))*delta_angle_sf) / ((10000.0 / (buffer[i] - 751 * previous_timestamp)) / 100.0)); 752 * // DEBUG: Plot Sub-Array Data in Terminal 753 * for (int j = 0; j < data_to_read; j++) { 754 * System.out.print(buffer[j]); 755 * System.out.print(" ,"); 756 * } 757 * System.out.println(" "); 758 * //System.out.println(((toInt(buffer[i + 3], buffer[i + 4], buffer[i + 5], 759 * buffer[i + 6]))*delta_angle_sf) / ((10000.0 / (buffer[i] - 760 * previous_timestamp)) / 100.0) + "," + buffer[3] + "," + buffer[4] + "," + 761 * buffer[5] + "," + buffer[6] 762 * /*toShort(buffer[7], buffer[8]) + "," + 763 * toShort(buffer[9], buffer[10]) + "," + 764 * toShort(buffer[11], buffer[12]) + "," + 765 * toShort(buffer[13], buffer[14]) + "," + 766 * toShort(buffer[15], buffer[16]) + "," 767 * + toShort(buffer[17], buffer[18])); 768 */ 769 770 /* 771 * Get delta angle value for selected yaw axis and scale by the elapsed time 772 * (based on timestamp) 773 */ 774 delta_angle = 775 (toInt(buffer[i + 3], buffer[i + 4], buffer[i + 5], buffer[i + 6]) * delta_angle_sf) 776 / (m_scaled_sample_rate / (buffer[i] - previous_timestamp)); 777 gyro_rate_x = (toShort(buffer[i + 7], buffer[i + 8]) / 10.0); 778 gyro_rate_y = (toShort(buffer[i + 9], buffer[i + 10]) / 10.0); 779 gyro_rate_z = (toShort(buffer[i + 11], buffer[i + 12]) / 10.0); 780 accel_x = (toShort(buffer[i + 13], buffer[i + 14]) / 800.0); 781 accel_y = (toShort(buffer[i + 15], buffer[i + 16]) / 800.0); 782 accel_z = (toShort(buffer[i + 17], buffer[i + 18]) / 800.0); 783 784 // Convert scaled sensor data to SI units (for tilt calculations) 785 // TODO: Should the unit outputs be selectable? 786 gyro_rate_x_si = gyro_rate_x * deg_to_rad; 787 gyro_rate_y_si = gyro_rate_y * deg_to_rad; 788 gyro_rate_z_si = gyro_rate_z * deg_to_rad; 789 accel_x_si = accel_x * grav; 790 accel_y_si = accel_y * grav; 791 accel_z_si = accel_z * grav; 792 793 // Store timestamp for next iteration 794 previous_timestamp = buffer[i]; 795 796 m_alpha = m_tau / (m_tau + m_dt); 797 798 if (m_first_run) { 799 // Set up inclinometer calculations for first run 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 compAngleX = accelAngleX; 807 compAngleY = accelAngleY; 808 } else { 809 // Run inclinometer calculations 810 accelAngleX = 811 Math.atan2( 812 accel_x_si, Math.sqrt((accel_y_si * accel_y_si) + (accel_z_si * accel_z_si))); 813 accelAngleY = 814 Math.atan2( 815 accel_y_si, Math.sqrt((accel_x_si * accel_x_si) + (accel_z_si * accel_z_si))); 816 accelAngleX = formatAccelRange(accelAngleX, accel_z_si); 817 accelAngleY = formatAccelRange(accelAngleY, accel_z_si); 818 compAngleX = compFilterProcess(compAngleX, accelAngleX, -gyro_rate_y_si); 819 compAngleY = compFilterProcess(compAngleY, accelAngleY, gyro_rate_x_si); 820 } 821 822 synchronized (this) { 823 /* Push data to global variables */ 824 if (m_first_run) { 825 /* 826 * Don't accumulate first run. previous_timestamp will be "very" old and the 827 * integration will end up way off 828 */ 829 m_integ_angle = 0.0; 830 } else { 831 m_integ_angle += delta_angle; 832 } 833 m_gyro_rate_x = gyro_rate_x; 834 m_gyro_rate_y = gyro_rate_y; 835 m_gyro_rate_z = gyro_rate_z; 836 m_accel_x = accel_x; 837 m_accel_y = accel_y; 838 m_accel_z = accel_z; 839 m_compAngleX = compAngleX * rad_to_deg; 840 m_compAngleY = compAngleY * rad_to_deg; 841 m_accelAngleX = accelAngleX * rad_to_deg; 842 m_accelAngleY = accelAngleY * rad_to_deg; 843 } 844 m_first_run = false; 845 } 846 } else { 847 m_thread_idle = true; 848 data_count = 0; 849 data_remainder = 0; 850 data_to_read = 0; 851 previous_timestamp = 0.0; 852 delta_angle = 0.0; 853 gyro_rate_x = 0.0; 854 gyro_rate_y = 0.0; 855 gyro_rate_z = 0.0; 856 accel_x = 0.0; 857 accel_y = 0.0; 858 accel_z = 0.0; 859 gyro_rate_x_si = 0.0; 860 gyro_rate_y_si = 0.0; 861 gyro_rate_z_si = 0.0; 862 accel_x_si = 0.0; 863 accel_y_si = 0.0; 864 accel_z_si = 0.0; 865 compAngleX = 0.0; 866 compAngleY = 0.0; 867 accelAngleX = 0.0; 868 accelAngleY = 0.0; 869 } 870 } 871 } 872 873 /** 874 * @param compAngle 875 * @param accAngle 876 * @return 877 */ 878 private double formatFastConverge(double compAngle, double accAngle) { 879 if (compAngle > accAngle + Math.PI) { 880 compAngle = compAngle - 2.0 * Math.PI; 881 } else if (accAngle > compAngle + Math.PI) { 882 compAngle = compAngle + 2.0 * Math.PI; 883 } 884 return compAngle; 885 } 886 887 /** 888 * @param compAngle 889 * @return 890 */ 891 private double formatRange0to2PI(double compAngle) { 892 while (compAngle >= 2 * Math.PI) { 893 compAngle = compAngle - 2.0 * Math.PI; 894 } 895 while (compAngle < 0.0) { 896 compAngle = compAngle + 2.0 * Math.PI; 897 } 898 return compAngle; 899 } 900 901 /** 902 * @param accelAngle 903 * @param accelZ 904 * @return 905 */ 906 private double formatAccelRange(double accelAngle, double accelZ) { 907 if (accelZ < 0.0) { 908 accelAngle = Math.PI - accelAngle; 909 } else if (accelZ > 0.0 && accelAngle < 0.0) { 910 accelAngle = 2.0 * Math.PI + accelAngle; 911 } 912 return accelAngle; 913 } 914 915 /** 916 * @param compAngle 917 * @param accelAngle 918 * @param omega 919 * @return 920 */ 921 private double compFilterProcess(double compAngle, double accelAngle, double omega) { 922 compAngle = formatFastConverge(compAngle, accelAngle); 923 compAngle = m_alpha * (compAngle + omega * m_dt) + (1.0 - m_alpha) * accelAngle; 924 compAngle = formatRange0to2PI(compAngle); 925 if (compAngle > Math.PI) { 926 compAngle = compAngle - 2.0 * Math.PI; 927 } 928 return compAngle; 929 } 930 931 /** 932 * @return Yaw axis angle in degrees (CCW positive) 933 */ 934 public synchronized double getAngle() { 935 switch (m_yaw_axis) { 936 case kX: 937 if (m_simGyroAngleX != null) { 938 return m_simGyroAngleX.get(); 939 } 940 break; 941 case kY: 942 if (m_simGyroAngleY != null) { 943 return m_simGyroAngleY.get(); 944 } 945 break; 946 case kZ: 947 if (m_simGyroAngleZ != null) { 948 return m_simGyroAngleZ.get(); 949 } 950 break; 951 } 952 return m_integ_angle; 953 } 954 955 /** 956 * @return Yaw axis angular rate in degrees per second (CCW positive) 957 */ 958 public synchronized double getRate() { 959 if (m_yaw_axis == IMUAxis.kX) { 960 if (m_simGyroRateX != null) { 961 return m_simGyroRateX.get(); 962 } 963 return m_gyro_rate_x; 964 } else if (m_yaw_axis == IMUAxis.kY) { 965 if (m_simGyroRateY != null) { 966 return m_simGyroRateY.get(); 967 } 968 return m_gyro_rate_y; 969 } else if (m_yaw_axis == IMUAxis.kZ) { 970 if (m_simGyroRateZ != null) { 971 return m_simGyroRateZ.get(); 972 } 973 return m_gyro_rate_z; 974 } else { 975 return 0.0; 976 } 977 } 978 979 /** 980 * @return Yaw Axis 981 */ 982 public IMUAxis getYawAxis() { 983 return m_yaw_axis; 984 } 985 986 /** 987 * @return current acceleration in the X axis 988 */ 989 public synchronized double getAccelX() { 990 return m_accel_x * 9.81; 991 } 992 993 /** 994 * @return current acceleration in the Y axis 995 */ 996 public synchronized double getAccelY() { 997 return m_accel_y * 9.81; 998 } 999 1000 /** 1001 * @return current acceleration in the Z axis 1002 */ 1003 public synchronized double getAccelZ() { 1004 return m_accel_z * 9.81; 1005 } 1006 1007 /** 1008 * @return X-axis complementary angle 1009 */ 1010 public synchronized double getXComplementaryAngle() { 1011 return m_compAngleX; 1012 } 1013 1014 /** 1015 * @return Y-axis complementary angle 1016 */ 1017 public synchronized double getYComplementaryAngle() { 1018 return m_compAngleY; 1019 } 1020 1021 /** 1022 * @return X-axis filtered acceleration angle 1023 */ 1024 public synchronized double getXFilteredAccelAngle() { 1025 return m_accelAngleX; 1026 } 1027 1028 /** 1029 * @return Y-axis filtered acceleration angle 1030 */ 1031 public synchronized double getYFilteredAccelAngle() { 1032 return m_accelAngleY; 1033 } 1034 1035 /** 1036 * Get the SPI port number. 1037 * 1038 * @return The SPI port number. 1039 */ 1040 public int getPort() { 1041 return m_spi_port.value; 1042 } 1043 1044 @Override 1045 public void initSendable(NTSendableBuilder builder) { 1046 builder.setSmartDashboardType("Gyro"); 1047 builder.addDoubleProperty("Value", this::getAngle, null); 1048 } 1049}