private void waitForWritesToChannelToDrain(int channel) {
try {
SynchronousOpMode.synchronousThreadWaitForLoopCycleEmptyOfActionKey(
this.getChannelWriteKey(channel));
} catch (InterruptedException e) {
throw SwerveRuntimeException.wrap(e);
}
}
/** Read a contiguous set of registers. */
@Override
public TimestampedData readTimeStamped(int ireg, int creg) {
try {
synchronized (this.concurrentClientLock) {
synchronized (this.callbackLock) {
// Wait until the write cache isn't busy. This honors the visibility semantic
// we intend to portray, namely that issuing a read after a write has been
// issued will see the state AFTER the write has had a chance to take effect.
while (this.writeCacheStatus != WRITE_CACHE_STATUS.IDLE) {
this.callbackLock.wait();
}
// If there's no read window given or what's there either can't service any
// more reads or it doesn't contain the required registers, auto-make a new window.
if (this.readWindow == null
|| !this.readWindow.isOkToRead()
|| !this.readWindow.contains(ireg, creg)) {
// If we can re-use the window that was there before that will help increase
// the chance that we don't need to take the time to switch the controller to
// read mode (with a different window) and thus can respond faster.
if (this.readWindow != null && this.readWindow.contains(ireg, creg)) {
setReadWindow(this.readWindow);
} else {
// Make a one-shot that just covers the data we need right now
setReadWindow(new ReadWindow(ireg, creg, READ_MODE.ONLY_ONCE));
}
}
// We can only fetch registers that lie within the current register window.
// This actually should never trigger now, as the above should ALWAYS auto-adjust
// the window if necessary, but we have it here still as a check.
if (!this.readWindow.contains(ireg, creg)) {
throw new IllegalArgumentException(
String.format(
"read request (%d,%d) outside of read window (%d, %d)",
ireg, creg, this.readWindow.getIregFirst(), this.readWindow.getCreg()));
}
// Wait until the read cache is valid
while (this.readWindowChanged || !this.readCacheStatus.isValid()) {
this.callbackLock.wait();
}
// Extract the data and return!
this.readCacheLock.lockInterruptibly();
try {
assertTrue(!BuildConfig.DEBUG || this.readWindowActuallyRead.contains(this.readWindow));
// The data of interest is somewhere in the read window, but not necessarily at the
// start.
int ibFirst = ireg - this.readWindowActuallyRead.getIregFirst() + dibCacheOverhead;
TimestampedData result = new TimestampedData();
result.data = Arrays.copyOfRange(this.readCache, ibFirst, ibFirst + creg);
result.nanoTime = this.nanoTimeReadCacheValid;
return result;
} finally {
this.readCacheLock.unlock();
// If that was a one-time read, invalidate the data so we won't read it again a second
// time.
// Note that this is the only place outside of the callback that we ever update
// readCacheStatus or writeCacheStatus
if (this.readCacheStatus == READ_CACHE_STATUS.VALID_ONLYONCE)
this.readCacheStatus = READ_CACHE_STATUS.IDLE;
}
}
}
} catch (InterruptedException e) {
Util.handleCapturedInterrupt(e);
// Can't return (no data to return!) so we must throw
throw SwerveRuntimeException.wrap(e);
}
}