Source code

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