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