private void waitForWritesToChannelToDrain(int channel) {
   try {
     SynchronousOpMode.synchronousThreadWaitForLoopCycleEmptyOfActionKey(
         this.getChannelWriteKey(channel));
   } catch (InterruptedException e) {
     throw SwerveRuntimeException.wrap(e);
   }
 }
Exemple #2
0
  /** 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);
    }
  }