From a69a9b4bf2944947dc3049e94bf489feae969b97 Mon Sep 17 00:00:00 2001 From: Robert Atkinson Date: Sun, 10 Jan 2016 17:15:34 -0800 Subject: [PATCH] 1. Add accel, gryo, and mag range, bw, etc parameterization. 2. get rid of II2cDeviceClient.executeFunctionWhileLocked 3. I2cDeviceClient.read() preserves read window 4. Fix naming convention in IMU parameter block Former-commit-id: 420905a2d18cd169f2b42b823105a473ed0715e4 Former-commit-id: 20ae056a319e38aeb743f0a671f6d36995c6fae3 --- .../library/examples/SynchIMUDemo.java | 10 +-- .../library/interfaces/IBNO055IMU.java | 49 +++++++++++- .../library/interfaces/II2cDeviceClient.java | 47 +---------- .../library/internal/AdaFruitBNO055IMU.java | 80 ++++++++----------- .../library/internal/I2cDeviceClient.java | 54 ++++++------- 5 files changed, 113 insertions(+), 127 deletions(-) diff --git a/SwerveRoboticsLibrary/src/main/java/org/swerverobotics/library/examples/SynchIMUDemo.java b/SwerveRoboticsLibrary/src/main/java/org/swerverobotics/library/examples/SynchIMUDemo.java index bc4bc9033db..b5e5603049c 100644 --- a/SwerveRoboticsLibrary/src/main/java/org/swerverobotics/library/examples/SynchIMUDemo.java +++ b/SwerveRoboticsLibrary/src/main/java/org/swerverobotics/library/examples/SynchIMUDemo.java @@ -37,11 +37,11 @@ public class SynchIMUDemo extends SynchronousOpMode @Override public void main() throws InterruptedException { - // We are expecting the IMU to be attached to an I2C port on a core device interface + // We are expecting the IMU to be attached to an I2C port on a core device interface // module and named "imu". Retrieve that raw I2cDevice and then wrap it in an object that // semantically understands this particular kind of sensor. - parameters.angleunit = IBNO055IMU.ANGLEUNIT.DEGREES; - parameters.accelunit = IBNO055IMU.ACCELUNIT.METERS_PERSEC_PERSEC; + parameters.angleUnit = IBNO055IMU.ANGLEUNIT.DEGREES; + parameters.accelUnit = IBNO055IMU.ACCELUNIT.METERS_PERSEC_PERSEC; parameters.loggingEnabled = false; parameters.loggingTag = "BNO055"; imu = ClassFactory.createAdaFruitBNO055IMU(hardwareMap.i2cDevice.get("imu"), parameters); @@ -186,7 +186,7 @@ public Object value() String formatAngle(double angle) { - return parameters.angleunit==IBNO055IMU.ANGLEUNIT.DEGREES ? formatDegrees(angle) : formatRadians(angle); + return parameters.angleUnit ==IBNO055IMU.ANGLEUNIT.DEGREES ? formatDegrees(angle) : formatRadians(angle); } String formatRadians(double radians) { @@ -202,7 +202,7 @@ String formatRate(double cyclesPerSecond) } String formatPosition(double coordinate) { - String unit = parameters.accelunit== IBNO055IMU.ACCELUNIT.METERS_PERSEC_PERSEC + String unit = parameters.accelUnit == IBNO055IMU.ACCELUNIT.METERS_PERSEC_PERSEC ? "m" : "??"; return String.format("%.2f%s", coordinate, unit); } diff --git a/SwerveRoboticsLibrary/src/main/java/org/swerverobotics/library/interfaces/IBNO055IMU.java b/SwerveRoboticsLibrary/src/main/java/org/swerverobotics/library/interfaces/IBNO055IMU.java index 8b43a3eb7a5..51c72251297 100644 --- a/SwerveRoboticsLibrary/src/main/java/org/swerverobotics/library/interfaces/IBNO055IMU.java +++ b/SwerveRoboticsLibrary/src/main/java/org/swerverobotics/library/interfaces/IBNO055IMU.java @@ -48,11 +48,32 @@ class Parameters /** units in which temperature are measured. See Section 3.6.1 (p31) of the BNO055 specification */ public TEMPUNIT temperatureUnit = TEMPUNIT.CELSIUS; /** units in which angles and angular rates are measured. See Section 3.6.1 (p31) of the BNO055 specification */ - public ANGLEUNIT angleunit = ANGLEUNIT.RADIANS; + public ANGLEUNIT angleUnit = ANGLEUNIT.RADIANS; /** units in which accelerations are measured. See Section 3.6.1 (p31) of the BNO055 specification */ - public ACCELUNIT accelunit = ACCELUNIT.METERS_PERSEC_PERSEC; + public ACCELUNIT accelUnit = ACCELUNIT.METERS_PERSEC_PERSEC; /** directional convention for measureing pitch angles. See Section 3.6.1 (p31) of the BNO055 specification */ - public PITCHMODE pitchmode = PITCHMODE.ANDROID; // Section 3.6.2 + public PITCHMODE pitchMode = PITCHMODE.ANDROID; // Section 3.6.2 + + /** accelerometer range. See Section 3.5.2 (p27) and Table 3-4 (p21) of the BNO055 specification */ + public ACCELRANGE accelRange = ACCELRANGE.G4; + /** accelerometer bandwidth. See Section 3.5.2 (p27) and Table 3-4 (p21) of the BNO055 specification */ + public ACCELBANDWIDTH accelBandwidth = ACCELBANDWIDTH.HZ62_5; + /** accelerometer power mode. See Section 3.5.2 (p27) and Section 4.2.2 (p77) of the BNO055 specification */ + public ACCELPOWERMODE accelPowerMode = ACCELPOWERMODE.NORMAL; + + /** gyroscope range. See Section 3.5.2 (p27) and Table 3-4 (p21) of the BNO055 specification */ + public GYRORANGE gyroRange = GYRORANGE.DPS2000; + /** gyroscope bandwidth. See Section 3.5.2 (p27) and Table 3-4 (p21) of the BNO055 specification */ + public GYROBANDWIDTH gyroBandwidth = GYROBANDWIDTH.HZ32; + /** gyroscope power mode. See Section 3.5.2 (p27) and Section 4.4.4 (p78) of the BNO055 specification */ + public GYROPOWERMODE gyroPowerMode = GYROPOWERMODE.NORMAL; + + /** magnetometer data rate. See Section 3.5.3 (p27) and Section 4.4.3 (p77) of the BNO055 specification */ + public MAGRATE magRate = MAGRATE.HZ10; + /** magnetometer op mode. See Section 3.5.3 (p27) and Section 4.4.3 (p77) of the BNO055 specification */ + public MAGOPMODE magOpMode = MAGOPMODE.REGULAR; + /** magnetometer power mode. See Section 3.5.3 (p27) and Section 4.4.3 (p77) of the BNO055 specification */ + public MAGPOWERMODE magPowerMode = MAGPOWERMODE.NORMAL; /** calibration data with which the BNO055 should be initialized */ public byte[] calibrationData = null; @@ -307,6 +328,17 @@ enum ANGLEUNIT { DEGREES(0), RADIANS(1); public fi enum ACCELUNIT { METERS_PERSEC_PERSEC(0), MILLIGALS(1); public final byte bVal; ACCELUNIT(int i) { bVal =(byte)i; }} enum PITCHMODE { WINDOWS(0), ANDROID(1); public final byte bVal; PITCHMODE(int i) { bVal =(byte)i; }} + enum GYRORANGE { DPS2000(0), DPS1000(1), DPS500(2), DPS250(3), DPS125(4); public final byte bVal; GYRORANGE(int i) { bVal =(byte)(i<<0);}} + enum GYROBANDWIDTH { HZ523(0), HZ230(1), HZ116(2), HZ47(3), HZ23(4), HZ12(5), HZ64(6), HZ32(7); public final byte bVal; GYROBANDWIDTH(int i) { bVal =(byte)(i<<3);}} + enum GYROPOWERMODE { NORMAL(0), FAST(1), DEEP(2), SUSPEND(3), ADVANCED(4) ; public final byte bVal; GYROPOWERMODE(int i) { bVal =(byte)(i<<0);}} + enum ACCELRANGE { G2(0), G4(1), G8(2), G16(3); public final byte bVal; ACCELRANGE(int i) { bVal =(byte)(i<<0);}} + enum ACCELBANDWIDTH { HZ7_81(0), HZ15_63(1), HZ31_25(2), HZ62_5(3), HZ125(4), HZ250(5), HZ500(6), HZ1000(7); public final byte bVal; ACCELBANDWIDTH(int i) { bVal =(byte)(i<<2);}} + enum ACCELPOWERMODE { NORMAL(0), SUSPEND(1), LOW1(2), STANDBY(3), LOW2(4), DEEP(5); public final byte bVal; ACCELPOWERMODE(int i) { bVal =(byte)(i<<5);}} + + enum MAGRATE { HZ2(0), HZ6(1), HZ8(2), HZ10(3), HZ15(4), HZ20(5), HZ25(6), HZ30(7); public final byte bVal; MAGRATE(int i) { bVal =(byte)(i<<0);}} + enum MAGOPMODE { LOW(0), REGULAR(1), ENHANCED(2), HIGH(3); public final byte bVal; MAGOPMODE(int i) { bVal =(byte)(i<<3);}} + enum MAGPOWERMODE { NORMAL(0), SLEEP(1), SUSPEND(2), FORCE(3); public final byte bVal; MAGPOWERMODE(int i) { bVal =(byte)(i<<5);}} + /** * Sensor modes are described in Table 3-5 (p21) of the BNO055 specification, * where they are termed "operation modes". @@ -472,7 +504,16 @@ enum REGISTER ACCEL_RADIUS_LSB(0X67), ACCEL_RADIUS_MSB(0X68), MAG_RADIUS_LSB(0X69), - MAG_RADIUS_MSB(0X6A); + MAG_RADIUS_MSB(0X6A), + + /** Selected Page 1 registers */ + ACC_CONFIG(0x08), + MAG_CONFIG(0x09), + GYR_CONFIG_0(0x0A), + GYR_CONFIG_1(0x0B), + ACC_SLEEP_CONFIG(0x0C), + GYR_SLEEP_CONFIG(0x0D); + //------------------------------------------------------------------------------------------ public final byte bVal; private REGISTER(int i) diff --git a/SwerveRoboticsLibrary/src/main/java/org/swerverobotics/library/interfaces/II2cDeviceClient.java b/SwerveRoboticsLibrary/src/main/java/org/swerverobotics/library/interfaces/II2cDeviceClient.java index 120d5aa21ba..bd9fa3fc9f1 100644 --- a/SwerveRoboticsLibrary/src/main/java/org/swerverobotics/library/interfaces/II2cDeviceClient.java +++ b/SwerveRoboticsLibrary/src/main/java/org/swerverobotics/library/interfaces/II2cDeviceClient.java @@ -77,7 +77,6 @@ public interface II2cDeviceClient extends HardwareDevice * * @see #setReadWindow(ReadWindow) * @see #read8(int) - * @see #executeFunctionWhileLocked(IFunc) */ void ensureReadWindow(ReadWindow windowNeeded, ReadWindow windowToSet); @@ -130,14 +129,6 @@ public interface II2cDeviceClient extends HardwareDevice * a new read fresh window will be created with the same set of registers. Otherwise, a * window that exactly covers the requested set of registers will be created.

* - *

If one is trying to optimize the the register window by calling - * {@link #ensureReadWindow(ReadWindow, ReadWindow) ensureReadWindow()}, this auto-window - * creation can cause difficulties if any concurrent access is present. In such situations, - * {@link #executeFunctionWhileLocked(IFunc)} can be used to allow you to atomically both - * set the read window and execute a read without the possibility of the read window being - * re-adjusted in the middle. - *

- * * @param ireg the register number of the first byte register to read * @param creg the number of bytes / registers to read * @return the data which was read, together with the timestamp @@ -145,7 +136,6 @@ public interface II2cDeviceClient extends HardwareDevice * @see #read(int, int) * @see #read8(int) * @see #ensureReadWindow(ReadWindow, ReadWindow) - * @see #executeFunctionWhileLocked(IFunc) */ TimestampedData readTimeStamped(int ireg, int creg); @@ -172,7 +162,6 @@ class TimestampedData * * @see #ensureReadWindow(ReadWindow, ReadWindow) * @see #readTimeStamped(int, int) - * @see #executeFunctionWhileLocked(IFunc) */ TimestampedData readTimeStamped(int ireg, int creg, ReadWindow readWindowNeeded, ReadWindow readWindowSet); @@ -237,30 +226,6 @@ class TimestampedData */ void waitForWriteCompletions(); - //---------------------------------------------------------------------------------------------- - // Concurrency management - //---------------------------------------------------------------------------------------------- - - /** - * Executes the indicated action while holding the concurrency lock on the object - * so as to prevent other threads from interleaving. - * - * @param action the action to execute - * @see #executeFunctionWhileLocked(IFunc) - */ - void executeActionWhileLocked(Runnable action); - - /** - * Executes the indicated function while holding the concurrency lock on the object - * so as to prevent other threads from interleaving. Returns the value of the function. - * - * @param function the function to execute - * @param the type of the data returned from the function - * @return the datum value returned from the function - * @see #executeActionWhileLocked(Runnable) - */ - T executeFunctionWhileLocked(IFunc function); - //---------------------------------------------------------------------------------------------- // Heartbeats //---------------------------------------------------------------------------------------------- @@ -317,13 +282,7 @@ class HeartbeatAction /** Priority #2: re-issue the last I2C write operation, if possible. */ public boolean rewriteLastWritten = false; - /** Priority #3: explicitly read a given register window. Note that using - * this form of heartbeat may cause the I2C device to experience concurrency it - * otherwise might not support for this heartbeat form may make use of - * worker threads. - * - * @see #executeFunctionWhileLocked(IFunc) - */ + /** Priority #3: explicitly read a given register window */ public ReadWindow heartbeatReadWindow = null; } @@ -567,9 +526,9 @@ public ReadWindow(int iregFirst, int creg, READ_MODE readMode) /** * Returns a copy of this window but with the {@link #readIssued} flag clear - * @return a fresh readable copy of the window + * @return a fresh copy of the window into which data can actually be read. */ - public ReadWindow freshCopy() + public ReadWindow readableCopy() { return new ReadWindow(this.iregFirst, this.creg, this.readMode); } diff --git a/SwerveRoboticsLibrary/src/main/java/org/swerverobotics/library/internal/AdaFruitBNO055IMU.java b/SwerveRoboticsLibrary/src/main/java/org/swerverobotics/library/internal/AdaFruitBNO055IMU.java index 1b5dbbf6cfc..ab4655eb93e 100644 --- a/SwerveRoboticsLibrary/src/main/java/org/swerverobotics/library/internal/AdaFruitBNO055IMU.java +++ b/SwerveRoboticsLibrary/src/main/java/org/swerverobotics/library/internal/AdaFruitBNO055IMU.java @@ -166,11 +166,11 @@ public void initialize(Parameters parameters) write8(REGISTER.PAGE_ID, 0); // Set the output units. Section 3.6, p31 - int unitsel = (parameters.pitchmode.bVal << 7) | // pitch angle convention + int unitsel = (parameters.pitchMode.bVal << 7) | // pitch angle convention (parameters.temperatureUnit.bVal << 4) | // temperature - (parameters.angleunit.bVal << 2) | // euler angle units - (parameters.angleunit.bVal << 1) | // gyro units, per second - (parameters.accelunit.bVal /*<< 0*/); // accelerometer units + (parameters.angleUnit.bVal << 2) | // euler angle units + (parameters.angleUnit.bVal << 1) | // gyro units, per second + (parameters.accelUnit.bVal /*<< 0*/); // accelerometer units write8(REGISTER.UNIT_SEL, unitsel); // Use or don't use the external crystal @@ -178,6 +178,18 @@ public void initialize(Parameters parameters) write8(REGISTER.SYS_TRIGGER, parameters.useExternalCrystal ? 0x80 : 0x00); delayLoreExtra(10); + // Switch to page 1 so we can write some more registers + write8(REGISTER.PAGE_ID, 1); + + // Configure selected page 1 registers + write8(REGISTER.ACC_CONFIG, parameters.accelPowerMode.bVal | parameters.accelBandwidth.bVal | parameters.accelRange.bVal); + write8(REGISTER.MAG_CONFIG, parameters.magPowerMode.bVal | parameters.magOpMode.bVal | parameters.magRate.bVal); + write8(REGISTER.GYR_CONFIG_0, parameters.gyroBandwidth.bVal | parameters.gyroRange.bVal); + write8(REGISTER.GYR_CONFIG_1, parameters.gyroPowerMode.bVal); + + // Switch back + write8(REGISTER.PAGE_ID, 0); + // Run a self test. This appears to be a necessary step in order for the // sensor to be able to actually be used. write8(REGISTER.SYS_TRIGGER, read8(REGISTER.SYS_TRIGGER) | 0x01); // SYS_TRIGGER=0x3F @@ -341,20 +353,14 @@ public synchronized EulerAngles getAngularOrientation() public synchronized Quaternion getQuaternionOrientation() { - return this.deviceClient.executeFunctionWhileLocked(new IFunc() - { - @Override public Quaternion value() - { - // Ensure we can see the registers we need - deviceClient.ensureReadWindow( - new II2cDeviceClient.ReadWindow(REGISTER.QUATERNION_DATA_W_LSB.bVal, 8, readMode), - upperWindow); - - // Section 3.6.5.5 of BNO055 specification - II2cDeviceClient.TimestampedData ts = deviceClient.readTimeStamped(REGISTER.QUATERNION_DATA_W_LSB.bVal, 8); - return new Quaternion(ts, (1 << 14)); - } - }); + // Ensure we can see the registers we need + deviceClient.ensureReadWindow( + new II2cDeviceClient.ReadWindow(REGISTER.QUATERNION_DATA_W_LSB.bVal, 8, readMode), + upperWindow); + + // Section 3.6.5.5 of BNO055 specification + II2cDeviceClient.TimestampedData ts = deviceClient.readTimeStamped(REGISTER.QUATERNION_DATA_W_LSB.bVal, 8); + return new Quaternion(ts, (1 << 14)); } /** @@ -363,7 +369,7 @@ public synchronized Quaternion getQuaternionOrientation() */ private double getAngularScale() { - return this.parameters.angleunit == ANGLEUNIT.DEGREES ? 16.0 : 900.0; + return this.parameters.angleUnit == ANGLEUNIT.DEGREES ? 16.0 : 900.0; } /** @@ -372,7 +378,7 @@ private double getAngularScale() */ private double getAccelerationScale() { - return this.parameters.accelunit == ACCELUNIT.METERS_PERSEC_PERSEC ? 100.0 : 1.0; + return this.parameters.accelUnit == ACCELUNIT.METERS_PERSEC_PERSEC ? 100.0 : 1.0; } /** @@ -387,17 +393,11 @@ private double getFluxScale() private II2cDeviceClient.TimestampedData getVector(final VECTOR vector) { - return this.deviceClient.executeFunctionWhileLocked(new IFunc() - { - @Override public II2cDeviceClient.TimestampedData value() - { - // Ensure that the 6 bytes for this vector are visible in the register window. - ensureReadWindow(new II2cDeviceClient.ReadWindow(vector.getValue(), 6, readMode)); + // Ensure that the 6 bytes for this vector are visible in the register window. + ensureReadWindow(new II2cDeviceClient.ReadWindow(vector.getValue(), 6, readMode)); - // Read the data - return deviceClient.readTimeStamped(vector.getValue(), 6); - } - }); + // Read the data + return deviceClient.readTimeStamped(vector.getValue(), 6); } //------------------------------------------------------------------------------------------ @@ -588,26 +588,14 @@ class AccelerationManager implements IHandshakeable @Override public synchronized byte read8(final REGISTER reg) { - return this.deviceClient.executeFunctionWhileLocked(new IFunc() - { - @Override public Byte value() - { - ensureReadWindow(new II2cDeviceClient.ReadWindow(reg.bVal, 1, readMode)); - return deviceClient.read8(reg.bVal); - } - }); + ensureReadWindow(new II2cDeviceClient.ReadWindow(reg.bVal, 1, readMode)); + return deviceClient.read8(reg.bVal); } @Override public synchronized byte[] read(final REGISTER reg, final int cb) { - return this.deviceClient.executeFunctionWhileLocked(new IFunc() - { - @Override public byte[] value() - { - ensureReadWindow(new II2cDeviceClient.ReadWindow(reg.bVal, cb, readMode)); - return deviceClient.read(reg.bVal, cb); - } - }); + ensureReadWindow(new II2cDeviceClient.ReadWindow(reg.bVal, cb, readMode)); + return deviceClient.read(reg.bVal, cb); } @Override public void write8(REGISTER reg, int data) diff --git a/SwerveRoboticsLibrary/src/main/java/org/swerverobotics/library/internal/I2cDeviceClient.java b/SwerveRoboticsLibrary/src/main/java/org/swerverobotics/library/internal/I2cDeviceClient.java index 2af7710644d..ec54722a601 100644 --- a/SwerveRoboticsLibrary/src/main/java/org/swerverobotics/library/internal/I2cDeviceClient.java +++ b/SwerveRoboticsLibrary/src/main/java/org/swerverobotics/library/internal/I2cDeviceClient.java @@ -300,22 +300,6 @@ public void close() // Operations //---------------------------------------------------------------------------------------------- - @Override public void executeActionWhileLocked(Runnable action) - { - synchronized (this.concurrentClientLock) - { - action.run(); - } - } - - @Override public T executeFunctionWhileLocked(IFunc func) - { - synchronized (this.concurrentClientLock) - { - return func.value(); - } - } - /** * Sets the set of I2C device registers that we wish to read. */ @@ -332,16 +316,22 @@ public void close() else { // Remember the new window, but get a fresh copy so we can implement the read mode policy - this.readWindow = newWindow.freshCopy(); + setReadWindowInternal(newWindow.readableCopy()); assertTrue(!BuildConfig.DEBUG || (this.readWindow.isOkToRead() && this.readWindow.maySwitchToReadMode())); - - // Let others know of the update - this.readWindowChanged = true; } } } } + /** locks must be externally taken */ + private void setReadWindowInternal(ReadWindow newWindow) + { + this.readWindow = newWindow; + + // Let others (specifically, the callback) know of the update + this.readWindowChanged = true; + } + /** * Return the current register window. */ @@ -391,6 +381,10 @@ public void close() /** * Read a contiguous set of registers. + * + * This is the core read routine. Note that the current read window is never + * adjusted or invalidated by the execution of this function; that helps support + * concurrent clients. */ @Override public TimestampedData readTimeStamped(int ireg, int creg) { @@ -411,6 +405,9 @@ public void close() this.callbackLock.wait(); } + // Remember what the read window was on entry so we can restore it later if needed + ReadWindow prevReadWindow = this.readWindow; + // Is what's in the read cache right now or shortly will be have what we want? if (readCacheValidityCurrentOrImminent() && readWindowActuallyRead != null && readWindowActuallyRead.contains(ireg, creg)) { @@ -435,7 +432,7 @@ public void close() if (readWindowRangeOk) { // log(Log.VERBOSE, String.format("reuse window: (0x%02x,%d)", ireg, creg)); - setReadWindow(this.readWindow); + setReadWindow(this.readWindow); // will make a readable copy } else { @@ -474,6 +471,12 @@ public void close() // readCacheStatus or writeCacheStatus if (this.readCacheStatus==READ_CACHE_STATUS.VALID_ONLYONCE) this.readCacheStatus=READ_CACHE_STATUS.IDLE; + + // Restore any read window that we may have disturbed + if (this.readWindow != prevReadWindow) + { + setReadWindowInternal(prevReadWindow); + } } } } @@ -489,13 +492,8 @@ public void close() @Override public TimestampedData readTimeStamped(final int ireg, final int creg, final ReadWindow readWindowNeeded, final ReadWindow readWindowSet) { - return this.executeFunctionWhileLocked(new IFunc() { - @Override public TimestampedData value() - { - ensureReadWindow(readWindowNeeded, readWindowSet); - return readTimeStamped(ireg, creg); - } - }); + ensureReadWindow(readWindowNeeded, readWindowSet); + return readTimeStamped(ireg, creg); } private boolean readCacheValidityCurrentOrImminent()