From 82fd77d95ebe4725d19f445e3a3044f0d3c3636c Mon Sep 17 00:00:00 2001 From: RobertAtkinson Date: Mon, 28 Sep 2015 11:46:35 -0700 Subject: [PATCH 1/7] comments Former-commit-id: 2a3f423b8f7e104a504f1f1440b9a8e112fb7296 Former-commit-id: e088351b0038b689ebd09bfa9bcf509ea7c37f74 --- .../swerverobotics/library/internal/AdaFruitBNO055IMU.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) 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 f18450fc03f..58978660182 100644 --- a/SwerveRoboticsLibrary/src/main/java/org/swerverobotics/library/internal/AdaFruitBNO055IMU.java +++ b/SwerveRoboticsLibrary/src/main/java/org/swerverobotics/library/internal/AdaFruitBNO055IMU.java @@ -706,11 +706,12 @@ private void ensureReadWindow(II2cDeviceClient.ReadWindow needed) } // Our write logic doesn't actually know when the I2C writes are issued. All it knows is - // when it has queued the write to the USB Core Device Interface Module. It's a pretty + // when the write has made it to the USB Core Device Interface Module. It's a pretty // deterministic interval after that that the I2C write occurs, we guess, but we don't // really know what that is. To account for this, we slop in some extra time to the // delays so that we're not cutting things too close to the edge. And given that this is - // initialization logic and so not time critical, we err on being generous. + // initialization logic and so not time critical, we err on being generous: the current + // setting of this extra can undoubtedly be reduced. private final static int msExtra = 50; From 880ce48bd6f98d5355501d91316377471668954a Mon Sep 17 00:00:00 2001 From: RobertAtkinson Date: Mon, 28 Sep 2015 12:04:49 -0700 Subject: [PATCH 2/7] remove default use of thread priority boost Former-commit-id: f162a4d3473e58012cfd7cd93933b487ea0ff649 Former-commit-id: 643bc007d8422bccd847c18db25bb7f3f77f2f8e --- .../java/org/swerverobotics/library/interfaces/IBNO055IMU.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 ec585beb8a0..21f5141cba3 100644 --- a/SwerveRoboticsLibrary/src/main/java/org/swerverobotics/library/interfaces/IBNO055IMU.java +++ b/SwerveRoboticsLibrary/src/main/java/org/swerverobotics/library/interfaces/IBNO055IMU.java @@ -63,7 +63,7 @@ class Parameters /** the boost in thread priority to use for data acquisition. A small increase in the * thread priority can help reduce timestamping jitter and improve acceleration integration * at only a small detriment to other parts of the system. */ - public int threadPriorityBoost = 1; + public int threadPriorityBoost = 0; /** debugging aid: enable logging for this device? */ public boolean loggingEnabled = false; From 0f278f571e76ee888e1b38dd29e73de269975b6e Mon Sep 17 00:00:00 2001 From: RobertAtkinson Date: Mon, 28 Sep 2015 12:17:02 -0700 Subject: [PATCH 3/7] comments Former-commit-id: c61cc44a15a314de8d646c174d4e44dee3be7140 Former-commit-id: 72dda5069cb221ec894f8137104914563716a3b7 --- .../org/swerverobotics/library/internal/I2cDeviceClient.java | 2 ++ 1 file changed, 2 insertions(+) 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 cc9f18af735..65d3917aed5 100644 --- a/SwerveRoboticsLibrary/src/main/java/org/swerverobotics/library/internal/I2cDeviceClient.java +++ b/SwerveRoboticsLibrary/src/main/java/org/swerverobotics/library/internal/I2cDeviceClient.java @@ -868,6 +868,8 @@ else if (readCacheStatus == READ_CACHE_STATUS.VALID_ONLYONCE) // In all cases, we want to read the latest from the controller to get read // vs write mode settings, if nothing else. + // TODO: Review this, as it may in fact be the case that the mode status + // settings are read automatically for you. queueRead = true; //---------------------------------------------------------------------------------- From dc8c3a1647dff78447235c90f13181ee9901eb88 Mon Sep 17 00:00:00 2001 From: RobertAtkinson Date: Mon, 28 Sep 2015 13:20:12 -0700 Subject: [PATCH 4/7] comments Former-commit-id: e9295346ac7bc3d5345a21bc26aa5f26fd7fe1e0 Former-commit-id: b257f4d6130be10602d036c64269323072a780f9 --- .../swerverobotics/library/internal/I2cDeviceClient.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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 65d3917aed5..d4b3dbb5a32 100644 --- a/SwerveRoboticsLibrary/src/main/java/org/swerverobotics/library/internal/I2cDeviceClient.java +++ b/SwerveRoboticsLibrary/src/main/java/org/swerverobotics/library/internal/I2cDeviceClient.java @@ -866,10 +866,10 @@ else if (readCacheStatus == READ_CACHE_STATUS.VALID_ONLYONCE) //-------------------------------------------------------------------------- // In all cases, we want to read the latest from the controller to get read - // vs write mode settings, if nothing else. + // vs write mode settings, if nothing else. Remember (for those confused) + // this only causes bytes to be read from the USB module to the phone; an I2C + // read isn't issued unless the action flag is ALSO set. - // TODO: Review this, as it may in fact be the case that the mode status - // settings are read automatically for you. queueRead = true; //---------------------------------------------------------------------------------- From 592bafe560603f33b061af844996c6eb0ef281d1 Mon Sep 17 00:00:00 2001 From: RobertAtkinson Date: Tue, 29 Sep 2015 09:34:46 -0700 Subject: [PATCH 5/7] prep for reduced I2C segment reading Former-commit-id: 516ada0265041b3f2f33cde40b7d295b89eeaa9f Former-commit-id: d789a765635efb6e2ba63e9ca1458b97d220ffa0 --- .../library/internal/I2cDeviceClient.java | 28 ++++++++++++++++--- 1 file changed, 24 insertions(+), 4 deletions(-) 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 d4b3dbb5a32..bc9351210f1 100644 --- a/SwerveRoboticsLibrary/src/main/java/org/swerverobotics/library/internal/I2cDeviceClient.java +++ b/SwerveRoboticsLibrary/src/main/java/org/swerverobotics/library/internal/I2cDeviceClient.java @@ -638,7 +638,8 @@ void startSwitchingToReadMode(ReadWindow window) readWindowSentToControllerInitialized = true; setActionFlag = true; // causes an I2C read to happen - queueFullWrite = true; // for just the mode bytes + queueFullWrite = true; // for just the mod bytes + queueRead = true; // read the mode byte so we can tell when the switch is done dirtyModeCacheStatus(); } @@ -654,7 +655,8 @@ void issueWrite() readWindowSentToControllerInitialized = true; setActionFlag = true; // causes the I2C write to happen - queueFullWrite = true; // for the mode bytes and the payload + queueFullWrite = true; // for the mode byte and the payload + queueRead = true; // *read* the mode byte too so that isI2cPortInReadMode() will report correctly dirtyModeCacheStatus(); } @@ -762,7 +764,11 @@ void updateStateMachines(UPDATE_STATE_MACHINE caller) } if (writeCacheStatus == WRITE_CACHE_STATUS.QUEUED) + { writeCacheStatus = WRITE_CACHE_STATUS.IDLE; + // Our write mode status should have been reported back to us + // assertTrue(!BuildConfig.DEBUG || i2cDevice.isI2cPortInWriteMode()); // ABCDEF + } //-------------------------------------------------------------------------- // That limits the number of states the caches can now be in @@ -784,7 +790,12 @@ void updateStateMachines(UPDATE_STATE_MACHINE caller) { // See also below XYZZY readCacheStatus = READ_CACHE_STATUS.QUEUED; - setActionFlag = true; // actually do an I2C read + setActionFlag = true; // actually do an I2C read + queueRead = true; // read the I2C read results + } + else + { + queueRead = true; // read the read-vs-write mode byte } } @@ -820,7 +831,8 @@ else if (readCacheStatus == READ_CACHE_STATUS.IDLE || readWindowChanged) // See also above XYZZY readWindowActuallyRead = readWindowSentToController; readCacheStatus = READ_CACHE_STATUS.QUEUED; - setActionFlag = true; // actually do an I2C read + setActionFlag = true; // actually do an I2C read + queueRead = true; // read the results of the read } else { @@ -849,6 +861,7 @@ else if (readCacheStatus == READ_CACHE_STATUS.QUEUE_COMPLETED) { readCacheStatus = READ_CACHE_STATUS.VALID_QUEUED; setActionFlag = true; // actually do an I2C read + queueRead = true; // read the results of the read } else { @@ -870,6 +883,13 @@ else if (readCacheStatus == READ_CACHE_STATUS.VALID_ONLYONCE) // this only causes bytes to be read from the USB module to the phone; an I2C // read isn't issued unless the action flag is ALSO set. + // TODO: we only need the mode byte when we're switching modes, not all the time. + // The other time we need to do a read is to read I2C payload. So we can probably + // remove this line now, as the above logic now sets it in each place it's needed. + // But that hasn't been tested enough yet, so for the moment we continue to read + // every time. If this line IS removed, be sure to turn on the assert above + // labeled ABCDEF. + queueRead = true; //---------------------------------------------------------------------------------- From 4ac84f3ba24276acc8250acfe4d0442f6aacc0a4 Mon Sep 17 00:00:00 2001 From: RobertAtkinson Date: Wed, 30 Sep 2015 22:23:12 -0700 Subject: [PATCH 6/7] optimize I2C reading from CDIM Former-commit-id: b00191d392ca11c5e4f2dbaa1d8a0ddaa1a72a88 Former-commit-id: ff591f4655531b58244f66037471e55c7e80ceb8 --- .../library/internal/I2cDeviceClient.java | 21 +++---------------- 1 file changed, 3 insertions(+), 18 deletions(-) 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 4398735d2b3..b09d697ae47 100644 --- a/SwerveRoboticsLibrary/src/main/java/org/swerverobotics/library/internal/I2cDeviceClient.java +++ b/SwerveRoboticsLibrary/src/main/java/org/swerverobotics/library/internal/I2cDeviceClient.java @@ -656,7 +656,7 @@ void issueWrite() setActionFlag = true; // causes the I2C write to happen queueFullWrite = true; // for the mode byte and the payload - queueRead = true; // *read* the mode byte too so that isI2cPortInReadMode() will report correctly + queueRead = true; // read the mode byte too so that isI2cPortInReadMode() will report correctly dirtyModeCacheStatus(); } @@ -767,7 +767,7 @@ void updateStateMachines(UPDATE_STATE_MACHINE caller) { writeCacheStatus = WRITE_CACHE_STATUS.IDLE; // Our write mode status should have been reported back to us - // assertTrue(!BuildConfig.DEBUG || i2cDevice.isI2cPortInWriteMode()); // ABCDEF + assertTrue(!BuildConfig.DEBUG || i2cDevice.isI2cPortInWriteMode()); } //-------------------------------------------------------------------------- @@ -795,7 +795,7 @@ void updateStateMachines(UPDATE_STATE_MACHINE caller) } else { - queueRead = true; // read the read-vs-write mode byte + queueRead = true; // read the mode byte } } @@ -877,21 +877,6 @@ else if (readCacheStatus == READ_CACHE_STATUS.VALID_ONLYONCE) // Just leave it there until someone reads it } - //-------------------------------------------------------------------------- - // In all cases, we want to read the latest from the controller to get read - // vs write mode settings, if nothing else. Remember (for those confused) - // this only causes bytes to be read from the USB module to the phone; an I2C - // read isn't issued unless the action flag is ALSO set. - - // TODO: we only need the mode byte when we're switching modes, not all the time. - // The other time we need to do a read is to read I2C payload. So we can probably - // remove this line now, as the above logic now sets it in each place it's needed. - // But that hasn't been tested enough yet, so for the moment we continue to read - // every time. If this line IS removed, be sure to turn on the assert above - // labeled ABCDEF. - - queueRead = true; - //---------------------------------------------------------------------------------- // Ok, after all that we finally know what how we're required to // interact with the device controller according to what we've been From ca52f8eb12ee46d2925eff76628b9c182678caca Mon Sep 17 00:00:00 2001 From: RobertAtkinson Date: Wed, 30 Sep 2015 22:24:31 -0700 Subject: [PATCH 7/7] comment typo Former-commit-id: c5bd2f4ab0c1c9890a5ab2effa0c78e680771fb6 Former-commit-id: d01889efe95deecd7b981064334c3a6a8416a23a --- .../org/swerverobotics/library/internal/I2cDeviceClient.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 b09d697ae47..3b1ed5fa4a1 100644 --- a/SwerveRoboticsLibrary/src/main/java/org/swerverobotics/library/internal/I2cDeviceClient.java +++ b/SwerveRoboticsLibrary/src/main/java/org/swerverobotics/library/internal/I2cDeviceClient.java @@ -638,7 +638,7 @@ void startSwitchingToReadMode(ReadWindow window) readWindowSentToControllerInitialized = true; setActionFlag = true; // causes an I2C read to happen - queueFullWrite = true; // for just the mod bytes + queueFullWrite = true; // for just the mode bytes queueRead = true; // read the mode byte so we can tell when the switch is done dirtyModeCacheStatus();