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.networktables.NTSendable;
022import edu.wpi.first.networktables.NTSendableBuilder;
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, NTSendable {
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 int value;
112
113    private 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  public ADIS16448_IMU(final IMUAxis yaw_axis, SPI.Port port, CalibrationTime cal_time) {
261    m_yaw_axis = yaw_axis;
262    m_spi_port = port;
263
264    m_acquire_task = new Thread(new AcquireTask(this));
265
266    m_simDevice = SimDevice.create("Gyro:ADIS16448", port.value);
267    if (m_simDevice != null) {
268      m_simConnected = m_simDevice.createBoolean("connected", SimDevice.Direction.kInput, true);
269      m_simGyroAngleX = m_simDevice.createDouble("gyro_angle_x", SimDevice.Direction.kInput, 0.0);
270      m_simGyroAngleY = m_simDevice.createDouble("gyro_angle_y", SimDevice.Direction.kInput, 0.0);
271      m_simGyroAngleZ = m_simDevice.createDouble("gyro_angle_z", SimDevice.Direction.kInput, 0.0);
272      m_simGyroRateX = m_simDevice.createDouble("gyro_rate_x", SimDevice.Direction.kInput, 0.0);
273      m_simGyroRateY = m_simDevice.createDouble("gyro_rate_y", SimDevice.Direction.kInput, 0.0);
274      m_simGyroRateZ = m_simDevice.createDouble("gyro_rate_z", SimDevice.Direction.kInput, 0.0);
275      m_simAccelX = m_simDevice.createDouble("accel_x", SimDevice.Direction.kInput, 0.0);
276      m_simAccelY = m_simDevice.createDouble("accel_y", SimDevice.Direction.kInput, 0.0);
277      m_simAccelZ = m_simDevice.createDouble("accel_z", SimDevice.Direction.kInput, 0.0);
278    }
279
280    if (m_simDevice == null) {
281      // Force the IMU reset pin to toggle on startup (doesn't require DS enable)
282      m_reset_out = new DigitalOutput(18); // Drive MXP DIO8 low
283      Timer.delay(0.01); // Wait 10ms
284      m_reset_out.close();
285      m_reset_in = new DigitalInput(18); // Set MXP DIO8 high
286      Timer.delay(0.25); // Wait 250ms
287
288      configCalTime(cal_time);
289
290      if (!switchToStandardSPI()) {
291        return;
292      }
293
294      // Set IMU internal decimation to 819.2 SPS
295      writeRegister(SMPL_PRD, 0x0001);
296      // Enable Data Ready (LOW = Good Data) on DIO1 (PWM0 on MXP)
297      writeRegister(MSC_CTRL, 0x0016);
298      // Disable IMU internal Bartlett filter
299      writeRegister(SENS_AVG, 0x0400);
300      // Clear offset registers
301      writeRegister(XGYRO_OFF, 0x0000);
302      writeRegister(YGYRO_OFF, 0x0000);
303      writeRegister(ZGYRO_OFF, 0x0000);
304
305      // Configure standard SPI
306      if (!switchToAutoSPI()) {
307        return;
308      }
309      // Notify DS that IMU calibration delay is active
310      DriverStation.reportWarning(
311          "ADIS16448 IMU Detected. Starting initial calibration delay.", false);
312      // Wait for whatever time the user set as the start-up delay
313      try {
314        Thread.sleep((long) (m_calibration_time.value * 1.2 * 1000));
315      } catch (InterruptedException e) {
316      }
317      // Execute calibration routine
318      calibrate();
319      // Reset accumulated offsets
320      reset();
321      // Indicate to the acquire loop that we're done starting up
322      m_start_up_mode = false;
323      // Let the user know the IMU was initiallized successfully
324      DriverStation.reportWarning("ADIS16448 IMU Successfully Initialized!", false);
325      // Drive MXP PWM5 (IMU ready LED) low (active low)
326      m_status_led = new DigitalOutput(19);
327    }
328
329    // Report usage and post data to DS
330    HAL.report(tResourceType.kResourceType_ADIS16448, 0);
331    m_connected = true;
332  }
333
334  public boolean isConnected() {
335    if (m_simConnected != null) {
336      return m_simConnected.get();
337    }
338    return m_connected;
339  }
340
341  /** */
342  private static int toUShort(byte[] buf) {
343    return (((buf[0] & 0xFF) << 8) + ((buf[1] & 0xFF) << 0));
344  }
345
346  private static int toUByte(int... buf) {
347    return (buf[0] & 0xFF);
348  }
349
350  public static int toUShort(int... buf) {
351    return (((buf[0] & 0xFF) << 8) + (buf[1] & 0xFF));
352  }
353
354  /** */
355  private static long toULong(int sint) {
356    return sint & 0x00000000FFFFFFFFL;
357  }
358
359  /** */
360  private static int toShort(int... buf) {
361    return (short) (((buf[0] & 0xFF) << 8) + ((buf[1] & 0xFF) << 0));
362  }
363
364  /** */
365  private static int toInt(int... buf) {
366    return (buf[0] & 0xFF) << 24 | (buf[1] & 0xFF) << 16 | (buf[2] & 0xFF) << 8 | (buf[3] & 0xFF);
367  }
368
369  /** */
370  private boolean switchToStandardSPI() {
371    // Check to see whether the acquire thread is active. If so, wait for it to stop
372    // producing data.
373    if (m_thread_active) {
374      m_thread_active = false;
375      while (!m_thread_idle) {
376        try {
377          Thread.sleep(10);
378        } catch (InterruptedException e) {
379        }
380      }
381      System.out.println("Paused the IMU processing thread successfully!");
382      // Maybe we're in auto SPI mode? If so, kill auto SPI, and then SPI.
383      if (m_spi != null && m_auto_configured) {
384        m_spi.stopAuto();
385        // We need to get rid of all the garbage left in the auto SPI buffer after
386        // stopping it.
387        // Sometimes data magically reappears, so we have to check the buffer size a
388        // couple of times
389        // to be sure we got it all. Yuck.
390        int[] trashBuffer = new int[200];
391        try {
392          Thread.sleep(100);
393        } catch (InterruptedException e) {
394        }
395        int data_count = m_spi.readAutoReceivedData(trashBuffer, 0, 0);
396        while (data_count > 0) {
397          /* Dequeue 200 at a time, or the remainder of the buffer if less than 200 */
398          m_spi.readAutoReceivedData(trashBuffer, Math.min(200, data_count), 0);
399          /* Update remaining buffer count */
400          data_count = m_spi.readAutoReceivedData(trashBuffer, 0, 0);
401        }
402        System.out.println("Paused auto SPI successfully.");
403      }
404    }
405    // There doesn't seem to be a SPI port active. Let's try to set one up
406    if (m_spi == null) {
407      System.out.println("Setting up a new SPI port.");
408      m_spi = new SPI(m_spi_port);
409      m_spi.setClockRate(1000000);
410      m_spi.setMode(SPI.Mode.kMode3);
411      m_spi.setChipSelectActiveLow();
412      readRegister(PROD_ID); // Dummy read
413
414      // Validate the product ID
415      if (readRegister(PROD_ID) != 16448) {
416        DriverStation.reportError("Could not find ADIS16448", false);
417        close();
418        return false;
419      }
420      return true;
421    } else {
422      // Maybe the SPI port is active, but not in auto SPI mode? Try to read the
423      // product ID.
424      readRegister(PROD_ID); // dummy read
425      if (readRegister(PROD_ID) != 16448) {
426        DriverStation.reportError("Could not find an ADIS16448", false);
427        close();
428        return false;
429      } else {
430        return true;
431      }
432    }
433  }
434
435  /** */
436  boolean switchToAutoSPI() {
437    // No SPI port has been set up. Go set one up first.
438    if (m_spi == null) {
439      if (!switchToStandardSPI()) {
440        DriverStation.reportError("Failed to start/restart auto SPI", false);
441        return false;
442      }
443    }
444    // Only set up the interrupt if needed.
445    if (m_auto_interrupt == null) {
446      m_auto_interrupt = new DigitalInput(10); // MXP DIO0
447    }
448    // The auto SPI controller gets angry if you try to set up two instances on one
449    // bus.
450    if (!m_auto_configured) {
451      m_spi.initAuto(8200);
452      m_auto_configured = true;
453    }
454    // Set auto SPI packet data and size
455    m_spi.setAutoTransmitData(new byte[] {GLOB_CMD}, 27);
456    // Configure auto stall time
457    m_spi.configureAutoStall(100, 1000, 255);
458    // Kick off auto SPI (Note: Device configuration impossible after auto SPI is
459    // activated)
460    m_spi.startAutoTrigger(m_auto_interrupt, true, false);
461
462    // Check to see if the acquire thread is running. If not, kick one off.
463    if (!m_acquire_task.isAlive()) {
464      m_first_run = true;
465      m_thread_active = true;
466      m_acquire_task.start();
467      System.out.println("New IMU Processing thread activated!");
468    } else {
469      // The thread was running, re-init run variables and start it up again.
470      m_first_run = true;
471      m_thread_active = true;
472      System.out.println("Old IMU Processing thread re-activated!");
473    }
474    // Looks like the thread didn't start for some reason. Abort.
475    if (!m_acquire_task.isAlive()) {
476      DriverStation.reportError("Failed to start/restart the acquire() thread.", false);
477      close();
478      return false;
479    }
480    return true;
481  }
482
483  public int configDecRate(int m_decRate) {
484    int writeValue = m_decRate;
485    int readbackValue;
486    if (!switchToStandardSPI()) {
487      DriverStation.reportError("Failed to configure/reconfigure standard SPI.", false);
488      return 2;
489    }
490
491    /* Check max */
492    if (m_decRate > 9) {
493      DriverStation.reportError(
494          "Attempted to write an invalid decimation value. Capping at 9", false);
495      m_decRate = 9;
496    }
497    if (m_decRate < 0) {
498      DriverStation.reportError(
499          "Attempted to write an invalid decimation value. Capping at 0", false);
500      m_decRate = 0;
501    }
502
503    /* Shift decimation setting to correct position and select internal sync */
504    writeValue = (m_decRate << 8) | 0x1;
505
506    /* Apply to IMU */
507    writeRegister(SMPL_PRD, writeValue);
508
509    /* Perform read back to verify write */
510    readbackValue = readRegister(SMPL_PRD);
511
512    /* Throw error for invalid write */
513    if (readbackValue != writeValue) {
514      DriverStation.reportError("ADIS16448 SMPL_PRD write failed.", false);
515    }
516
517    if (!switchToAutoSPI()) {
518      DriverStation.reportError("Failed to configure/reconfigure auto SPI.", false);
519      return 2;
520    }
521    return 0;
522  }
523
524  /**
525   * Configures calibration time
526   *
527   * @param new_cal_time New calibration time
528   * @return 1 if the new calibration time is the same as the current one else 0
529   */
530  public int configCalTime(CalibrationTime new_cal_time) {
531    if (m_calibration_time == new_cal_time) {
532      return 1;
533    } else {
534      m_calibration_time = new_cal_time;
535      m_avg_size = m_calibration_time.value * 819;
536      initOffsetBuffer(m_avg_size);
537      return 0;
538    }
539  }
540
541  private void initOffsetBuffer(int size) {
542    // Avoid exceptions in the case of bad arguments
543    if (size < 1) {
544      size = 1;
545    }
546    // Set average size to size (correct bad values)
547    m_avg_size = size;
548    synchronized (this) {
549      // Resize vector
550      m_offset_data_gyro_rate_x = new double[size];
551      m_offset_data_gyro_rate_y = new double[size];
552      m_offset_data_gyro_rate_z = new double[size];
553      // Set accumulate count to 0
554      m_accum_count = 0;
555    }
556  }
557
558  /**
559   * Calibrate the gyro. It's important to make sure that the robot is not moving while the
560   * calibration is in progress, this is typically done when the robot is first turned on while it's
561   * sitting at rest before the match starts.
562   */
563  public void calibrate() {
564    synchronized (this) {
565      int gyroAverageSize = Math.min(m_accum_count, m_avg_size);
566      double accum_gyro_rate_x = 0.0;
567      double accum_gyro_rate_y = 0.0;
568      double accum_gyro_rate_z = 0.0;
569      for (int i = 0; i < gyroAverageSize; i++) {
570        accum_gyro_rate_x += m_offset_data_gyro_rate_x[i];
571        accum_gyro_rate_y += m_offset_data_gyro_rate_y[i];
572        accum_gyro_rate_z += m_offset_data_gyro_rate_z[i];
573      }
574      m_gyro_rate_offset_x = accum_gyro_rate_x / gyroAverageSize;
575      m_gyro_rate_offset_y = accum_gyro_rate_y / gyroAverageSize;
576      m_gyro_rate_offset_z = accum_gyro_rate_z / gyroAverageSize;
577      m_integ_gyro_angle_x = 0.0;
578      m_integ_gyro_angle_y = 0.0;
579      m_integ_gyro_angle_z = 0.0;
580      // System.out.println("Avg Size: " + gyroAverageSize + "X Off: " +
581      // m_gyro_rate_offset_x + "Y Off: " + m_gyro_rate_offset_y + "Z Off: " +
582      // m_gyro_rate_offset_z);
583    }
584  }
585
586  /**
587   * Sets the yaw axis
588   *
589   * @param yaw_axis The new yaw axis to use
590   * @return 1 if the new yaw axis is the same as the current one else 0.
591   */
592  public int setYawAxis(IMUAxis yaw_axis) {
593    if (m_yaw_axis == yaw_axis) {
594      return 1;
595    }
596    m_yaw_axis = yaw_axis;
597    reset();
598    return 0;
599  }
600
601  private int readRegister(final int reg) {
602    // ByteBuffer buf = ByteBuffer.allocateDirect(2);
603    final byte[] buf = new byte[2];
604    // buf.order(ByteOrder.BIG_ENDIAN);
605    buf[0] = (byte) (reg & 0x7f);
606    buf[1] = (byte) 0;
607
608    m_spi.write(buf, 2);
609    m_spi.read(false, buf, 2);
610
611    return toUShort(buf);
612  }
613
614  private void writeRegister(final int reg, final int val) {
615    final byte[] buf = new byte[2];
616    // low byte
617    buf[0] = (byte) (0x80 | reg);
618    buf[1] = (byte) (val & 0xff);
619    m_spi.write(buf, 2);
620    // high byte
621    buf[0] = (byte) (0x81 | reg);
622    buf[1] = (byte) (val >> 8);
623    m_spi.write(buf, 2);
624  }
625
626  public void reset() {
627    synchronized (this) {
628      m_integ_gyro_angle_x = 0.0;
629      m_integ_gyro_angle_y = 0.0;
630      m_integ_gyro_angle_z = 0.0;
631    }
632  }
633
634  /** Delete (free) the spi port used for the IMU. */
635  @Override
636  public void close() {
637    if (m_thread_active) {
638      m_thread_active = false;
639      try {
640        if (m_acquire_task != null) {
641          m_acquire_task.join();
642          m_acquire_task = null;
643        }
644      } catch (InterruptedException e) {
645      }
646      if (m_spi != null) {
647        if (m_auto_configured) {
648          m_spi.stopAuto();
649        }
650        m_spi.close();
651        m_auto_configured = false;
652        if (m_auto_interrupt != null) {
653          m_auto_interrupt.close();
654          m_auto_interrupt = null;
655        }
656        m_spi = null;
657      }
658    }
659    System.out.println("Finished cleaning up after the IMU driver.");
660  }
661
662  /** */
663  private void acquire() {
664    // Set data packet length
665    final int dataset_len = 29; // 18 data points + timestamp
666    final int BUFFER_SIZE = 4000;
667
668    // Set up buffers and variables
669    int[] buffer = new int[BUFFER_SIZE];
670    int data_count = 0;
671    int data_remainder = 0;
672    int data_to_read = 0;
673    int bufferAvgIndex = 0;
674    double previous_timestamp = 0.0;
675    double delta_angle = 0.0;
676    double gyro_rate_x = 0.0;
677    double gyro_rate_y = 0.0;
678    double gyro_rate_z = 0.0;
679    double accel_x = 0.0;
680    double accel_y = 0.0;
681    double accel_z = 0.0;
682    double mag_x = 0.0;
683    double mag_y = 0.0;
684    double mag_z = 0.0;
685    double baro = 0.0;
686    double temp = 0.0;
687    double gyro_rate_x_si = 0.0;
688    double gyro_rate_y_si = 0.0;
689    double gyro_rate_z_si = 0.0;
690    double accel_x_si = 0.0;
691    double accel_y_si = 0.0;
692    double accel_z_si = 0.0;
693    double compAngleX = 0.0;
694    double compAngleY = 0.0;
695    double accelAngleX = 0.0;
696    double accelAngleY = 0.0;
697
698    while (true) {
699      // Sleep loop for 5ms
700      try {
701        Thread.sleep(5);
702      } catch (InterruptedException e) {
703      }
704
705      if (m_thread_active) {
706        m_thread_idle = false;
707
708        data_count =
709            m_spi.readAutoReceivedData(
710                buffer, 0, 0); // Read number of bytes currently stored in the buffer
711        data_remainder =
712            data_count % dataset_len; // Check if frame is incomplete. Add 1 because of timestamp
713        data_to_read = data_count - data_remainder; // Remove incomplete data from read count
714        if (data_to_read > BUFFER_SIZE) {
715          DriverStation.reportWarning(
716              "ADIS16448 data processing thread overrun has occurred!", false);
717          data_to_read = BUFFER_SIZE - (BUFFER_SIZE % dataset_len);
718        }
719        m_spi.readAutoReceivedData(
720            buffer, data_to_read, 0); // Read data from DMA buffer (only complete sets)
721
722        // Could be multiple data sets in the buffer. Handle each one.
723        for (int i = 0; i < data_to_read; i += dataset_len) {
724          // Calculate CRC-16 on each data packet
725          int calc_crc = 0x0000FFFF; // Starting word
726          int read_byte = 0;
727          int imu_crc = 0;
728          for (int k = 5;
729              k < 27;
730              k += 2) { // Cycle through XYZ GYRO, XYZ ACCEL, XYZ MAG, BARO, TEMP (Ignore Status &
731            // CRC)
732            read_byte = buffer[i + k + 1]; // Process LSB
733            calc_crc = (calc_crc >>> 8) ^ adiscrc[(calc_crc & 0x000000FF) ^ read_byte];
734            read_byte = buffer[i + k]; // Process MSB
735            calc_crc = (calc_crc >>> 8) ^ adiscrc[(calc_crc & 0x000000FF) ^ read_byte];
736          }
737          calc_crc = ~calc_crc & 0xFFFF; // Complement
738          calc_crc = ((calc_crc << 8) | (calc_crc >> 8)) & 0xFFFF; // Flip LSB & MSB
739          imu_crc = toUShort(buffer[i + 27], buffer[i + 28]); // Extract DUT CRC from data buffer
740
741          if (calc_crc == imu_crc) {
742            // Timestamp is at buffer[i]
743            m_dt = ((double) buffer[i] - previous_timestamp) / 1000000.0;
744
745            // Scale sensor data
746            gyro_rate_x = (toShort(buffer[i + 5], buffer[i + 6]) * 0.04);
747            gyro_rate_y = (toShort(buffer[i + 7], buffer[i + 8]) * 0.04);
748            gyro_rate_z = (toShort(buffer[i + 9], buffer[i + 10]) * 0.04);
749            accel_x = (toShort(buffer[i + 11], buffer[i + 12]) * 0.833);
750            accel_y = (toShort(buffer[i + 13], buffer[i + 14]) * 0.833);
751            accel_z = (toShort(buffer[i + 15], buffer[i + 16]) * 0.833);
752            mag_x = (toShort(buffer[i + 17], buffer[i + 18]) * 0.1429);
753            mag_y = (toShort(buffer[i + 19], buffer[i + 20]) * 0.1429);
754            mag_z = (toShort(buffer[i + 21], buffer[i + 22]) * 0.1429);
755            baro = (toShort(buffer[i + 23], buffer[i + 24]) * 0.02);
756            temp = (toShort(buffer[i + 25], buffer[i + 26]) * 0.07386 + 31.0);
757
758            // Convert scaled sensor data to SI units (for tilt calculations)
759            // TODO: Should the unit outputs be selectable?
760            gyro_rate_x_si = gyro_rate_x * deg_to_rad;
761            gyro_rate_y_si = gyro_rate_y * deg_to_rad;
762            gyro_rate_z_si = gyro_rate_z * deg_to_rad;
763            accel_x_si = accel_x * grav;
764            accel_y_si = accel_y * grav;
765            accel_z_si = accel_z * grav;
766            // Store timestamp for next iteration
767            previous_timestamp = buffer[i];
768            // Calculate alpha for use with the complementary filter
769            m_alpha = m_tau / (m_tau + m_dt);
770            // Calculate complementary filter
771            if (m_first_run) {
772              // Set up inclinometer calculations for first run
773              accelAngleX =
774                  Math.atan2(
775                      -accel_x_si,
776                      Math.sqrt((accel_y_si * accel_y_si) + (-accel_z_si * -accel_z_si)));
777              accelAngleY =
778                  Math.atan2(
779                      accel_y_si,
780                      Math.sqrt((-accel_x_si * -accel_x_si) + (-accel_z_si * -accel_z_si)));
781              compAngleX = accelAngleX;
782              compAngleY = accelAngleY;
783            } else {
784              // Run inclinometer calculations
785              accelAngleX =
786                  Math.atan2(
787                      -accel_x_si,
788                      Math.sqrt((accel_y_si * accel_y_si) + (-accel_z_si * -accel_z_si)));
789              accelAngleY =
790                  Math.atan2(
791                      accel_y_si,
792                      Math.sqrt((-accel_x_si * -accel_x_si) + (-accel_z_si * -accel_z_si)));
793              accelAngleX = formatAccelRange(accelAngleX, -accel_z_si);
794              accelAngleY = formatAccelRange(accelAngleY, -accel_z_si);
795              compAngleX = compFilterProcess(compAngleX, accelAngleX, -gyro_rate_y_si);
796              compAngleY = compFilterProcess(compAngleY, accelAngleY, -gyro_rate_x_si);
797            }
798
799            // Update global variables and state
800            synchronized (this) {
801              // Ignore first, integrated sample
802              if (m_first_run) {
803                m_integ_gyro_angle_x = 0.0;
804                m_integ_gyro_angle_y = 0.0;
805                m_integ_gyro_angle_z = 0.0;
806              } else {
807                // Accumulate gyro for offset calibration
808                // Add to buffer
809                bufferAvgIndex = m_accum_count % m_avg_size;
810                m_offset_data_gyro_rate_x[bufferAvgIndex] = gyro_rate_x;
811                m_offset_data_gyro_rate_y[bufferAvgIndex] = gyro_rate_y;
812                m_offset_data_gyro_rate_z[bufferAvgIndex] = gyro_rate_z;
813                // Increment counter
814                m_accum_count++;
815              }
816              if (!m_start_up_mode) {
817                m_gyro_rate_x = gyro_rate_x;
818                m_gyro_rate_y = gyro_rate_y;
819                m_gyro_rate_z = gyro_rate_z;
820                m_accel_x = accel_x;
821                m_accel_y = accel_y;
822                m_accel_z = accel_z;
823                m_mag_x = mag_x;
824                m_mag_y = mag_y;
825                m_mag_z = mag_z;
826                m_baro = baro;
827                m_temp = temp;
828                m_compAngleX = compAngleX * rad_to_deg;
829                m_compAngleY = compAngleY * rad_to_deg;
830                m_accelAngleX = accelAngleX * rad_to_deg;
831                m_accelAngleY = accelAngleY * rad_to_deg;
832                // Accumulate gyro for angle integration and publish to global variables
833                m_integ_gyro_angle_x += (gyro_rate_x - m_gyro_rate_offset_x) * m_dt;
834                m_integ_gyro_angle_y += (gyro_rate_y - m_gyro_rate_offset_y) * m_dt;
835                m_integ_gyro_angle_z += (gyro_rate_z - m_gyro_rate_offset_z) * m_dt;
836              }
837              // System.out.println("Good CRC");
838            }
839            m_first_run = false;
840          } else {
841            // System.out.println("Bad CRC");
842            /*
843             * System.out.println("Calc CRC: " + calc_crc);
844             * System.out.println("IMU CRC: " + imu_crc);
845             * System.out.println(
846             * buffer[i] + " " +
847             * (buffer[i + 1]) + " " + (buffer[i + 2]) + " " +
848             * (buffer[i + 3]) + " " + (buffer[i + 4]) + " " +
849             * (buffer[i + 5]) + " " + (buffer[i + 6]) + " " +
850             * (buffer[i + 7]) + " " + (buffer[i + 8]) + " " +
851             * (buffer[i + 9]) + " " + (buffer[i + 10]) + " " +
852             * (buffer[i + 11]) + " " + (buffer[i + 12]) + " " +
853             * (buffer[i + 13]) + " " + (buffer[i + 14]) + " " +
854             * (buffer[i + 15]) + " " + (buffer[i + 16]) + " " +
855             * (buffer[i + 17]) + " " + (buffer[i + 18]) + " " +
856             * (buffer[i + 19]) + " " + (buffer[i + 20]) + " " +
857             * (buffer[i + 21]) + " " + (buffer[i + 22]) + " " +
858             * (buffer[i + 23]) + " " + (buffer[i + 24]) + " " +
859             * (buffer[i + 25]) + " " + (buffer[i + 26]) + " " +
860             * (buffer[i + 27]) + " " + (buffer[i + 28]));
861             */
862          }
863        }
864      } else {
865        m_thread_idle = true;
866        data_count = 0;
867        data_remainder = 0;
868        data_to_read = 0;
869        previous_timestamp = 0.0;
870        delta_angle = 0.0;
871        gyro_rate_x = 0.0;
872        gyro_rate_y = 0.0;
873        gyro_rate_z = 0.0;
874        accel_x = 0.0;
875        accel_y = 0.0;
876        accel_z = 0.0;
877        mag_x = 0.0;
878        mag_y = 0.0;
879        mag_z = 0.0;
880        baro = 0.0;
881        temp = 0.0;
882        gyro_rate_x_si = 0.0;
883        gyro_rate_y_si = 0.0;
884        gyro_rate_z_si = 0.0;
885        accel_x_si = 0.0;
886        accel_y_si = 0.0;
887        accel_z_si = 0.0;
888        compAngleX = 0.0;
889        compAngleY = 0.0;
890        accelAngleX = 0.0;
891        accelAngleY = 0.0;
892      }
893    }
894  }
895
896  /**
897   * @param compAngle
898   * @param accAngle
899   * @return
900   */
901  private double formatFastConverge(double compAngle, double accAngle) {
902    if (compAngle > accAngle + Math.PI) {
903      compAngle = compAngle - 2.0 * Math.PI;
904    } else if (accAngle > compAngle + Math.PI) {
905      compAngle = compAngle + 2.0 * Math.PI;
906    }
907    return compAngle;
908  }
909
910  /**
911   * @param compAngle
912   * @return
913   */
914  private double formatRange0to2PI(double compAngle) {
915    while (compAngle >= 2 * Math.PI) {
916      compAngle = compAngle - 2.0 * Math.PI;
917    }
918    while (compAngle < 0.0) {
919      compAngle = compAngle + 2.0 * Math.PI;
920    }
921    return compAngle;
922  }
923
924  /**
925   * @param accelAngle
926   * @param accelZ
927   * @return
928   */
929  private double formatAccelRange(double accelAngle, double accelZ) {
930    if (accelZ < 0.0) {
931      accelAngle = Math.PI - accelAngle;
932    } else if (accelZ > 0.0 && accelAngle < 0.0) {
933      accelAngle = 2.0 * Math.PI + accelAngle;
934    }
935    return accelAngle;
936  }
937
938  /**
939   * @param compAngle
940   * @param accelAngle
941   * @param omega
942   * @return
943   */
944  private double compFilterProcess(double compAngle, double accelAngle, double omega) {
945    compAngle = formatFastConverge(compAngle, accelAngle);
946    compAngle = m_alpha * (compAngle + omega * m_dt) + (1.0 - m_alpha) * accelAngle;
947    compAngle = formatRange0to2PI(compAngle);
948    if (compAngle > Math.PI) {
949      compAngle = compAngle - 2.0 * Math.PI;
950    }
951    return compAngle;
952  }
953
954  /**
955   * @return Yaw axis angle in degrees (CCW positive)
956   */
957  public synchronized double getAngle() {
958    switch (m_yaw_axis) {
959      case kX:
960        return getGyroAngleX();
961      case kY:
962        return getGyroAngleY();
963      case kZ:
964        return getGyroAngleZ();
965      default:
966        return 0.0;
967    }
968  }
969
970  /**
971   * @return Yaw axis angular rate in degrees per second (CCW positive)
972   */
973  public synchronized double getRate() {
974    switch (m_yaw_axis) {
975      case kX:
976        return getGyroRateX();
977      case kY:
978        return getGyroRateY();
979      case kZ:
980        return getGyroRateZ();
981      default:
982        return 0.0;
983    }
984  }
985
986  /**
987   * @return Yaw Axis
988   */
989  public IMUAxis getYawAxis() {
990    return m_yaw_axis;
991  }
992
993  /**
994   * @return accumulated gyro angle in the X axis in degrees
995   */
996  public synchronized double getGyroAngleX() {
997    if (m_simGyroAngleX != null) {
998      return m_simGyroAngleX.get();
999    }
1000    return m_integ_gyro_angle_x;
1001  }
1002
1003  /**
1004   * @return accumulated gyro angle in the Y axis in degrees
1005   */
1006  public synchronized double getGyroAngleY() {
1007    if (m_simGyroAngleY != null) {
1008      return m_simGyroAngleY.get();
1009    }
1010    return m_integ_gyro_angle_y;
1011  }
1012
1013  /**
1014   * @return accumulated gyro angle in the Z axis in degrees
1015   */
1016  public synchronized double getGyroAngleZ() {
1017    if (m_simGyroAngleZ != null) {
1018      return m_simGyroAngleZ.get();
1019    }
1020    return m_integ_gyro_angle_z;
1021  }
1022
1023  /**
1024   * @return gyro angular rate in the X axis in degrees per second
1025   */
1026  public synchronized double getGyroRateX() {
1027    if (m_simGyroRateX != null) {
1028      return m_simGyroRateX.get();
1029    }
1030    return m_gyro_rate_x;
1031  }
1032
1033  /**
1034   * @return gyro angular rate in the Y axis in degrees per second
1035   */
1036  public synchronized double getGyroRateY() {
1037    if (m_simGyroRateY != null) {
1038      return m_simGyroRateY.get();
1039    }
1040    return m_gyro_rate_y;
1041  }
1042
1043  /**
1044   * @return gyro angular rate in the Z axis in degrees per second
1045   */
1046  public synchronized double getGyroRateZ() {
1047    if (m_simGyroRateZ != null) {
1048      return m_simGyroRateZ.get();
1049    }
1050    return m_gyro_rate_z;
1051  }
1052
1053  /**
1054   * @return urrent acceleration in the X axis in meters per second squared
1055   */
1056  public synchronized double getAccelX() {
1057    if (m_simAccelX != null) {
1058      return m_simAccelX.get();
1059    }
1060    return m_accel_x * 9.81;
1061  }
1062
1063  /**
1064   * @return current acceleration in the Y axis in meters per second squared
1065   */
1066  public synchronized double getAccelY() {
1067    if (m_simAccelY != null) {
1068      return m_simAccelY.get();
1069    }
1070    return m_accel_y * 9.81;
1071  }
1072
1073  /**
1074   * @return current acceleration in the Z axis in meters per second squared
1075   */
1076  public synchronized double getAccelZ() {
1077    if (m_simAccelZ != null) {
1078      return m_simAccelZ.get();
1079    }
1080    return m_accel_z * 9.81;
1081  }
1082
1083  /**
1084   * @return Magnetic field strength in the X axis in Tesla
1085   */
1086  public synchronized double getMagneticFieldX() {
1087    // mG to T
1088    return m_mag_x * 1e-7;
1089  }
1090
1091  /**
1092   * @return Magnetic field strength in the Y axis in Tesla
1093   */
1094  public synchronized double getMagneticFieldY() {
1095    // mG to T
1096    return m_mag_y * 1e-7;
1097  }
1098
1099  /**
1100   * @return Magnetic field strength in the Z axis in Tesla
1101   */
1102  public synchronized double getMagneticFieldZ() {
1103    // mG to T
1104    return m_mag_z * 1e-7;
1105  }
1106
1107  /**
1108   * @return X-axis complementary angle in degrees
1109   */
1110  public synchronized double getXComplementaryAngle() {
1111    return m_compAngleX;
1112  }
1113
1114  /**
1115   * @return Y-axis complementary angle in degrees
1116   */
1117  public synchronized double getYComplementaryAngle() {
1118    return m_compAngleY;
1119  }
1120
1121  /**
1122   * @return X-axis filtered acceleration angle in degrees
1123   */
1124  public synchronized double getXFilteredAccelAngle() {
1125    return m_accelAngleX;
1126  }
1127
1128  /**
1129   * @return Y-axis filtered acceleration angle in degrees
1130   */
1131  public synchronized double getYFilteredAccelAngle() {
1132    return m_accelAngleY;
1133  }
1134
1135  /**
1136   * @return Barometric Pressure in PSI
1137   */
1138  public synchronized double getBarometricPressure() {
1139    // mbar to PSI conversion
1140    return m_baro * 0.0145;
1141  }
1142
1143  /**
1144   * @return Temperature in degrees Celsius
1145   */
1146  public synchronized double getTemperature() {
1147    return m_temp;
1148  }
1149
1150  /**
1151   * Get the SPI port number.
1152   *
1153   * @return The SPI port number.
1154   */
1155  public int getPort() {
1156    return m_spi_port.value;
1157  }
1158
1159  @Override
1160  public void initSendable(NTSendableBuilder builder) {
1161    builder.setSmartDashboardType("Gyro");
1162    builder.addDoubleProperty("Value", this::getAngle, null);
1163  }
1164}