Commit | Line | Data |
---|---|---|
5fbefcd5 E |
1 | // GyroITG3200 I2C device class file |
2 | // Based on InvenSense ITG-3200 datasheet rev. 1.4, 3/30/2010 (PS-ITG-3200A-00-01.4) | |
3 | // Original work by 7/31/2011 by Jeff Rowberg <jeff@rowberg.net> | |
1e039ebd | 4 | // Java implementation for First Robotics Competition Team 2521 using WPILibj |
5fbefcd5 E |
5 | // 1/27/2015 by Joe Bussell <joe dot bussell at gmail dot com> |
6 | // Updates should (hopefully) always be available at https://github.com/bussell | |
7 | // | |
8 | // Changelog: | |
9 | // 2011-07-31 - initial release | |
10 | // 2015-01-30 - Java FRC revision | |
11 | ||
12 | /* ============================================ | |
13 | GyroITG3200 device library code is placed under the MIT license | |
14 | Copyright (c) 2011 by Jeff Rowberg | |
15 | Copyright (c) 2015 Joe Bussell | |
16 | Permission is hereby granted, free of charge, to any person obtaining a copy | |
17 | of this software and associated documentation files (the "Software"), to deal | |
18 | in the Software without restriction, including without limitation the rights | |
19 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | |
20 | copies of the Software, and to permit persons to whom the Software is | |
21 | furnished to do so, subject to the following conditions: | |
22 | The above copyright notice and this permission notice shall be included in | |
23 | all copies or substantial portions of the Software. | |
24 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | |
25 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | |
26 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | |
27 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | |
28 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | |
29 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | |
30 | THE SOFTWARE. | |
31 | =============================================== | |
32 | */ | |
33 | ||
34 | package org.usfirst.frc.team3501.robot; | |
35 | ||
36 | import java.util.Arrays; | |
37 | ||
38 | import edu.wpi.first.wpilibj.DriverStation; | |
39 | import edu.wpi.first.wpilibj.I2C; | |
40 | import edu.wpi.first.wpilibj.PIDSource; | |
41 | import edu.wpi.first.wpilibj.PIDSourceType; | |
42 | import edu.wpi.first.wpilibj.SensorBase; | |
43 | import edu.wpi.first.wpilibj.livewindow.LiveWindow; | |
44 | import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable; | |
45 | import edu.wpi.first.wpilibj.tables.ITable; | |
46 | ||
47 | /** | |
48 | * @author Joe Bussell Team 2521 Mentor With thanks to the c++ version authors | |
49 | * at: https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/ITG3200 | |
50 | * | |
51 | */ | |
a6c9c627 | 52 | public class GyroClass extends SensorBase |
5fbefcd5 E |
53 | implements PIDSource, LiveWindowSendable { |
54 | int devAddr; | |
55 | byte buffer[] = new byte[7]; | |
56 | ||
57 | public static final boolean DEBUG = true; | |
58 | ||
59 | I2C m_i2c; | |
60 | ||
61 | /** | |
62 | * Default constructor, uses default I2C address. | |
1e039ebd | 63 | * |
5fbefcd5 E |
64 | * @see ITG3200_DEFAULT_ADDRESS |
65 | */ | |
a6c9c627 | 66 | public GyroClass(I2C.Port port) { |
5fbefcd5 E |
67 | devAddr = ITG3200_DEFAULT_ADDRESS; |
68 | ||
69 | m_i2c = new I2C(port, devAddr); | |
70 | ||
71 | // TODO: This report is incorrect. Need to create instance for I2C ITG3200 | |
72 | // Gyro | |
73 | // UsageReporting.report( tResourceType.kResourceType_I2C, tInstances.?? ); | |
74 | LiveWindow.addSensor("ITG3200_Gyro_I2C", port.getValue(), this); | |
75 | } | |
76 | ||
77 | /** | |
78 | * Specific address constructor. | |
1e039ebd | 79 | * |
5fbefcd5 E |
80 | * @param address |
81 | * I2C address | |
82 | * @see ITG3200_DEFAULT_ADDRESS | |
83 | * @see ITG3200_ADDRESS_AD0_LOW | |
84 | * @see ITG3200_ADDRESS_AD0_HIGH | |
85 | */ | |
a6c9c627 | 86 | public GyroClass(I2C.Port port, byte address) { |
5fbefcd5 E |
87 | devAddr = address; |
88 | ||
89 | m_i2c = new I2C(port, address); | |
90 | ||
91 | // TODO: This report is incorrect. Need to create instance for I2C ITG3200 | |
92 | // Gyro | |
93 | // UsageReporting.report( tResourceType.kResourceType_I2C, tInstances.?? ); | |
94 | LiveWindow.addSensor("ITG3200_Gyro_I2C", port.getValue(), this); | |
95 | } | |
96 | ||
97 | /** | |
98 | * Power on and prepare for general usage. This will activate the gyroscope, | |
99 | * so be sure to adjust the power settings after you call this method if you | |
100 | * want it to enter standby mode, or another less demanding mode of operation. | |
101 | * This also sets the gyroscope to use the X-axis gyro for a clock source. | |
102 | * Note that it doesn't have any delays in the routine, which means you might | |
103 | * want to add ~50ms to be safe if you happen to need to read gyro data | |
104 | * immediately after initialization. The data will flow in either case, but | |
105 | * the first reports may have higher error offsets. | |
106 | */ | |
107 | public void initialize() { | |
108 | if (!testConnection()) { | |
109 | DriverStation.reportError("Test connection failed!", false); | |
110 | } | |
111 | setFullScaleRange(ITG3200_FULLSCALE_2000); | |
112 | setClockSource(ITG3200_CLOCK_PLL_XGYRO); | |
113 | setIntDeviceReadyEnabled(true); | |
114 | setIntDataReadyEnabled(true); | |
115 | } | |
116 | ||
117 | /** | |
118 | * Verify the I2C connection. Make sure the device is connected and responds | |
119 | * as expected. | |
1e039ebd | 120 | * |
5fbefcd5 E |
121 | * @return True if connection is valid, false otherwise |
122 | */ | |
123 | public boolean testConnection() { | |
124 | return getDeviceID() == 0b110100; | |
125 | } | |
126 | ||
127 | private void writeBit(int register, byte bit, boolean value) { | |
128 | byte[] buf = new byte[1]; | |
129 | ReadI2CBuffer(register, 1, buf); | |
130 | byte newValue = (byte) (value ? (buf[0] | (1 << bit)) | |
131 | : (buf[0] & ~(1 << bit))); | |
132 | writeI2CBuffer(register, newValue); | |
133 | ||
134 | if (DEBUG) { | |
135 | ReadI2CBuffer(register, 1, buf); | |
136 | if (newValue != buf[0]) { | |
137 | System.out.println("Expected " + newValue + " seeing " + buf[0]); | |
138 | } | |
139 | } | |
140 | } | |
141 | ||
142 | // this routine should update the original byte with the new data properly | |
143 | // shifted to the correct bit location | |
144 | public static byte updateByte(byte original, int bit, int numBits, | |
145 | byte value) { | |
146 | if (numBits > 7) { | |
147 | throw new IllegalArgumentException( | |
148 | "This routine is intended to use 8-bit bytes. \n Value: " | |
149 | + GetBinaryString(value) + "\n Number bits: " + numBits); | |
150 | } | |
151 | if (bit > 7) { | |
152 | throw new IllegalArgumentException( | |
153 | "This routine is intended to use 8-bit bytes. \n Value: " | |
154 | + GetBinaryString(value) + "\n Bit: " + bit); | |
155 | } | |
156 | if (bit < numBits - 1) { | |
157 | throw new IllegalArgumentException( | |
158 | "This routine is intended to use 8-bit bytes. \n Value: " | |
159 | + GetBinaryString(value) + "\n Bit: " + bit + "\n Number bits: " | |
160 | + numBits); | |
161 | } | |
162 | if (value > Math.pow(2, numBits)) { | |
163 | throw new IllegalArgumentException( | |
164 | "Cannot encode a number this big using the number of bits requested \n Value: " | |
165 | + GetBinaryString(value) + "\n Number bits: " + numBits); | |
166 | } | |
167 | if (bit < 0 || numBits < 0 || value < 0) { | |
168 | throw new IllegalArgumentException( | |
169 | "This routine is intended to use 8-bit bytes. " | |
170 | + "\n All inputs should be greater than 0. " + "\n Value: " | |
171 | + GetBinaryString(value) + "\n Bit: " + bit + "\n Number bits: " | |
172 | + numBits); | |
173 | ||
174 | } | |
175 | byte mask = getMask(bit, numBits); | |
176 | byte maskedOriginal = (byte) ((original & mask) & 0xFF); | |
177 | byte shiftedValue = (byte) ((value << (1 + bit - numBits)) & 0xFF); | |
178 | byte result = (byte) ((shiftedValue | maskedOriginal) & 0xFF); | |
179 | /* | |
180 | * // Debug code System.out.println( "bit = " + bit ); | |
181 | * System.out.println( "num bits = " + numBits ); System.out.println( | |
182 | * "original = " + GetBinaryString(original) ); System.out.println( | |
183 | * " Value = " + GetBinaryString(value) ); System.out.println( "" ); | |
184 | * System.out.println( "mask = " + GetBinaryString(mask) ); | |
185 | * System.out.println( "maskedOriginal = " + GetBinaryString(maskedOriginal) | |
186 | * ); System.out.println( "shifted Value = " + | |
187 | * GetBinaryString(shiftedValue) ); System.out.println( "" ); | |
188 | * System.out.println( "result = " + GetBinaryString(result) ); | |
189 | */ | |
190 | return result; | |
191 | } | |
192 | ||
193 | public static String GetBinaryString(byte value) { | |
194 | return String.format("%8s", Integer.toBinaryString(value & 0xFF)) | |
195 | .replace(' ', '0'); | |
196 | } | |
197 | ||
198 | public boolean writeI2CBuffer(int registerAddress, int data) { | |
199 | boolean retVal = false; | |
200 | try { | |
201 | retVal = m_i2c.write(registerAddress, data); | |
202 | if (DEBUG) { | |
203 | byte[] buf = new byte[1]; | |
204 | ReadI2CBuffer(registerAddress, 1, buf); | |
205 | if (data != buf[0]) { | |
206 | DriverStation.reportError( | |
207 | "Expected " + data + "\nseeing " + buf[0] + "\n", false); | |
208 | } | |
209 | } | |
210 | } catch (Throwable t) { | |
211 | DriverStation.reportError("ERROR Unhandled exception: " + t.toString() | |
212 | + " at " + Arrays.toString(t.getStackTrace()), false); | |
213 | } | |
214 | return retVal; | |
215 | } | |
216 | ||
217 | // | |
218 | // I2Cdev::writeBits(devAddr, ITG3200_RA_WHO_AM_I, ITG3200_DEVID_BIT, | |
219 | // ITG3200_DEVID_LENGTH, id); | |
220 | private void writeBits(int register, int bit, int numBits, byte value) { | |
221 | try { | |
222 | byte[] rawData = new byte[1]; | |
223 | ReadI2CBuffer(register, 1, rawData); | |
224 | byte newValue = updateByte(rawData[0], bit, numBits, value); | |
225 | writeI2CBuffer(register, newValue); | |
226 | } catch (Throwable t) { | |
227 | DriverStation.reportError("ERROR Unhandled exception: " + t.toString() | |
228 | + " at " + Arrays.toString(t.getStackTrace()), false); | |
229 | } | |
230 | } | |
231 | ||
232 | private boolean readBit(int register, byte bit) { | |
233 | byte buf[] = new byte[1]; | |
234 | ReadI2CBuffer(register, 1, buf); | |
235 | return (buf[0] & bit) != 0; | |
236 | } | |
237 | ||
238 | // Get n bits from the byte to form a byte slice | |
239 | private static byte getBits(byte bitField, int bit, int numBits) { | |
240 | ||
241 | if (numBits > 7) { | |
242 | throw new IllegalArgumentException( | |
243 | "This routine is intended to use 8-bit bytes." + "\n Number bits: " | |
244 | + numBits); | |
245 | } | |
246 | if (bit > 7) { | |
247 | throw new IllegalArgumentException( | |
248 | "This routine is intended to use 8-bit bytes. " + "\n Bit: " + bit); | |
249 | } | |
250 | if (bit < numBits - 1) { | |
251 | throw new IllegalArgumentException( | |
252 | "This routine is intended to use 8-bit bytes. " + "\n Bit: " + bit | |
253 | + "\n Number bits: " + numBits); | |
254 | } | |
255 | if (bit < 0 || numBits < 0) { | |
256 | throw new IllegalArgumentException( | |
257 | "This routine is intended to use 8-bit bytes. " | |
258 | + "\n All inputs should be greater than 0. " + "\n Bit: " + bit | |
259 | + "\n Number bits: " + numBits); | |
260 | ||
261 | } | |
262 | byte result = 0; | |
263 | ||
264 | byte mask = (byte) (~getMask(bit, numBits) & 0xFF); | |
265 | byte maskedInput = (byte) ((bitField & mask) & 0xFF); | |
266 | result = (byte) ((maskedInput >>> (1 + bit - numBits)) & 0xFF); | |
267 | ||
268 | /* | |
269 | * // Debug code System.out.println( "mask = " + | |
270 | * GetBinaryString(mask) ); System.out.println( "maskedInput = " + | |
271 | * GetBinaryString(maskedInput) ); System.out.println( "result = " + | |
272 | * GetBinaryString(result) ); | |
273 | */ | |
274 | ||
275 | return result; | |
276 | } | |
277 | ||
278 | // Gets the bit mask for the given bit and number of bits | |
279 | private static byte getMask(int bit, int numBits) { | |
280 | int newMask = 0; | |
281 | for (int i = 0; i <= 7; i++) { | |
282 | if (i > bit || i <= bit - numBits) { | |
283 | // set the mask bit | |
284 | newMask = (int) (newMask + Math.pow(2, i)); | |
285 | } | |
286 | } | |
287 | byte mask = (byte) (newMask & 0xFF); | |
288 | return mask; | |
289 | } | |
290 | ||
291 | private byte getRegisterByte(int register) { | |
292 | byte[] buf = new byte[1]; | |
293 | ReadI2CBuffer(register, 1, buf); | |
294 | return buf[0]; | |
295 | } | |
296 | ||
297 | /** | |
298 | * Get specified bits from the specified register. Form a new value from a | |
299 | * byte (0b10110100) get the 3rd bit request 6 bits and you should get a new | |
300 | * byte (0b00110100). | |
301 | */ | |
302 | private byte getRegisterBits(int register, int bit, int numBits) { | |
303 | byte containingByte = getRegisterByte(register); | |
304 | return getBits(containingByte, bit, numBits); | |
305 | } | |
306 | ||
307 | // WHO_AM_I register | |
308 | /** | |
309 | * Get Device ID. This register is used to verify the identity of the device | |
310 | * (0b110100). | |
1e039ebd | 311 | * |
5fbefcd5 E |
312 | * @return Device ID (should be 0x34, 52 dec, 64 oct) |
313 | * @see ITG3200_RA_WHO_AM_I | |
314 | * @see ITG3200_RA_DEVID_BIT | |
315 | * @see ITG3200_RA_DEVID_LENGTH | |
316 | */ | |
317 | public byte getDeviceID() { | |
318 | return getRegisterBits(ITG3200_RA_WHO_AM_I, ITG3200_DEVID_BIT, | |
319 | ITG3200_DEVID_LENGTH); | |
320 | } | |
321 | ||
322 | /** | |
323 | * Set Device ID. Write a new ID into the WHO_AM_I register (no idea why this | |
324 | * should ever be necessary though). | |
1e039ebd | 325 | * |
5fbefcd5 E |
326 | * @param id |
327 | * New device ID to set. | |
328 | * @see getDeviceID() | |
329 | * @see ITG3200_RA_WHO_AM_I | |
330 | * @see ITG3200_RA_DEVID_BIT | |
331 | * @see ITG3200_RA_DEVID_LENGTH | |
332 | */ | |
333 | public void setDeviceID(byte id) { | |
334 | writeBits(ITG3200_RA_WHO_AM_I, ITG3200_DEVID_BIT, ITG3200_DEVID_LENGTH, id); | |
335 | } | |
336 | ||
337 | // SMPLRT_DIV register | |
338 | /** | |
339 | * Get sample rate. This register determines the sample rate of the ITG-3200 | |
340 | * gyros. The gyros' outputs are sampled internally at either 1kHz or 8kHz, | |
341 | * determined by the DLPF_CFG setting (see register 22). This sampling is then | |
342 | * filtered digitally and delivered into the sensor registers after the number | |
343 | * of cycles determined by this register. The sample rate is given by the | |
344 | * following formula: | |
345 | * | |
346 | * F_sample = F_internal / (divider+1), where F_internal is either 1kHz or | |
347 | * 8kHz | |
348 | * | |
349 | * As an example, if the internal sampling is at 1kHz, then setting this | |
350 | * register to 7 would give the following: | |
351 | * | |
352 | * F_sample = 1kHz / (7 + 1) = 125Hz, or 8ms per sample | |
353 | * | |
354 | * @return Current sample rate | |
355 | * @see setDLPFBandwidth() | |
356 | * @see ITG3200_RA_SMPLRT_DIV | |
357 | */ | |
358 | public byte getRate() { | |
359 | return getRegisterByte(ITG3200_RA_SMPLRT_DIV); | |
360 | } | |
361 | ||
362 | /** | |
363 | * Set sample rate. | |
1e039ebd | 364 | * |
5fbefcd5 E |
365 | * @param rate |
366 | * New sample rate | |
367 | * @see getRate() | |
368 | * @see setDLPFBandwidth() | |
369 | * @see ITG3200_RA_SMPLRT_DIV | |
370 | */ | |
371 | public void setRate(byte rate) { | |
372 | writeI2CBuffer(ITG3200_RA_SMPLRT_DIV, rate); | |
373 | } | |
374 | ||
375 | // DLPF_FS register | |
376 | /** | |
377 | * Full-scale range. The FS_SEL parameter allows setting the full-scale range | |
378 | * of the gyro sensors, as described in the table below. The power-on-reset | |
379 | * value of FS_SEL is 00h. Set to 03h for proper operation. | |
380 | * | |
381 | * 0 = Reserved 1 = Reserved 2 = Reserved 3 = +/- 2000 degrees/sec | |
382 | * | |
383 | * @return Current full-scale range setting | |
384 | * @see ITG3200_FULLSCALE_2000 | |
385 | * @see ITG3200_RA_DLPF_FS | |
386 | * @see ITG3200_DF_FS_SEL_BIT | |
387 | * @see ITG3200_DF_FS_SEL_LENGTH | |
388 | */ | |
389 | public byte getFullScaleRange() { | |
390 | return getRegisterBits(ITG3200_RA_DLPF_FS, ITG3200_DF_FS_SEL_BIT, | |
391 | ITG3200_DF_FS_SEL_LENGTH); | |
392 | } | |
393 | ||
394 | /** | |
395 | * Set full-scale range setting. | |
1e039ebd | 396 | * |
5fbefcd5 E |
397 | * @param range |
398 | * New full-scale range value | |
399 | * @see getFullScaleRange() | |
400 | * @see ITG3200_FULLSCALE_2000 | |
401 | * @see ITG3200_RA_DLPF_FS | |
402 | * @see ITG3200_DF_FS_SEL_BIT | |
403 | * @see ITG3200_DF_FS_SEL_LENGTH | |
404 | */ | |
405 | public void setFullScaleRange(byte range) { | |
406 | writeBits(ITG3200_RA_DLPF_FS, ITG3200_DF_FS_SEL_BIT, | |
407 | ITG3200_DF_FS_SEL_LENGTH, range); | |
408 | } | |
409 | ||
410 | /** | |
411 | * Get digital low-pass filter bandwidth. The DLPF_CFG parameter sets the | |
412 | * digital low pass filter configuration. It also determines the internal | |
413 | * sampling rate used by the device as shown in the table below. | |
414 | * | |
415 | * DLPF_CFG | Low-Pass Filter Bandwidth | Internal Sample Rate | |
416 | * ---------+---------------------------+--------------------- 0 | 256Hz | | |
417 | * 8kHz 1 | 188Hz | 1kHz 2 | 98Hz | 1kHz 3 | 42Hz | 1kHz 4 | 20Hz | 1kHz 5 | | |
418 | * 10Hz | 1kHz 6 | 5Hz | 1kHz 7 | Reserved | Reserved | |
419 | * | |
420 | * @return DLFP bandwidth setting | |
421 | * @see ITG3200_RA_DLPF_FS | |
422 | * @see ITG3200_DF_DLPF_CFG_BIT | |
423 | * @see ITG3200_DF_DLPF_CFG_LENGTH | |
424 | */ | |
425 | public byte getDLPFBandwidth() { | |
426 | return getRegisterBits(ITG3200_RA_DLPF_FS, ITG3200_DF_DLPF_CFG_BIT, | |
427 | ITG3200_DF_DLPF_CFG_LENGTH); | |
428 | } | |
429 | ||
430 | /** | |
431 | * Set digital low-pass filter bandwidth. | |
1e039ebd | 432 | * |
5fbefcd5 E |
433 | * @param bandwidth |
434 | * New DLFP bandwidth setting | |
435 | * @see getDLPFBandwidth() | |
436 | * @see ITG3200_DLPF_BW_256 | |
437 | * @see ITG3200_RA_DLPF_FS | |
438 | * @see ITG3200_DF_DLPF_CFG_BIT | |
439 | * @see ITG3200_DF_DLPF_CFG_LENGTH | |
440 | */ | |
441 | public void setDLPFBandwidth(byte bandwidth) { | |
442 | writeBits(ITG3200_RA_DLPF_FS, ITG3200_DF_DLPF_CFG_BIT, | |
443 | ITG3200_DF_DLPF_CFG_LENGTH, bandwidth); | |
444 | } | |
445 | ||
446 | // INT_CFG register | |
447 | ||
448 | /** | |
449 | * Get interrupt logic level mode. Will be set 0 for active-high, 1 for | |
450 | * active-low. | |
1e039ebd | 451 | * |
5fbefcd5 E |
452 | * @return Current interrupt mode (0=active-high, 1=active-low) |
453 | * @see ITG3200_RA_INT_CFG | |
454 | * @see ITG3200_INTCFG_ACTL_BIT | |
455 | */ | |
456 | public boolean getInterruptMode() { | |
457 | return readBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_ACTL_BIT); | |
458 | } | |
459 | ||
460 | /** | |
461 | * Set interrupt logic level mode. | |
1e039ebd | 462 | * |
5fbefcd5 E |
463 | * @param mode |
464 | * New interrupt mode (0=active-high, 1=active-low) | |
465 | * @see getInterruptMode() | |
466 | * @see ITG3200_RA_INT_CFG | |
467 | * @see ITG3200_INTCFG_ACTL_BIT | |
468 | */ | |
469 | public void setInterruptMode(boolean mode) { | |
470 | writeBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_ACTL_BIT, mode); | |
471 | } | |
472 | ||
473 | /** | |
474 | * Get interrupt drive mode. Will be set 0 for push-pull, 1 for open-drain. | |
1e039ebd | 475 | * |
5fbefcd5 E |
476 | * @return Current interrupt drive mode (0=push-pull, 1=open-drain) |
477 | * @see ITG3200_RA_INT_CFG | |
478 | * @see ITG3200_INTCFG_OPEN_BIT | |
479 | */ | |
480 | public boolean getInterruptDrive() { | |
481 | return readBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_OPEN_BIT); | |
482 | } | |
483 | ||
484 | /** | |
485 | * Set interrupt drive mode. | |
1e039ebd | 486 | * |
5fbefcd5 E |
487 | * @param drive |
488 | * New interrupt drive mode (0=push-pull, 1=open-drain) | |
489 | * @see getInterruptDrive() | |
490 | * @see ITG3200_RA_INT_CFG | |
491 | * @see ITG3200_INTCFG_OPEN_BIT | |
492 | */ | |
493 | public void setInterruptDrive(boolean drive) { | |
494 | writeBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_OPEN_BIT, drive); | |
495 | } | |
496 | ||
497 | /** | |
498 | * Get interrupt latch mode. Will be set 0 for 50us-pulse, 1 for | |
499 | * latch-until-int-cleared. | |
1e039ebd | 500 | * |
5fbefcd5 E |
501 | * @return Current latch mode (0=50us-pulse, 1=latch-until-int-cleared) |
502 | * @see ITG3200_RA_INT_CFG | |
503 | * @see ITG3200_INTCFG_LATCH_INT_EN_BIT | |
504 | */ | |
505 | public boolean getInterruptLatch() { | |
506 | return readBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_LATCH_INT_EN_BIT); | |
507 | } | |
508 | ||
509 | /** | |
510 | * Set interrupt latch mode. | |
1e039ebd | 511 | * |
5fbefcd5 E |
512 | * @param latch |
513 | * New latch mode (0=50us-pulse, 1=latch-until-int-cleared) | |
514 | * @see getInterruptLatch() | |
515 | * @see ITG3200_RA_INT_CFG | |
516 | * @see ITG3200_INTCFG_LATCH_INT_EN_BIT | |
517 | */ | |
518 | public void setInterruptLatch(boolean latch) { | |
519 | writeBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_LATCH_INT_EN_BIT, latch); | |
520 | } | |
521 | ||
522 | /** | |
523 | * Get interrupt latch clear mode. Will be set 0 for status-read-only, 1 for | |
524 | * any-register-read. | |
1e039ebd | 525 | * |
5fbefcd5 E |
526 | * @return Current latch clear mode (0=status-read-only, 1=any-register-read) |
527 | * @see ITG3200_RA_INT_CFG | |
528 | * @see ITG3200_INTCFG_INT_ANYRD_2CLEAR_BIT | |
529 | */ | |
530 | public boolean getInterruptLatchClear() { | |
531 | return readBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_INT_ANYRD_2CLEAR_BIT); | |
532 | } | |
533 | ||
534 | /** | |
535 | * Set interrupt latch clear mode. | |
1e039ebd | 536 | * |
5fbefcd5 E |
537 | * @param clear |
538 | * New latch clear mode (0=status-read-only, 1=any-register-read) | |
539 | * @see getInterruptLatchClear() | |
540 | * @see ITG3200_RA_INT_CFG | |
541 | * @see ITG3200_INTCFG_INT_ANYRD_2CLEAR_BIT | |
542 | */ | |
543 | public void setInterruptLatchClear(boolean clear) { | |
544 | writeBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_INT_ANYRD_2CLEAR_BIT, clear); | |
545 | } | |
546 | ||
547 | /** | |
548 | * Get "device ready" interrupt enabled setting. Will be set 0 for disabled, 1 | |
549 | * for enabled. | |
1e039ebd | 550 | * |
5fbefcd5 E |
551 | * @return Current interrupt enabled setting |
552 | * @see ITG3200_RA_INT_CFG | |
553 | * @see ITG3200_INTCFG_ITG_RDY_EN_BIT | |
554 | */ | |
555 | public boolean getIntDeviceReadyEnabled() { | |
556 | return readBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_ITG_RDY_EN_BIT); | |
557 | } | |
558 | ||
559 | /** | |
560 | * Set "device ready" interrupt enabled setting. | |
1e039ebd | 561 | * |
5fbefcd5 E |
562 | * @param enabled |
563 | * New interrupt enabled setting | |
564 | * @see getIntDeviceReadyEnabled() | |
565 | * @see ITG3200_RA_INT_CFG | |
566 | * @see ITG3200_INTCFG_ITG_RDY_EN_BIT | |
567 | */ | |
568 | public void setIntDeviceReadyEnabled(boolean enabled) { | |
569 | writeBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_ITG_RDY_EN_BIT, enabled); | |
570 | } | |
571 | ||
572 | /** | |
573 | * Get "data ready" interrupt enabled setting. Will be set 0 for disabled, 1 | |
574 | * for enabled. | |
1e039ebd | 575 | * |
5fbefcd5 E |
576 | * @return Current interrupt enabled setting |
577 | * @see ITG3200_RA_INT_CFG | |
578 | * @see ITG3200_INTCFG_RAW_RDY_EN_BIT | |
579 | */ | |
580 | public boolean getIntDataReadyEnabled() { | |
581 | return readBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_RAW_RDY_EN_BIT); | |
582 | } | |
583 | ||
584 | /** | |
585 | * Set "data ready" interrupt enabled setting. | |
1e039ebd | 586 | * |
5fbefcd5 E |
587 | * @param enabled |
588 | * New interrupt enabled setting | |
589 | * @see getIntDataReadyEnabled() | |
590 | * @see ITG3200_RA_INT_CFG | |
591 | * @see ITG3200_INTCFG_RAW_RDY_EN_BIT | |
592 | */ | |
593 | public void setIntDataReadyEnabled(boolean enabled) { | |
594 | writeBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_RAW_RDY_EN_BIT, enabled); | |
595 | } | |
596 | ||
597 | // INT_STATUS register | |
598 | ||
599 | /** | |
600 | * Get Device Ready interrupt status. The ITG_RDY interrupt indicates that the | |
601 | * PLL is ready and gyroscopic data can be read. | |
1e039ebd | 602 | * |
5fbefcd5 E |
603 | * @return Device Ready interrupt status |
604 | * @see ITG3200_RA_INT_STATUS | |
605 | * @see ITG3200_INTSTAT_RAW_DATA_READY_BIT | |
606 | */ | |
607 | public boolean getIntDeviceReadyStatus() { | |
608 | return readBit(ITG3200_RA_INT_STATUS, ITG3200_INTSTAT_ITG_RDY_BIT); | |
609 | } | |
610 | ||
611 | /** | |
612 | * Get Data Ready interrupt status. In normal use, the RAW_DATA_RDY interrupt | |
613 | * is used to determine when new sensor data is available in and of the sensor | |
614 | * registers (27 to 32). | |
1e039ebd | 615 | * |
5fbefcd5 E |
616 | * @return Data Ready interrupt status |
617 | * @see ITG3200_RA_INT_STATUS | |
618 | * @see ITG3200_INTSTAT_RAW_DATA_READY_BIT | |
619 | */ | |
620 | public boolean getIntDataReadyStatus() { | |
621 | return readBit(ITG3200_RA_INT_STATUS, ITG3200_INTSTAT_RAW_DATA_READY_BIT); | |
622 | } | |
623 | ||
624 | // TEMP_OUT_* registers | |
625 | /** | |
626 | * Get current internal temperature. | |
1e039ebd | 627 | * |
5fbefcd5 E |
628 | * @return Temperature reading in 16-bit 2's complement format |
629 | * @see ITG3200_RA_TEMP_OUT_H | |
630 | */ | |
631 | public short getTemperature() { | |
632 | byte[] buf = new byte[2]; | |
633 | ReadI2CBuffer(ITG3200_RA_TEMP_OUT_H, 2, buf); | |
1e039ebd | 634 | return (short) (((short) (buf[0]) << 8) | (short) buf[1]); |
5fbefcd5 E |
635 | } |
636 | ||
637 | // GYRO_*OUT_* registers | |
638 | ||
639 | public static class AllAxes { | |
640 | public short XAxis; | |
641 | public short YAxis; | |
642 | public short ZAxis; | |
643 | } | |
644 | ||
645 | /** | |
646 | * Get 3-axis gyroscope readings. | |
1e039ebd | 647 | * |
5fbefcd5 E |
648 | * @param x |
649 | * 16-bit signed integer container for X-axis rotation | |
650 | * @param y | |
651 | * 16-bit signed integer container for Y-axis rotation | |
652 | * @param z | |
653 | * 16-bit signed integer container for Z-axis rotation | |
654 | * @see ITG3200_RA_GYRO_XOUT_H | |
655 | */ | |
656 | public AllAxes getRotation() { | |
657 | AllAxes data = new AllAxes(); | |
658 | byte[] buffer = new byte[6]; | |
659 | ReadI2CBuffer(ITG3200_RA_GYRO_XOUT_H, 6, buffer); | |
1e039ebd E |
660 | data.XAxis = (short) ((((short) buffer[0]) << 8) | buffer[1]); |
661 | data.YAxis = (short) ((((short) buffer[2]) << 8) | buffer[3]); | |
662 | data.ZAxis = (short) ((((short) buffer[4]) << 8) | buffer[5]); | |
5fbefcd5 E |
663 | return data; |
664 | } | |
665 | ||
666 | public void ReadI2CBuffer(int registerAddress, int count, byte[] buffer) { | |
667 | try { | |
668 | m_i2c.read(registerAddress, count, buffer); | |
669 | } catch (Throwable t) { | |
670 | DriverStation.reportError("ERROR Unhandled exception in I2C Read: " | |
671 | + t.toString() + " at " + Arrays.toString(t.getStackTrace()), false); | |
672 | } | |
673 | } | |
674 | ||
675 | public short ReadShortFromRegister(byte register, int count) { | |
676 | byte[] buffer = new byte[count]; | |
677 | ReadI2CBuffer(register, count, buffer); | |
1e039ebd | 678 | return (short) ((((short) buffer[0]) << 8) | buffer[1]); |
5fbefcd5 E |
679 | } |
680 | ||
681 | /** | |
682 | * Get X-axis gyroscope reading. | |
1e039ebd | 683 | * |
5fbefcd5 E |
684 | * @return X-axis rotation measurement in 16-bit 2's complement format |
685 | * @see ITG3200_RA_GYRO_XOUT_H | |
686 | */ | |
687 | public short getRotationX() { | |
688 | return ReadShortFromRegister(ITG3200_RA_GYRO_XOUT_H, 2); | |
689 | } | |
690 | ||
691 | /** | |
692 | * Get Y-axis gyroscope reading. | |
1e039ebd | 693 | * |
5fbefcd5 E |
694 | * @return Y-axis rotation measurement in 16-bit 2's complement format |
695 | * @see ITG3200_RA_GYRO_YOUT_H | |
696 | */ | |
697 | public short getRotationY() { | |
698 | return ReadShortFromRegister(ITG3200_RA_GYRO_YOUT_H, 2); | |
699 | } | |
700 | ||
701 | /** | |
702 | * Get Z-axis gyroscope reading. | |
1e039ebd | 703 | * |
5fbefcd5 E |
704 | * @return Z-axis rotation measurement in 16-bit 2's complement format |
705 | * @see ITG3200_RA_GYRO_ZOUT_H | |
706 | */ | |
707 | public short getRotationZ() { | |
1e039ebd | 708 | return ReadShortFromRegister(ITG3200_RA_GYRO_ZOUT_H, 2); |
5fbefcd5 E |
709 | } |
710 | ||
711 | // PWR_MGM register | |
712 | ||
713 | /** | |
714 | * Trigger a full device reset. A small delay of ~50ms may be desirable after | |
715 | * triggering a reset. | |
1e039ebd | 716 | * |
5fbefcd5 E |
717 | * @see ITG3200_RA_PWR_MGM |
718 | * @see ITG3200_PWR_H_RESET_BIT | |
719 | */ | |
720 | ||
721 | public void reset() { | |
722 | writeBit(ITG3200_RA_PWR_MGM, ITG3200_PWR_H_RESET_BIT, true); | |
723 | } | |
724 | ||
725 | /** | |
726 | * Get sleep mode status. Setting the SLEEP bit in the register puts the | |
727 | * device into very low power sleep mode. In this mode, only the serial | |
728 | * interface and internal registers remain active, allowing for a very low | |
729 | * standby current. Clearing this bit puts the device back into normal mode. | |
730 | * To save power, the individual standby selections for each of the gyros | |
731 | * should be used if any gyro axis is not used by the application. | |
1e039ebd | 732 | * |
5fbefcd5 E |
733 | * @return Current sleep mode enabled status |
734 | * @see ITG3200_RA_PWR_MGM | |
735 | * @see ITG3200_PWR_SLEEP_BIT | |
736 | */ | |
737 | public boolean getSleepEnabled() { | |
738 | return readBit(ITG3200_RA_PWR_MGM, ITG3200_PWR_SLEEP_BIT); | |
739 | } | |
740 | ||
741 | /** | |
742 | * Set sleep mode status. | |
1e039ebd | 743 | * |
5fbefcd5 E |
744 | * @param enabled |
745 | * New sleep mode enabled status | |
746 | * @see getSleepEnabled() | |
747 | * @see ITG3200_RA_PWR_MGM | |
748 | * @see ITG3200_PWR_SLEEP_BIT | |
749 | */ | |
750 | public void setSleepEnabled(boolean enabled) { | |
751 | writeBit(ITG3200_RA_PWR_MGM, ITG3200_PWR_SLEEP_BIT, enabled); | |
752 | } | |
753 | ||
754 | /** | |
755 | * Get X-axis standby enabled status. If enabled, the X-axis will not gather | |
756 | * or report data (or use power). | |
1e039ebd | 757 | * |
5fbefcd5 E |
758 | * @return Current X-axis standby enabled status |
759 | * @see ITG3200_RA_PWR_MGM | |
760 | * @see ITG3200_PWR_STBY_XG_BIT | |
761 | */ | |
762 | public boolean getStandbyXEnabled() { | |
763 | return readBit(ITG3200_RA_PWR_MGM, ITG3200_PWR_STBY_XG_BIT); | |
764 | } | |
765 | ||
766 | /** | |
767 | * Set X-axis standby enabled status. | |
1e039ebd | 768 | * |
5fbefcd5 E |
769 | * @param New |
770 | * X-axis standby enabled status | |
771 | * @see getStandbyXEnabled() | |
772 | * @see ITG3200_RA_PWR_MGM | |
773 | * @see ITG3200_PWR_STBY_XG_BIT | |
774 | */ | |
775 | public void setStandbyXEnabled(boolean enabled) { | |
776 | writeBit(ITG3200_RA_PWR_MGM, ITG3200_PWR_STBY_XG_BIT, enabled); | |
777 | } | |
778 | ||
779 | /** | |
780 | * Get Y-axis standby enabled status. If enabled, the Y-axis will not gather | |
781 | * or report data (or use power). | |
1e039ebd | 782 | * |
5fbefcd5 E |
783 | * @return Current Y-axis standby enabled status |
784 | * @see ITG3200_RA_PWR_MGM | |
785 | * @see ITG3200_PWR_STBY_YG_BIT | |
786 | */ | |
787 | public boolean getStandbyYEnabled() { | |
788 | return readBit(ITG3200_RA_PWR_MGM, ITG3200_PWR_STBY_YG_BIT); | |
789 | } | |
790 | ||
791 | /** | |
792 | * Set Y-axis standby enabled status. | |
1e039ebd | 793 | * |
5fbefcd5 E |
794 | * @param New |
795 | * Y-axis standby enabled status | |
796 | * @see getStandbyYEnabled() | |
797 | * @see ITG3200_RA_PWR_MGM | |
798 | * @see ITG3200_PWR_STBY_YG_BIT | |
799 | */ | |
800 | public void setStandbyYEnabled(boolean enabled) { | |
801 | writeBit(ITG3200_RA_PWR_MGM, ITG3200_PWR_STBY_YG_BIT, enabled); | |
802 | } | |
803 | ||
804 | /** | |
805 | * Get Z-axis standby enabled status. If enabled, the Z-axis will not gather | |
806 | * or report data (or use power). | |
1e039ebd | 807 | * |
5fbefcd5 E |
808 | * @return Current Z-axis standby enabled status |
809 | * @see ITG3200_RA_PWR_MGM | |
810 | * @see ITG3200_PWR_STBY_ZG_BIT | |
811 | */ | |
812 | public boolean getStandbyZEnabled() { | |
813 | return readBit(ITG3200_RA_PWR_MGM, ITG3200_PWR_STBY_ZG_BIT); | |
814 | } | |
815 | ||
816 | /** | |
817 | * Set Z-axis standby enabled status. | |
1e039ebd | 818 | * |
5fbefcd5 E |
819 | * @param New |
820 | * Z-axis standby enabled status | |
821 | * @see getStandbyZEnabled() | |
822 | * @see ITG3200_RA_PWR_MGM | |
823 | * @see ITG3200_PWR_STBY_ZG_BIT | |
824 | */ | |
825 | public void setStandbyZEnabled(boolean enabled) { | |
826 | writeBit(ITG3200_RA_PWR_MGM, ITG3200_PWR_STBY_ZG_BIT, enabled); | |
827 | } | |
828 | ||
829 | /** | |
830 | * Get clock source setting. | |
1e039ebd | 831 | * |
5fbefcd5 E |
832 | * @return Current clock source setting |
833 | * @see ITG3200_RA_PWR_MGM | |
834 | * @see ITG3200_PWR_CLK_SEL_BIT | |
835 | * @see ITG3200_PWR_CLK_SEL_LENGTH | |
836 | */ | |
837 | public byte getClockSource() { | |
838 | byte[] buf = new byte[1]; | |
839 | ReadI2CBuffer(ITG3200_RA_PWR_MGM, 1, buf); | |
840 | // I2Cdev::readBits(devAddr, ITG3200_RA_PWR_MGM, ITG3200_PWR_CLK_SEL_BIT, | |
841 | // ITG3200_PWR_CLK_SEL_LENGTH, buffer); | |
842 | return (byte) (buf[0] & ITG3200_PWR_CLK_SEL_BIT); | |
843 | } | |
844 | ||
845 | /** | |
846 | * Set clock source setting. On power up, the ITG-3200 defaults to the | |
847 | * internal oscillator. It is highly recommended that the device is configured | |
848 | * to use one of the gyros (or an external clock) as the clock reference, due | |
849 | * to the improved stability. | |
850 | * | |
851 | * The CLK_SEL setting determines the device clock source as follows: | |
852 | * | |
853 | * CLK_SEL | Clock Source --------+-------------------------------------- 0 | | |
854 | * Internal oscillator 1 | PLL with X Gyro reference 2 | PLL with Y Gyro | |
855 | * reference 3 | PLL with Z Gyro reference 4 | PLL with external 32.768kHz | |
856 | * reference 5 | PLL with external 19.2MHz reference 6 | Reserved 7 | Reserved | |
857 | * | |
858 | * @param source | |
859 | * New clock source setting | |
860 | * @see getClockSource() | |
861 | * @see ITG3200_RA_PWR_MGM | |
862 | * @see ITG3200_PWR_CLK_SEL_BIT | |
863 | * @see ITG3200_PWR_CLK_SEL_LENGTH | |
864 | */ | |
865 | public void setClockSource(byte source) { | |
866 | writeBits(ITG3200_RA_PWR_MGM, ITG3200_PWR_CLK_SEL_BIT, | |
867 | ITG3200_PWR_CLK_SEL_LENGTH, source); | |
868 | } | |
869 | ||
870 | private ITable m_table; | |
871 | ||
872 | /** | |
873 | * {@inheritDoc} | |
874 | */ | |
875 | @Override | |
876 | public void initTable(ITable subtable) { | |
877 | m_table = subtable; | |
878 | updateTable(); | |
879 | } | |
880 | ||
881 | /** | |
882 | * {@inheritDoc} | |
883 | */ | |
884 | @Override | |
885 | public ITable getTable() { | |
886 | return m_table; | |
887 | } | |
888 | ||
889 | /** | |
890 | * {@inheritDoc} | |
891 | */ | |
892 | @Override | |
893 | public void updateTable() { | |
894 | if (m_table != null) { | |
895 | m_table.putNumber("GyroX", getRotationX()); | |
896 | m_table.putNumber("GyroY", getRotationY()); | |
897 | m_table.putNumber("GyroZ", getRotationZ()); | |
898 | m_table.putNumber("GyroPIDValue", pidGet()); | |
899 | } | |
900 | } | |
901 | ||
902 | /* | |
903 | * (non-Javadoc) | |
1e039ebd | 904 | * |
5fbefcd5 E |
905 | * @see edu.wpi.first.wpilibj.Sendable#getSmartDashboardType() |
906 | */ | |
907 | @Override | |
908 | public String getSmartDashboardType() { | |
909 | return "Gyro"; | |
910 | } | |
911 | ||
912 | /* | |
913 | * (non-Javadoc) | |
1e039ebd | 914 | * |
5fbefcd5 E |
915 | * @see |
916 | * edu.wpi.first.wpilibj.livewindow.LiveWindowSendable#startLiveWindowMode() | |
917 | */ | |
918 | @Override | |
919 | public void startLiveWindowMode() { | |
920 | } | |
921 | ||
922 | /* | |
923 | * (non-Javadoc) | |
1e039ebd | 924 | * |
5fbefcd5 E |
925 | * @see |
926 | * edu.wpi.first.wpilibj.livewindow.LiveWindowSendable#stopLiveWindowMode() | |
927 | */ | |
928 | @Override | |
929 | public void stopLiveWindowMode() { | |
930 | } | |
931 | ||
932 | /* | |
933 | * (non-Javadoc) | |
1e039ebd | 934 | * |
5fbefcd5 E |
935 | * @see edu.wpi.first.wpilibj.PIDSource#pidGet() |
936 | */ | |
937 | @Override | |
938 | public double pidGet() { | |
939 | // TODO We likely want to return one of the axes based on a setup option. | |
940 | AllAxes var = getRotation(); | |
941 | double result = Math.cbrt( | |
942 | var.XAxis * var.XAxis + var.YAxis * var.YAxis + var.ZAxis * var.ZAxis); | |
943 | return result; | |
944 | // return 0; | |
945 | } | |
946 | ||
947 | public static final byte ITG3200_ADDRESS_AD0_LOW = 0x68; // address pin low | |
948 | // (GND), default for | |
949 | // SparkFun IMU | |
950 | // Digital Combo | |
951 | // board | |
952 | public static final byte ITG3200_ADDRESS_AD0_HIGH = 0x69; // address pin high | |
953 | // (VCC), default | |
954 | // for SparkFun | |
955 | // ITG-3200 Breakout | |
956 | // board | |
957 | ||
958 | public static final int ITG3200_SPARKFUN_ADDRES = 0xD2; | |
959 | ||
960 | public static final int ITG3200_DEFAULT_ADDRESS = ITG3200_ADDRESS_AD0_LOW; // ITG3200_ADDRESS_AD0_HIGH; | |
961 | ||
962 | public static final byte ITG3200_RA_WHO_AM_I = 0x00; | |
963 | public static final byte ITG3200_RA_SMPLRT_DIV = 0x15; | |
964 | public static final byte ITG3200_RA_DLPF_FS = 0x16; | |
965 | public static final byte ITG3200_RA_INT_CFG = 0x17; | |
966 | public static final byte ITG3200_RA_INT_STATUS = 0x1A; | |
967 | public static final byte ITG3200_RA_TEMP_OUT_H = 0x1B; | |
968 | public static final byte ITG3200_RA_TEMP_OUT_L = 0x1C; | |
969 | public static final byte ITG3200_RA_GYRO_XOUT_H = 0x1D; | |
970 | public static final byte ITG3200_RA_GYRO_XOUT_L = 0x1E; | |
971 | public static final byte ITG3200_RA_GYRO_YOUT_H = 0x1F; | |
972 | public static final byte ITG3200_RA_GYRO_YOUT_L = 0x20; | |
973 | public static final byte ITG3200_RA_GYRO_ZOUT_H = 0x21; | |
974 | public static final byte ITG3200_RA_GYRO_ZOUT_L = 0x22; | |
975 | public static final byte ITG3200_RA_PWR_MGM = 0x3E; | |
976 | ||
977 | public static final short ITG3200_DEVID_BIT = 6; | |
978 | public static final short ITG3200_DEVID_LENGTH = 6; | |
979 | ||
980 | public static final short ITG3200_DF_FS_SEL_BIT = 4; | |
981 | public static final short ITG3200_DF_FS_SEL_LENGTH = 2; | |
982 | public static final short ITG3200_DF_DLPF_CFG_BIT = 2; | |
983 | public static final short ITG3200_DF_DLPF_CFG_LENGTH = 3; | |
984 | ||
985 | public static final byte ITG3200_FULLSCALE_2000 = 0x03; | |
986 | ||
987 | public static final byte ITG3200_DLPF_BW_256 = 0x00; | |
988 | public static final byte ITG3200_DLPF_BW_188 = 0x01; | |
989 | public static final byte ITG3200_DLPF_BW_98 = 0x02; | |
990 | public static final byte ITG3200_DLPF_BW_42 = 0x03; | |
991 | public static final byte ITG3200_DLPF_BW_20 = 0x04; | |
992 | public static final byte ITG3200_DLPF_BW_10 = 0x05; | |
993 | public static final byte ITG3200_DLPF_BW_5 = 0x06; | |
994 | ||
995 | public static final byte ITG3200_INTCFG_ACTL_BIT = 7; | |
996 | public static final byte ITG3200_INTCFG_OPEN_BIT = 6; | |
997 | public static final byte ITG3200_INTCFG_LATCH_INT_EN_BIT = 5; | |
998 | public static final byte ITG3200_INTCFG_INT_ANYRD_2CLEAR_BIT = 4; | |
999 | public static final byte ITG3200_INTCFG_ITG_RDY_EN_BIT = 2; | |
1000 | public static final byte ITG3200_INTCFG_RAW_RDY_EN_BIT = 0; | |
1001 | ||
1002 | public static final byte ITG3200_INTMODE_ACTIVEHIGH = 0x00; | |
1003 | public static final byte ITG3200_INTMODE_ACTIVELOW = 0x01; | |
1004 | ||
1005 | public static final byte ITG3200_INTDRV_PUSHPULL = 0x00; | |
1006 | public static final byte ITG3200_INTDRV_OPENDRAIN = 0x01; | |
1007 | ||
1008 | public static final byte ITG3200_INTLATCH_50USPULSE = 0x00; | |
1009 | public static final byte ITG3200_INTLATCH_WAITCLEAR = 0x01; | |
1010 | ||
1011 | public static final byte ITG3200_INTCLEAR_STATUSREAD = 0x00; | |
1012 | public static final byte ITG3200_INTCLEAR_ANYREAD = 0x01; | |
1013 | ||
1014 | public static final byte ITG3200_INTSTAT_ITG_RDY_BIT = 2; | |
1015 | public static final byte ITG3200_INTSTAT_RAW_DATA_READY_BIT = 0; | |
1016 | ||
1017 | public static final byte ITG3200_PWR_H_RESET_BIT = 7; | |
1018 | public static final byte ITG3200_PWR_SLEEP_BIT = 6; | |
1019 | public static final byte ITG3200_PWR_STBY_XG_BIT = 5; | |
1020 | public static final byte ITG3200_PWR_STBY_YG_BIT = 4; | |
1021 | public static final byte ITG3200_PWR_STBY_ZG_BIT = 3; | |
1022 | public static final byte ITG3200_PWR_CLK_SEL_BIT = 2; | |
1023 | public static final byte ITG3200_PWR_CLK_SEL_LENGTH = 3; | |
1024 | ||
1025 | public static final byte ITG3200_CLOCK_INTERNAL = 0x00; | |
1026 | public static final byte ITG3200_CLOCK_PLL_XGYRO = 0x01; | |
1027 | public static final byte ITG3200_CLOCK_PLL_YGYRO = 0x02; | |
1028 | public static final byte ITG3200_CLOCK_PLL_ZGYRO = 0x03; | |
1029 | public static final byte ITG3200_CLOCK_PLL_EXT32K = 0x04; | |
1030 | public static final byte ITG3200_CLOCK_PLL_EXT19M = 0x05; | |
1031 | ||
1032 | @Override | |
1033 | public void setPIDSourceType(PIDSourceType pidSource) { | |
1034 | // TODO Auto-generated method stub | |
1035 | ||
1036 | } | |
1037 | ||
1038 | @Override | |
1039 | public PIDSourceType getPIDSourceType() { | |
1040 | // TODO Auto-generated method stub | |
1041 | return null; | |
1042 | } | |
5fbefcd5 | 1043 | } |