diff --git a/hal/src/main/native/athena/SPI.cpp b/hal/src/main/native/athena/SPI.cpp index d5fd2ec3bd..b6977dd3aa 100644 --- a/hal/src/main/native/athena/SPI.cpp +++ b/hal/src/main/native/athena/SPI.cpp @@ -15,7 +15,6 @@ #include #include #include -#include #include #include @@ -23,7 +22,6 @@ #include "DigitalInternal.h" #include "HAL/DIO.h" #include "HAL/HAL.h" -#include "HAL/Notifier.h" #include "HAL/cpp/make_unique.h" #include "HAL/handles/HandlesInternal.h" @@ -43,34 +41,24 @@ static std::array spiApiMutexes; static std::array spiAccumulatorMutexes; // MXP SPI does not count towards this -std::atomic spiPortCount{0}; +static std::atomic spiPortCount{0}; static HAL_DigitalHandle digitalHandles[9]{HAL_kInvalidHandle}; -struct SPIAccumulator { - std::atomic notifier{0}; - uint64_t triggerTime; - int32_t period; +static wpi::mutex spiAutoMutex; +static int32_t spiAutoPort = kSpiMaxHandles; +static std::atomic_bool spiAutoRunning{false}; +static std::unique_ptr spiAutoDMA; - int64_t value = 0; - uint32_t count = 0; - int32_t lastValue = 0; - - int32_t center = 0; - int32_t deadband = 0; - - uint8_t cmd[4]; // command to send (up to 4 bytes) - int32_t validMask; - int32_t validValue; - int32_t dataMax; // one more than max data value - int32_t dataMsbMask; // data field MSB mask (for signed) - uint8_t dataShift; // data field shift right amount, in bits - uint8_t xferSize; // SPI transfer size, in bytes (up to 4) - HAL_SPIPort port; - bool isSigned; // is data field signed? - bool bigEndian; // is response big endian? -}; -std::unique_ptr spiAccumulators[5]; +static bool SPIInUseByAuto(HAL_SPIPort port) { + // SPI engine conflicts with any other chip selects on the same SPI device. + // There are two SPI devices: one for ports 0-3 (onboard), the other for port + // 4 (MXP). + if (!spiAutoRunning) return false; + std::lock_guard lock(spiAutoMutex); + return (spiAutoPort >= 0 && spiAutoPort <= 3 && port >= 0 && port <= 3) || + (spiAutoPort == 4 && port == 4); +} namespace hal { namespace init { @@ -273,6 +261,8 @@ int32_t HAL_TransactionSPI(HAL_SPIPort port, const uint8_t* dataToSend, return -1; } + if (SPIInUseByAuto(port)) return -1; + struct spi_ioc_transfer xfer; std::memset(&xfer, 0, sizeof(xfer)); xfer.tx_buf = (__u64)dataToSend; @@ -299,6 +289,8 @@ int32_t HAL_WriteSPI(HAL_SPIPort port, const uint8_t* dataToSend, return -1; } + if (SPIInUseByAuto(port)) return -1; + struct spi_ioc_transfer xfer; std::memset(&xfer, 0, sizeof(xfer)); xfer.tx_buf = (__u64)dataToSend; @@ -326,6 +318,8 @@ int32_t HAL_ReadSPI(HAL_SPIPort port, uint8_t* buffer, int32_t count) { return -1; } + if (SPIInUseByAuto(port)) return -1; + struct spi_ioc_transfer xfer; std::memset(&xfer, 0, sizeof(xfer)); xfer.rx_buf = (__u64)buffer; @@ -346,7 +340,7 @@ void HAL_CloseSPI(HAL_SPIPort port) { } int32_t status = 0; - HAL_FreeSPIAccumulator(port, &status); + HAL_FreeSPIAuto(port, &status); { std::lock_guard lock(spiApiMutexes[port]); @@ -522,313 +516,202 @@ void HAL_SetSPIHandle(HAL_SPIPort port, int32_t handle) { } } -static void spiAccumulatorProcess(uint64_t currentTime, SPIAccumulator* accum) { - // perform SPI transaction - uint8_t resp_b[4]; - HAL_TransactionSPI(accum->port, accum->cmd, resp_b, accum->xferSize); +void HAL_InitSPIAuto(HAL_SPIPort port, int32_t bufferSize, int32_t* status) { + if (port < 0 || port >= kSpiMaxHandles) { + *status = PARAMETER_OUT_OF_RANGE; + return; + } - // convert from bytes - uint32_t resp = 0; - if (accum->bigEndian) { - for (int32_t i = 0; i < accum->xferSize; ++i) { - resp <<= 8; - resp |= resp_b[i] & 0xff; - } + std::lock_guard lock(spiAutoMutex); + // FPGA only has one auto SPI engine + if (spiAutoPort != kSpiMaxHandles) { + *status = RESOURCE_IS_ALLOCATED; + return; + } + + // remember the initialized port for other entry points + spiAutoPort = port; + + // configure the correct chip select + if (port < 4) { + spiSystem->writeAutoSPI1Select(false, status); + spiSystem->writeAutoChipSelect(port, status); } else { - for (int32_t i = accum->xferSize - 1; i >= 0; --i) { - resp <<= 8; - resp |= resp_b[i] & 0xff; - } + spiSystem->writeAutoSPI1Select(true, status); + spiSystem->writeAutoChipSelect(0, status); } - // process response - if ((resp & accum->validMask) == static_cast(accum->validValue)) { - // valid sensor data; extract data field - int32_t data = static_cast(resp >> accum->dataShift); - data &= accum->dataMax - 1; - // 2s complement conversion if signed MSB is set - if (accum->isSigned && (data & accum->dataMsbMask) != 0) - data -= accum->dataMax; - // center offset - data -= accum->center; - // only accumulate if outside deadband - if (data < -accum->deadband || data > accum->deadband) accum->value += data; - ++accum->count; - accum->lastValue = data; - } else { - // no data from the sensor; just clear the last value - accum->lastValue = 0; - } - - // reschedule timer - accum->triggerTime += accum->period; - // handle timer slip - if (accum->triggerTime < currentTime) - accum->triggerTime = currentTime + accum->period; - int32_t status = 0; - HAL_UpdateNotifierAlarm(accum->notifier, accum->triggerTime, &status); + // configure DMA + tDMAChannelDescriptor desc; + spiSystem->getSystemInterface()->getDmaDescriptor(g_SpiAutoData_index, &desc); + spiAutoDMA = std::make_unique(desc.channel, bufferSize, status); } -/** - * Initialize a SPI accumulator. - * - * @param port SPI port - * @param period Time between reads, in us - * @param cmd SPI command to send to request data - * @param xferSize SPI transfer size, in bytes - * @param validMask Mask to apply to received data for validity checking - * @param valid_data After validMask is applied, required matching value for - * validity checking - * @param dataShift Bit shift to apply to received data to get actual data - * value - * @param dataSize Size (in bits) of data field - * @param isSigned Is data field signed? - * @param bigEndian Is device big endian? - */ -void HAL_InitSPIAccumulator(HAL_SPIPort port, int32_t period, int32_t cmd, - int32_t xferSize, int32_t validMask, - int32_t validValue, int32_t dataShift, - int32_t dataSize, HAL_Bool isSigned, - HAL_Bool bigEndian, int32_t* status) { +void HAL_FreeSPIAuto(HAL_SPIPort port, int32_t* status) { if (port < 0 || port >= kSpiMaxHandles) { *status = PARAMETER_OUT_OF_RANGE; return; } - std::lock_guard lock(spiAccumulatorMutexes[port]); - if (!spiAccumulators[port]) - spiAccumulators[port] = std::make_unique(); - SPIAccumulator* accum = spiAccumulators[port].get(); - if (bigEndian) { - for (int32_t i = xferSize - 1; i >= 0; --i) { - accum->cmd[i] = cmd & 0xff; - cmd >>= 8; - } - } else { - accum->cmd[0] = cmd & 0xff; - cmd >>= 8; - accum->cmd[1] = cmd & 0xff; - cmd >>= 8; - accum->cmd[2] = cmd & 0xff; - cmd >>= 8; - accum->cmd[3] = cmd & 0xff; - } - accum->period = period; - accum->xferSize = xferSize; - accum->validMask = validMask; - accum->validValue = validValue; - accum->dataShift = dataShift; - accum->dataMax = (1 << dataSize); - accum->dataMsbMask = (1 << (dataSize - 1)); - accum->isSigned = isSigned; - accum->bigEndian = bigEndian; - accum->port = port; - if (!accum->notifier) { - accum->notifier = HAL_InitializeNotifier(status); - accum->triggerTime = HAL_GetFPGATime(status) + period; - if (*status != 0) return; - std::thread thr([=] { - int32_t status2 = 0; - while (status2 == 0) { - uint64_t curTime = HAL_WaitForNotifierAlarm(accum->notifier, &status2); - if (curTime == 0 || status2 != 0) break; - spiAccumulatorProcess(curTime, accum); - } - }); - thr.detach(); - HAL_UpdateNotifierAlarm(accum->notifier, accum->triggerTime, status); - } + std::lock_guard lock(spiAutoMutex); + if (spiAutoPort != port) return; + spiAutoPort = kSpiMaxHandles; + + // disable by setting to internal clock and setting rate=0 + spiSystem->writeAutoRate(0, status); + spiSystem->writeAutoTriggerConfig_ExternalClock(false, status); + + // stop the DMA + spiAutoDMA->stop(status); + + spiAutoDMA.reset(nullptr); + + spiAutoRunning = false; } -/** - * Frees a SPI accumulator. - */ -void HAL_FreeSPIAccumulator(HAL_SPIPort port, int32_t* status) { - if (port < 0 || port >= kSpiMaxHandles) { +void HAL_StartSPIAutoRate(HAL_SPIPort port, double period, int32_t* status) { + std::lock_guard lock(spiAutoMutex); + // FPGA only has one auto SPI engine + if (port != spiAutoPort) { + *status = INCOMPATIBLE_STATE; + return; + } + + spiAutoRunning = true; + + // start the DMA + spiAutoDMA->start(status); + + // auto rate is in microseconds + spiSystem->writeAutoRate(period * 1000000, status); + + // disable the external clock + spiSystem->writeAutoTriggerConfig_ExternalClock(false, status); +} + +void HAL_StartSPIAutoTrigger(HAL_SPIPort port, HAL_Handle digitalSourceHandle, + HAL_AnalogTriggerType analogTriggerType, + HAL_Bool triggerRising, HAL_Bool triggerFalling, + int32_t* status) { + std::lock_guard lock(spiAutoMutex); + // FPGA only has one auto SPI engine + if (port != spiAutoPort) { + *status = INCOMPATIBLE_STATE; + return; + } + + spiAutoRunning = true; + + // start the DMA + spiAutoDMA->start(status); + + // get channel routing + bool routingAnalogTrigger = false; + uint8_t routingChannel = 0; + uint8_t routingModule = 0; + if (!remapDigitalSource(digitalSourceHandle, analogTriggerType, + routingChannel, routingModule, + routingAnalogTrigger)) { + *status = HAL_HANDLE_ERROR; + return; + } + + // configure external trigger and enable it + tSPI::tAutoTriggerConfig config; + config.ExternalClock = 1; + config.FallingEdge = triggerFalling ? 1 : 0; + config.RisingEdge = triggerRising ? 1 : 0; + config.ExternalClockSource_AnalogTrigger = routingAnalogTrigger ? 1 : 0; + config.ExternalClockSource_Module = routingModule; + config.ExternalClockSource_Channel = routingChannel; + spiSystem->writeAutoTriggerConfig(config, status); +} + +void HAL_StopSPIAuto(HAL_SPIPort port, int32_t* status) { + std::lock_guard lock(spiAutoMutex); + // FPGA only has one auto SPI engine + if (port != spiAutoPort) { + *status = INCOMPATIBLE_STATE; + return; + } + + // disable by setting to internal clock and setting rate=0 + spiSystem->writeAutoRate(0, status); + spiSystem->writeAutoTriggerConfig_ExternalClock(false, status); + + // stop the DMA + spiAutoDMA->stop(status); + + spiAutoRunning = false; +} + +void HAL_SetSPIAutoTransmitData(HAL_SPIPort port, const uint8_t* dataToSend, + int32_t dataSize, int32_t zeroSize, + int32_t* status) { + if (dataSize < 0 || dataSize > 16) { *status = PARAMETER_OUT_OF_RANGE; return; } - std::lock_guard lock(spiAccumulatorMutexes[port]); - SPIAccumulator* accum = spiAccumulators[port].get(); - if (!accum) { - *status = NULL_PARAMETER; - return; - } - HAL_NotifierHandle handle = accum->notifier.exchange(0); - HAL_CleanNotifier(handle, status); - spiAccumulators[port] = nullptr; -} - -/** - * Resets the accumulator to zero. - */ -void HAL_ResetSPIAccumulator(HAL_SPIPort port, int32_t* status) { - if (port < 0 || port >= kSpiMaxHandles) { + if (zeroSize < 0 || zeroSize > 127) { *status = PARAMETER_OUT_OF_RANGE; return; } - std::lock_guard lock(spiApiMutexes[port]); - SPIAccumulator* accum = spiAccumulators[port].get(); - if (!accum) { - *status = NULL_PARAMETER; + std::lock_guard lock(spiAutoMutex); + // FPGA only has one auto SPI engine + if (port != spiAutoPort) { + *status = INCOMPATIBLE_STATE; return; } - accum->value = 0; - accum->count = 0; - accum->lastValue = 0; + + // set tx data registers + for (int32_t i = 0; i < dataSize; ++i) + spiSystem->writeAutoTx(i >> 2, i & 3, dataToSend[i], status); + + // set byte counts + tSPI::tAutoByteCount config; + config.ZeroByteCount = static_cast(zeroSize) & 0x7f; + config.TxByteCount = static_cast(dataSize) & 0xf; + spiSystem->writeAutoByteCount(config, status); } -/** - * Set the center value of the accumulator. - * - * The center value is subtracted from each value before it is added to the - * accumulator. This - * is used for the center value of devices like gyros and accelerometers to make - * integration work - * and to take the device offset into account when integrating. - */ -void HAL_SetSPIAccumulatorCenter(HAL_SPIPort port, int32_t center, - int32_t* status) { - if (port < 0 || port >= kSpiMaxHandles) { - *status = PARAMETER_OUT_OF_RANGE; +void HAL_ForceSPIAutoRead(HAL_SPIPort port, int32_t* status) { + std::lock_guard lock(spiAutoMutex); + // FPGA only has one auto SPI engine + if (port != spiAutoPort) { + *status = INCOMPATIBLE_STATE; return; } - std::lock_guard lock(spiAccumulatorMutexes[port]); - SPIAccumulator* accum = spiAccumulators[port].get(); - if (!accum) { - *status = NULL_PARAMETER; - return; - } - accum->center = center; + spiSystem->strobeAutoForceOne(status); } -/** - * Set the accumulator's deadband. - */ -void HAL_SetSPIAccumulatorDeadband(HAL_SPIPort port, int32_t deadband, - int32_t* status) { - if (port < 0 || port >= kSpiMaxHandles) { - *status = PARAMETER_OUT_OF_RANGE; - return; - } - - std::lock_guard lock(spiAccumulatorMutexes[port]); - SPIAccumulator* accum = spiAccumulators[port].get(); - if (!accum) { - *status = NULL_PARAMETER; - return; - } - accum->deadband = deadband; -} - -/** - * Read the last value read by the accumulator engine. - */ -int32_t HAL_GetSPIAccumulatorLastValue(HAL_SPIPort port, int32_t* status) { - if (port < 0 || port >= kSpiMaxHandles) { - *status = PARAMETER_OUT_OF_RANGE; +int32_t HAL_ReadSPIAutoReceivedData(HAL_SPIPort port, uint8_t* buffer, + int32_t numToRead, double timeout, + int32_t* status) { + std::lock_guard lock(spiAutoMutex); + // FPGA only has one auto SPI engine + if (port != spiAutoPort) { + *status = INCOMPATIBLE_STATE; return 0; } - std::lock_guard lock(spiAccumulatorMutexes[port]); - SPIAccumulator* accum = spiAccumulators[port].get(); - if (!accum) { - *status = NULL_PARAMETER; - return 0; - } - return accum->lastValue; + size_t numRemaining = 0; + // timeout is in ms + spiAutoDMA->read(buffer, numToRead, timeout * 1000, &numRemaining, status); + return numRemaining; } -/** - * Read the accumulated value. - * - * @return The 64-bit value accumulated since the last Reset(). - */ -int64_t HAL_GetSPIAccumulatorValue(HAL_SPIPort port, int32_t* status) { - if (port < 0 || port >= kSpiMaxHandles) { - *status = PARAMETER_OUT_OF_RANGE; +int32_t HAL_GetSPIAutoDroppedCount(HAL_SPIPort port, int32_t* status) { + std::lock_guard lock(spiAutoMutex); + // FPGA only has one auto SPI engine + if (port != spiAutoPort) { + *status = INCOMPATIBLE_STATE; return 0; } - std::lock_guard lock(spiAccumulatorMutexes[port]); - SPIAccumulator* accum = spiAccumulators[port].get(); - if (!accum) { - *status = NULL_PARAMETER; - return 0; - } - return accum->value; -} - -/** - * Read the number of accumulated values. - * - * Read the count of the accumulated values since the accumulator was last - * Reset(). - * - * @return The number of times samples from the channel were accumulated. - */ -int64_t HAL_GetSPIAccumulatorCount(HAL_SPIPort port, int32_t* status) { - if (port < 0 || port >= kSpiMaxHandles) { - *status = PARAMETER_OUT_OF_RANGE; - return 0; - } - - std::lock_guard lock(spiAccumulatorMutexes[port]); - SPIAccumulator* accum = spiAccumulators[port].get(); - if (!accum) { - *status = NULL_PARAMETER; - return 0; - } - return accum->count; -} - -/** - * Read the average of the accumulated value. - * - * @return The accumulated average value (value / count). - */ -double HAL_GetSPIAccumulatorAverage(HAL_SPIPort port, int32_t* status) { - if (port < 0 || port >= kSpiMaxHandles) { - *status = PARAMETER_OUT_OF_RANGE; - return 0.0; - } - - int64_t value; - int64_t count; - HAL_GetSPIAccumulatorOutput(port, &value, &count, status); - if (count == 0) return 0.0; - return static_cast(value) / count; -} - -/** - * Read the accumulated value and the number of accumulated values atomically. - * - * This function reads the value and count atomically. - * This can be used for averaging. - * - * @param value Pointer to the 64-bit accumulated output. - * @param count Pointer to the number of accumulation cycles. - */ -void HAL_GetSPIAccumulatorOutput(HAL_SPIPort port, int64_t* value, - int64_t* count, int32_t* status) { - if (port < 0 || port >= kSpiMaxHandles) { - *status = PARAMETER_OUT_OF_RANGE; - return; - } - - std::lock_guard lock(spiAccumulatorMutexes[port]); - SPIAccumulator* accum = spiAccumulators[port].get(); - if (!accum) { - *status = NULL_PARAMETER; - *value = 0; - *count = 0; - return; - } - *value = accum->value; - *count = accum->count; + return spiSystem->readTransferSkippedFullCount(status); } } // extern "C" diff --git a/hal/src/main/native/include/HAL/SPI.h b/hal/src/main/native/include/HAL/SPI.h index cd438f45fc..d1474b30ee 100644 --- a/hal/src/main/native/include/HAL/SPI.h +++ b/hal/src/main/native/include/HAL/SPI.h @@ -9,6 +9,7 @@ #include +#include "HAL/AnalogTrigger.h" #include "HAL/Types.h" enum HAL_SPIPort : int32_t { @@ -38,23 +39,23 @@ void HAL_SetSPIChipSelectActiveLow(HAL_SPIPort port, int32_t* status); int32_t HAL_GetSPIHandle(HAL_SPIPort port); void HAL_SetSPIHandle(HAL_SPIPort port, int32_t handle); -void HAL_InitSPIAccumulator(HAL_SPIPort port, int32_t period, int32_t cmd, - int32_t xferSize, int32_t validMask, - int32_t validValue, int32_t dataShift, - int32_t dataSize, HAL_Bool isSigned, - HAL_Bool bigEndian, int32_t* status); -void HAL_FreeSPIAccumulator(HAL_SPIPort port, int32_t* status); -void HAL_ResetSPIAccumulator(HAL_SPIPort port, int32_t* status); -void HAL_SetSPIAccumulatorCenter(HAL_SPIPort port, int32_t center, - int32_t* status); -void HAL_SetSPIAccumulatorDeadband(HAL_SPIPort port, int32_t deadband, - int32_t* status); -int32_t HAL_GetSPIAccumulatorLastValue(HAL_SPIPort port, int32_t* status); -int64_t HAL_GetSPIAccumulatorValue(HAL_SPIPort port, int32_t* status); -int64_t HAL_GetSPIAccumulatorCount(HAL_SPIPort port, int32_t* status); -double HAL_GetSPIAccumulatorAverage(HAL_SPIPort port, int32_t* status); -void HAL_GetSPIAccumulatorOutput(HAL_SPIPort port, int64_t* value, - int64_t* count, int32_t* status); +void HAL_InitSPIAuto(HAL_SPIPort port, int32_t bufferSize, int32_t* status); +void HAL_FreeSPIAuto(HAL_SPIPort port, int32_t* status); +void HAL_StartSPIAutoRate(HAL_SPIPort port, double period, int32_t* status); +void HAL_StartSPIAutoTrigger(HAL_SPIPort port, HAL_Handle digitalSourceHandle, + HAL_AnalogTriggerType analogTriggerType, + HAL_Bool triggerRising, HAL_Bool triggerFalling, + int32_t* status); +void HAL_StopSPIAuto(HAL_SPIPort port, int32_t* status); +void HAL_SetSPIAutoTransmitData(HAL_SPIPort port, const uint8_t* dataToSend, + int32_t dataSize, int32_t zeroSize, + int32_t* status); +void HAL_ForceSPIAutoRead(HAL_SPIPort port, int32_t* status); +int32_t HAL_ReadSPIAutoReceivedData(HAL_SPIPort port, uint8_t* buffer, + int32_t numToRead, double timeout, + int32_t* status); +int32_t HAL_GetSPIAutoDroppedCount(HAL_SPIPort port, int32_t* status); + #ifdef __cplusplus } // extern "C" #endif diff --git a/hal/src/main/native/sim/SPI.cpp b/hal/src/main/native/sim/SPI.cpp index 37c0d3e8e7..76b2b8076a 100644 --- a/hal/src/main/native/sim/SPI.cpp +++ b/hal/src/main/native/sim/SPI.cpp @@ -40,30 +40,23 @@ void HAL_SetSPIChipSelectActiveLow(HAL_SPIPort port, int32_t* status) {} int32_t HAL_GetSPIHandle(HAL_SPIPort port) { return 0; } void HAL_SetSPIHandle(HAL_SPIPort port, int32_t handle) {} -void HAL_InitSPIAccumulator(HAL_SPIPort port, int32_t period, int32_t cmd, - int32_t xferSize, int32_t validMask, - int32_t validValue, int32_t dataShift, - int32_t dataSize, HAL_Bool isSigned, - HAL_Bool bigEndian, int32_t* status) {} -void HAL_FreeSPIAccumulator(HAL_SPIPort port, int32_t* status) {} -void HAL_ResetSPIAccumulator(HAL_SPIPort port, int32_t* status) { - SimSPIData[port].ResetAccumulator(); -} -void HAL_SetSPIAccumulatorCenter(HAL_SPIPort port, int32_t center, - int32_t* status) {} -void HAL_SetSPIAccumulatorDeadband(HAL_SPIPort port, int32_t deadband, - int32_t* status) {} -int32_t HAL_GetSPIAccumulatorLastValue(HAL_SPIPort port, int32_t* status) { +void HAL_InitSPIAuto(HAL_SPIPort port, int32_t bufferSize, int32_t* status) {} +void HAL_FreeSPIAuto(HAL_SPIPort port, int32_t* status) {} +void HAL_StartSPIAutoRate(HAL_SPIPort port, double period, int32_t* status) {} +void HAL_StartSPIAutoTrigger(HAL_SPIPort port, HAL_Handle digitalSourceHandle, + HAL_AnalogTriggerType analogTriggerType, + HAL_Bool triggerRising, HAL_Bool triggerFalling, + int32_t* status) {} +void HAL_StopSPIAuto(HAL_SPIPort port, int32_t* status) {} +void HAL_SetSPIAutoTransmitData(HAL_SPIPort port, const uint8_t* dataToSend, + int32_t dataSize, int32_t zeroSize, + int32_t* status) {} +void HAL_ForceSPIAutoRead(HAL_SPIPort port, int32_t* status) {} +int32_t HAL_ReadSPIAutoReceivedData(HAL_SPIPort port, uint8_t* buffer, + int32_t numToRead, double timeout, + int32_t* status) { return 0; } -int64_t HAL_GetSPIAccumulatorValue(HAL_SPIPort port, int32_t* status) { - return SimSPIData[port].GetAccumulatorValue(); -} -int64_t HAL_GetSPIAccumulatorCount(HAL_SPIPort port, int32_t* status) { +int32_t HAL_GetSPIAutoDroppedCount(HAL_SPIPort port, int32_t* status) { return 0; } -double HAL_GetSPIAccumulatorAverage(HAL_SPIPort port, int32_t* status) { - return 0; -} -void HAL_GetSPIAccumulatorOutput(HAL_SPIPort port, int64_t* value, - int64_t* count, int32_t* status) {} diff --git a/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp b/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp index bf96388249..5bd76a68ba 100644 --- a/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp +++ b/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp @@ -14,7 +14,7 @@ using namespace frc; -static constexpr double kSamplePeriod = 0.001; +static constexpr double kSamplePeriod = 0.0005; static constexpr double kCalibrationSampleTime = 5.0; static constexpr double kDegreePerSecondPerLSB = 0.0125; diff --git a/wpilibc/src/main/native/cpp/SPI.cpp b/wpilibc/src/main/native/cpp/SPI.cpp index 09c38be01d..04f32cb7b3 100644 --- a/wpilibc/src/main/native/cpp/SPI.cpp +++ b/wpilibc/src/main/native/cpp/SPI.cpp @@ -12,11 +12,120 @@ #include #include #include +#include +#include "DigitalSource.h" +#include "Notifier.h" #include "WPIErrors.h" using namespace frc; +static constexpr int kAccumulateDepth = 2048; + +class SPI::Accumulator { + public: + Accumulator(HAL_SPIPort port, int xferSize, int validMask, int validValue, + int dataShift, int dataSize, bool isSigned, bool bigEndian) + : m_notifier([=]() { + std::lock_guard lock(m_mutex); + Update(); + }), + m_buf(new uint8_t[xferSize * kAccumulateDepth]), + m_validMask(validMask), + m_validValue(validValue), + m_dataMax(1 << dataSize), + m_dataMsbMask(1 << (dataSize - 1)), + m_dataShift(dataShift), + m_xferSize(xferSize), + m_isSigned(isSigned), + m_bigEndian(bigEndian), + m_port(port) {} + ~Accumulator() { delete[] m_buf; } + + void Update(); + + Notifier m_notifier; + uint8_t* m_buf; + wpi::mutex m_mutex; + + int64_t m_value = 0; + uint32_t m_count = 0; + int32_t m_lastValue = 0; + + int32_t m_center = 0; + int32_t m_deadband = 0; + + int32_t m_validMask; + int32_t m_validValue; + int32_t m_dataMax; // one more than max data value + int32_t m_dataMsbMask; // data field MSB mask (for signed) + uint8_t m_dataShift; // data field shift right amount, in bits + int32_t m_xferSize; // SPI transfer size, in bytes + bool m_isSigned; // is data field signed? + bool m_bigEndian; // is response big endian? + HAL_SPIPort m_port; +}; + +void SPI::Accumulator::Update() { + bool done; + do { + done = true; + int32_t status = 0; + + // get amount of data available + int32_t numToRead = + HAL_ReadSPIAutoReceivedData(m_port, m_buf, 0, 0, &status); + if (status != 0) return; // error reading + + // only get whole responses + numToRead -= numToRead % m_xferSize; + if (numToRead > m_xferSize * kAccumulateDepth) { + numToRead = m_xferSize * kAccumulateDepth; + done = false; + } + if (numToRead == 0) return; // no samples + + // read buffered data + HAL_ReadSPIAutoReceivedData(m_port, m_buf, numToRead, 0, &status); + if (status != 0) return; // error reading + + // loop over all responses + for (int32_t off = 0; off < numToRead; off += m_xferSize) { + // convert from bytes + uint32_t resp = 0; + if (m_bigEndian) { + for (int32_t i = 0; i < m_xferSize; ++i) { + resp <<= 8; + resp |= m_buf[off + i] & 0xff; + } + } else { + for (int32_t i = m_xferSize - 1; i >= 0; --i) { + resp <<= 8; + resp |= m_buf[off + i] & 0xff; + } + } + + // process response + if ((resp & m_validMask) == static_cast(m_validValue)) { + // valid sensor data; extract data field + int32_t data = static_cast(resp >> m_dataShift); + data &= m_dataMax - 1; + // 2s complement conversion if signed MSB is set + if (m_isSigned && (data & m_dataMsbMask) != 0) data -= m_dataMax; + // center offset + data -= m_center; + // only accumulate if outside deadband + if (data < -m_deadband || data > m_deadband) m_value += data; + ++m_count; + m_lastValue = data; + } else { + // no data from the sensor; just clear the last value + m_lastValue = 0; + } + } + } while (!done); +} + /** * Constructor * @@ -170,6 +279,132 @@ int SPI::Transaction(uint8_t* dataToSend, uint8_t* dataReceived, int size) { return retVal; } +/** + * Initialize automatic SPI transfer engine. + * + * Only a single engine is available, and use of it blocks use of all other + * chip select usage on the same physical SPI port while it is running. + * + * @param bufferSize buffer size in bytes + */ +void SPI::InitAuto(int bufferSize) { + int32_t status = 0; + HAL_InitSPIAuto(m_port, bufferSize, &status); + wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); +} + +/** + * Frees the automatic SPI transfer engine. + */ +void SPI::FreeAuto() { + int32_t status = 0; + HAL_FreeSPIAuto(m_port, &status); + wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); +} + +/** + * Set the data to be transmitted by the engine. + * + * Up to 16 bytes are configurable, and may be followed by up to 127 zero + * bytes. + * + * @param dataToSend data to send (maximum 16 bytes) + * @param zeroSize number of zeros to send after the data + */ +void SPI::SetAutoTransmitData(llvm::ArrayRef dataToSend, + int zeroSize) { + int32_t status = 0; + HAL_SetSPIAutoTransmitData(m_port, dataToSend.data(), dataToSend.size(), + zeroSize, &status); + wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); +} + +/** + * Start running the automatic SPI transfer engine at a periodic rate. + * + * InitAuto() and SetAutoTransmitData() must be called before calling this + * function. + * + * @param period period between transfers, in seconds (us resolution) + */ +void SPI::StartAutoRate(double period) { + int32_t status = 0; + HAL_StartSPIAutoRate(m_port, period, &status); + wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); +} + +/** + * Start running the automatic SPI transfer engine when a trigger occurs. + * + * InitAuto() and SetAutoTransmitData() must be called before calling this + * function. + * + * @param source digital source for the trigger (may be an analog trigger) + * @param rising trigger on the rising edge + * @param falling trigger on the falling edge + */ +void SPI::StartAutoTrigger(DigitalSource& source, bool rising, bool falling) { + int32_t status = 0; + HAL_StartSPIAutoTrigger( + m_port, source.GetPortHandleForRouting(), + (HAL_AnalogTriggerType)source.GetAnalogTriggerTypeForRouting(), rising, + falling, &status); + wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); +} + +/** + * Stop running the automatic SPI transfer engine. + */ +void SPI::StopAuto() { + int32_t status = 0; + HAL_StopSPIAuto(m_port, &status); + wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); +} + +/** + * Force the engine to make a single transfer. + */ +void SPI::ForceAutoRead() { + int32_t status = 0; + HAL_ForceSPIAutoRead(m_port, &status); + wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); +} + +/** + * Read data that has been transferred by the automatic SPI transfer engine. + * + * Transfers may be made a byte at a time, so it's necessary for the caller + * to handle cases where an entire transfer has not been completed. + * + * Blocks until numToRead bytes have been read or timeout expires. + * May be called with numToRead=0 to retrieve how many bytes are available. + * + * @param buffer buffer where read bytes are stored + * @param numToRead number of bytes to read + * @param timeout timeout in seconds (ms resolution) + * @return Number of bytes remaining to be read + */ +int SPI::ReadAutoReceivedData(uint8_t* buffer, int numToRead, double timeout) { + int32_t status = 0; + int32_t val = + HAL_ReadSPIAutoReceivedData(m_port, buffer, numToRead, timeout, &status); + wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); + return val; +} + +/** + * Get the number of bytes dropped by the automatic SPI transfer engine due + * to the receive buffer being full. + * + * @return Number of bytes dropped + */ +int SPI::GetAutoDroppedCount() { + int32_t status = 0; + int32_t val = HAL_GetSPIAutoDroppedCount(m_port, &status); + wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); + return val; +} + /** * Initialize the accumulator. * @@ -188,29 +423,47 @@ int SPI::Transaction(uint8_t* dataToSend, uint8_t* dataReceived, int size) { void SPI::InitAccumulator(double period, int cmd, int xferSize, int validMask, int validValue, int dataShift, int dataSize, bool isSigned, bool bigEndian) { - int32_t status = 0; - HAL_InitSPIAccumulator(m_port, static_cast(period * 1e6), cmd, - xferSize, validMask, validValue, dataShift, dataSize, - isSigned, bigEndian, &status); - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); + InitAuto(xferSize * kAccumulateDepth); + uint8_t cmdBytes[4] = {0, 0, 0, 0}; + if (bigEndian) { + for (int32_t i = xferSize - 1; i >= 0; --i) { + cmdBytes[i] = cmd & 0xff; + cmd >>= 8; + } + } else { + cmdBytes[0] = cmd & 0xff; + cmd >>= 8; + cmdBytes[1] = cmd & 0xff; + cmd >>= 8; + cmdBytes[2] = cmd & 0xff; + cmd >>= 8; + cmdBytes[3] = cmd & 0xff; + } + SetAutoTransmitData(cmdBytes, xferSize - 4); + StartAutoRate(period); + + m_accum.reset(new Accumulator(m_port, xferSize, validMask, validValue, + dataShift, dataSize, isSigned, bigEndian)); + m_accum->m_notifier.StartPeriodic(period * kAccumulateDepth / 2); } /** * Frees the accumulator. */ void SPI::FreeAccumulator() { - int32_t status = 0; - HAL_FreeSPIAccumulator(m_port, &status); - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); + m_accum.reset(nullptr); + FreeAuto(); } /** * Resets the accumulator to zero. */ void SPI::ResetAccumulator() { - int32_t status = 0; - HAL_ResetSPIAccumulator(m_port, &status); - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); + if (!m_accum) return; + std::lock_guard lock(m_accum->m_mutex); + m_accum->m_value = 0; + m_accum->m_count = 0; + m_accum->m_lastValue = 0; } /** @@ -222,28 +475,28 @@ void SPI::ResetAccumulator() { * account when integrating. */ void SPI::SetAccumulatorCenter(int center) { - int32_t status = 0; - HAL_SetSPIAccumulatorCenter(m_port, center, &status); - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); + if (!m_accum) return; + std::lock_guard lock(m_accum->m_mutex); + m_accum->m_center = center; } /** * Set the accumulator's deadband. */ void SPI::SetAccumulatorDeadband(int deadband) { - int32_t status = 0; - HAL_SetSPIAccumulatorDeadband(m_port, deadband, &status); - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); + if (!m_accum) return; + std::lock_guard lock(m_accum->m_mutex); + m_accum->m_deadband = deadband; } /** * Read the last value read by the accumulator engine. */ int SPI::GetAccumulatorLastValue() const { - int32_t status = 0; - int retVal = HAL_GetSPIAccumulatorLastValue(m_port, &status); - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - return retVal; + if (!m_accum) return 0; + std::lock_guard lock(m_accum->m_mutex); + m_accum->Update(); + return m_accum->m_lastValue; } /** @@ -252,10 +505,10 @@ int SPI::GetAccumulatorLastValue() const { * @return The 64-bit value accumulated since the last Reset(). */ int64_t SPI::GetAccumulatorValue() const { - int32_t status = 0; - int64_t retVal = HAL_GetSPIAccumulatorValue(m_port, &status); - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - return retVal; + if (!m_accum) return 0; + std::lock_guard lock(m_accum->m_mutex); + m_accum->Update(); + return m_accum->m_value; } /** @@ -267,10 +520,10 @@ int64_t SPI::GetAccumulatorValue() const { * @return The number of times samples from the channel were accumulated. */ int64_t SPI::GetAccumulatorCount() const { - int32_t status = 0; - int64_t retVal = HAL_GetSPIAccumulatorCount(m_port, &status); - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - return retVal; + if (!m_accum) return 0; + std::lock_guard lock(m_accum->m_mutex); + m_accum->Update(); + return m_accum->m_count; } /** @@ -279,10 +532,11 @@ int64_t SPI::GetAccumulatorCount() const { * @return The accumulated average value (value / count). */ double SPI::GetAccumulatorAverage() const { - int32_t status = 0; - double retVal = HAL_GetSPIAccumulatorAverage(m_port, &status); - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - return retVal; + if (!m_accum) return 0; + std::lock_guard lock(m_accum->m_mutex); + m_accum->Update(); + if (m_accum->m_count == 0) return 0.0; + return static_cast(m_accum->m_value) / m_accum->m_count; } /** @@ -295,7 +549,13 @@ double SPI::GetAccumulatorAverage() const { * @param count Pointer to the number of accumulation cycles. */ void SPI::GetAccumulatorOutput(int64_t& value, int64_t& count) const { - int32_t status = 0; - HAL_GetSPIAccumulatorOutput(m_port, &value, &count, &status); - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); + if (!m_accum) { + value = 0; + count = 0; + return; + } + std::lock_guard lock(m_accum->m_mutex); + m_accum->Update(); + value = m_accum->m_value; + count = m_accum->m_count; } diff --git a/wpilibc/src/main/native/include/SPI.h b/wpilibc/src/main/native/include/SPI.h index 315361addf..34127bf304 100644 --- a/wpilibc/src/main/native/include/SPI.h +++ b/wpilibc/src/main/native/include/SPI.h @@ -9,12 +9,18 @@ #include +#include + +#include + #include "ErrorBase.h" enum HAL_SPIPort : int32_t; namespace frc { +class DigitalSource; + /** * SPI bus interface class. * @@ -50,6 +56,16 @@ class SPI : public ErrorBase { virtual int Read(bool initiate, uint8_t* dataReceived, int size); virtual int Transaction(uint8_t* dataToSend, uint8_t* dataReceived, int size); + void InitAuto(int bufferSize); + void FreeAuto(); + void SetAutoTransmitData(llvm::ArrayRef dataToSend, int zeroSize); + void StartAutoRate(double period); + void StartAutoTrigger(DigitalSource& source, bool rising, bool falling); + void StopAuto(); + void ForceAutoRead(); + int ReadAutoReceivedData(uint8_t* buffer, int numToRead, double timeout); + int GetAutoDroppedCount(); + void InitAccumulator(double period, int cmd, int xferSize, int validMask, int validValue, int dataShift, int dataSize, bool isSigned, bool bigEndian); @@ -71,6 +87,9 @@ class SPI : public ErrorBase { private: void Init(); + + class Accumulator; + std::unique_ptr m_accum; }; } // namespace frc diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java index 37e451555d..ac269b015f 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java @@ -25,7 +25,7 @@ import edu.wpi.first.wpilibj.interfaces.Gyro; */ @SuppressWarnings({"TypeName", "AbbreviationAsWordInName", "PMD.UnusedPrivateField"}) public class ADXRS450_Gyro extends GyroBase implements Gyro, PIDSource, Sendable { - private static final double kSamplePeriod = 0.001; + private static final double kSamplePeriod = 0.0005; private static final double kCalibrationSampleTime = 5.0; private static final double kDegreePerSecondPerLSB = 0.0125; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SPI.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SPI.java index 58f3eb992c..910a016087 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SPI.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SPI.java @@ -54,6 +54,10 @@ public class SPI { * Free the resources used by this object. */ public void free() { + if (m_accum != null) { + m_accum.free(); + m_accum = null; + } SPIJNI.spiClose(m_port); } @@ -254,6 +258,247 @@ public class SPI { return SPIJNI.spiTransaction(m_port, dataToSend, dataReceived, (byte) size); } + /** + * Initialize automatic SPI transfer engine. + * + *

Only a single engine is available, and use of it blocks use of all other + * chip select usage on the same physical SPI port while it is running. + * + * @param bufferSize buffer size in bytes + */ + public void initAuto(int bufferSize) { + SPIJNI.spiInitAuto(m_port, bufferSize); + } + + /** + * Frees the automatic SPI transfer engine. + */ + public void freeAuto() { + SPIJNI.spiFreeAuto(m_port); + } + + /** + * Set the data to be transmitted by the engine. + * + *

Up to 16 bytes are configurable, and may be followed by up to 127 zero + * bytes. + * + * @param dataToSend data to send (maximum 16 bytes) + * @param zeroSize number of zeros to send after the data + */ + public void setAutoTransmitData(byte[] dataToSend, int zeroSize) { + SPIJNI.spiSetAutoTransmitData(m_port, dataToSend, zeroSize); + } + + /** + * Start running the automatic SPI transfer engine at a periodic rate. + * + *

{@link #initAuto(int)} and {@link #setAutoTransmitData(byte[], int)} must + * be called before calling this function. + * + * @param period period between transfers, in seconds (us resolution) + */ + public void startAutoRate(double period) { + SPIJNI.spiStartAutoRate(m_port, period); + } + + /** + * Start running the automatic SPI transfer engine when a trigger occurs. + * + *

{@link #initAuto(int)} and {@link #setAutoTransmitData(byte[], int)} must + * be called before calling this function. + * + * @param source digital source for the trigger (may be an analog trigger) + * @param rising trigger on the rising edge + * @param falling trigger on the falling edge + */ + public void startAutoTrigger(DigitalSource source, boolean rising, boolean falling) { + SPIJNI.spiStartAutoTrigger(m_port, source.getPortHandleForRouting(), + source.getAnalogTriggerTypeForRouting(), rising, falling); + } + + /** + * Stop running the automatic SPI transfer engine. + */ + public void stopAuto() { + SPIJNI.spiStopAuto(m_port); + } + + /** + * Force the engine to make a single transfer. + */ + public void forceAutoRead() { + SPIJNI.spiForceAutoRead(m_port); + } + + /** + * Read data that has been transferred by the automatic SPI transfer engine. + * + *

Transfers may be made a byte at a time, so it's necessary for the caller + * to handle cases where an entire transfer has not been completed. + * + *

Blocks until numToRead bytes have been read or timeout expires. + * May be called with numToRead=0 to retrieve how many bytes are available. + * + * @param buffer buffer where read bytes are stored + * @param numToRead number of bytes to read + * @param timeout timeout in seconds (ms resolution) + * @return Number of bytes remaining to be read + */ + public int readAutoReceivedData(ByteBuffer buffer, int numToRead, double timeout) { + if (buffer.hasArray()) { + return readAutoReceivedData(buffer.array(), numToRead, timeout); + } + if (!buffer.isDirect()) { + throw new IllegalArgumentException("must be a direct buffer"); + } + if (buffer.capacity() < numToRead) { + throw new IllegalArgumentException("buffer is too small, must be at least " + numToRead); + } + return SPIJNI.spiReadAutoReceivedData(m_port, buffer, numToRead, timeout); + } + + /** + * Read data that has been transferred by the automatic SPI transfer engine. + * + *

Transfers may be made a byte at a time, so it's necessary for the caller + * to handle cases where an entire transfer has not been completed. + * + *

Blocks until numToRead bytes have been read or timeout expires. + * May be called with numToRead=0 to retrieve how many bytes are available. + * + * @param buffer array where read bytes are stored + * @param numToRead number of bytes to read + * @param timeout timeout in seconds (ms resolution) + * @return Number of bytes remaining to be read + */ + public int readAutoReceivedData(byte[] buffer, int numToRead, double timeout) { + if (buffer.length < numToRead) { + throw new IllegalArgumentException("buffer is too small, must be at least " + numToRead); + } + return SPIJNI.spiReadAutoReceivedData(m_port, buffer, numToRead, timeout); + } + + /** + * Get the number of bytes dropped by the automatic SPI transfer engine due + * to the receive buffer being full. + * + * @return Number of bytes dropped + */ + public int getAutoDroppedCount() { + return SPIJNI.spiGetAutoDroppedCount(m_port); + } + + private static final int kAccumulateDepth = 2048; + + private static class Accumulator { + Accumulator(int port, int xferSize, int validMask, int validValue, int dataShift, + int dataSize, boolean isSigned, boolean bigEndian) { + m_notifier = new Notifier(this::update); + m_buf = ByteBuffer.allocateDirect(xferSize * kAccumulateDepth); + m_xferSize = xferSize; + m_validMask = validMask; + m_validValue = validValue; + m_dataShift = dataShift; + m_dataMax = 1 << dataSize; + m_dataMsbMask = 1 << (dataSize - 1); + m_isSigned = isSigned; + m_bigEndian = bigEndian; + m_port = port; + } + + void free() { + m_notifier.stop(); + } + + final Notifier m_notifier; + final ByteBuffer m_buf; + final Object m_mutex = new Object(); + + long m_value; + int m_count; + int m_lastValue; + + int m_center; + int m_deadband; + + final int m_validMask; + final int m_validValue; + final int m_dataMax; // one more than max data value + final int m_dataMsbMask; // data field MSB mask (for signed) + final int m_dataShift; // data field shift right amount, in bits + final int m_xferSize; // SPI transfer size, in bytes + final boolean m_isSigned; // is data field signed? + final boolean m_bigEndian; // is response big endian? + final int m_port; + + void update() { + synchronized (m_mutex) { + boolean done = false; + while (!done) { + done = true; + + // get amount of data available + int numToRead = SPIJNI.spiReadAutoReceivedData(m_port, m_buf, 0, 0); + + // only get whole responses + numToRead -= numToRead % m_xferSize; + if (numToRead > m_xferSize * kAccumulateDepth) { + numToRead = m_xferSize * kAccumulateDepth; + done = false; + } + if (numToRead == 0) { + return; // no samples + } + + // read buffered data + SPIJNI.spiReadAutoReceivedData(m_port, m_buf, numToRead, 0); + + // loop over all responses + for (int off = 0; off < numToRead; off += m_xferSize) { + // convert from bytes + int resp = 0; + if (m_bigEndian) { + for (int i = 0; i < m_xferSize; ++i) { + resp <<= 8; + resp |= ((int) m_buf.get(off + i)) & 0xff; + } + } else { + for (int i = m_xferSize - 1; i >= 0; --i) { + resp <<= 8; + resp |= ((int) m_buf.get(off + i)) & 0xff; + } + } + + // process response + if ((resp & m_validMask) == m_validValue) { + // valid sensor data; extract data field + int data = resp >> m_dataShift; + data &= m_dataMax - 1; + // 2s complement conversion if signed MSB is set + if (m_isSigned && (data & m_dataMsbMask) != 0) { + data -= m_dataMax; + } + // center offset + data -= m_center; + // only accumulate if outside deadband + if (data < -m_deadband || data > m_deadband) { + m_value += data; + } + ++m_count; + m_lastValue = data; + } else { + // no data from the sensor; just clear the last value + m_lastValue = 0; + } + } + } + } + } + } + + private Accumulator m_accum = null; + /** * Initialize the accumulator. * @@ -271,23 +516,51 @@ public class SPI { int validMask, int validValue, int dataShift, int dataSize, boolean isSigned, boolean bigEndian) { - SPIJNI.spiInitAccumulator(m_port, (int) (period * 1.0e6), cmd, - (byte) xferSize, validMask, validValue, (byte) dataShift, - (byte) dataSize, isSigned, bigEndian); + initAuto(xferSize * 2048); + byte[] cmdBytes = new byte[] {0, 0, 0, 0}; + if (bigEndian) { + for (int i = xferSize - 1; i >= 0; --i) { + cmdBytes[i] = (byte) (cmd & 0xff); + cmd >>= 8; + } + } else { + cmdBytes[0] = (byte) (cmd & 0xff); + cmd >>= 8; + cmdBytes[1] = (byte) (cmd & 0xff); + cmd >>= 8; + cmdBytes[2] = (byte) (cmd & 0xff); + cmd >>= 8; + cmdBytes[3] = (byte) (cmd & 0xff); + } + setAutoTransmitData(cmdBytes, xferSize - 4); + startAutoRate(period); + + m_accum = new Accumulator(m_port, xferSize, validMask, validValue, dataShift, dataSize, + isSigned, bigEndian); + m_accum.m_notifier.startPeriodic(period * 1024); } /** * Frees the accumulator. */ public void freeAccumulator() { - SPIJNI.spiFreeAccumulator(m_port); + m_accum.free(); + m_accum = null; + freeAuto(); } /** * Resets the accumulator to zero. */ public void resetAccumulator() { - SPIJNI.spiResetAccumulator(m_port); + if (m_accum == null) { + return; + } + synchronized (m_accum.m_mutex) { + m_accum.m_value = 0; + m_accum.m_count = 0; + m_accum.m_lastValue = 0; + } } /** @@ -298,21 +571,37 @@ public class SPI { * and to take the device offset into account when integrating. */ public void setAccumulatorCenter(int center) { - SPIJNI.spiSetAccumulatorCenter(m_port, center); + if (m_accum == null) { + return; + } + synchronized (m_accum.m_mutex) { + m_accum.m_center = center; + } } /** * Set the accumulator's deadband. */ public void setAccumulatorDeadband(int deadband) { - SPIJNI.spiSetAccumulatorDeadband(m_port, deadband); + if (m_accum == null) { + return; + } + synchronized (m_accum.m_mutex) { + m_accum.m_deadband = deadband; + } } /** * Read the last value read by the accumulator engine. */ public int getAccumulatorLastValue() { - return SPIJNI.spiGetAccumulatorLastValue(m_port); + if (m_accum == null) { + return 0; + } + synchronized (m_accum.m_mutex) { + m_accum.update(); + return m_accum.m_lastValue; + } } /** @@ -321,7 +610,13 @@ public class SPI { * @return The 64-bit value accumulated since the last Reset(). */ public long getAccumulatorValue() { - return SPIJNI.spiGetAccumulatorValue(m_port); + if (m_accum == null) { + return 0; + } + synchronized (m_accum.m_mutex) { + m_accum.update(); + return m_accum.m_value; + } } /** @@ -332,7 +627,13 @@ public class SPI { * @return The number of times samples from the channel were accumulated. */ public int getAccumulatorCount() { - return SPIJNI.spiGetAccumulatorCount(m_port); + if (m_accum == null) { + return 0; + } + synchronized (m_accum.m_mutex) { + m_accum.update(); + return m_accum.m_count; + } } /** @@ -341,7 +642,16 @@ public class SPI { * @return The accumulated average value (value / count). */ public double getAccumulatorAverage() { - return SPIJNI.spiGetAccumulatorAverage(m_port); + if (m_accum == null) { + return 0; + } + synchronized (m_accum.m_mutex) { + m_accum.update(); + if (m_accum.m_count == 0) { + return 0.0; + } + return ((double) m_accum.m_value) / m_accum.m_count; + } } /** @@ -355,6 +665,15 @@ public class SPI { if (result == null) { throw new IllegalArgumentException("Null parameter `result'"); } - SPIJNI.spiGetAccumulatorOutput(m_port, result); + if (m_accum == null) { + result.value = 0; + result.count = 0; + return; + } + synchronized (m_accum.m_mutex) { + m_accum.update(); + result.value = m_accum.m_value; + result.count = m_accum.m_count; + } } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/hal/SPIJNI.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/hal/SPIJNI.java index 2843c49551..bd5fdf857f 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/hal/SPIJNI.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/hal/SPIJNI.java @@ -7,7 +7,6 @@ package edu.wpi.first.wpilibj.hal; -import edu.wpi.first.wpilibj.AccumulatorResult; import java.nio.ByteBuffer; @SuppressWarnings("AbbreviationAsWordInName") @@ -39,25 +38,27 @@ public class SPIJNI extends JNIWrapper { public static native void spiSetChipSelectActiveLow(int port); - public static native void spiInitAccumulator(int port, int period, int cmd, byte xferSize, - int validMask, int validValue, byte dataShift, - byte dataSize, boolean isSigned, boolean bigEndian); + public static native void spiInitAuto(int port, int bufferSize); - public static native void spiFreeAccumulator(int port); + public static native void spiFreeAuto(int port); - public static native void spiResetAccumulator(int port); + public static native void spiStartAutoRate(int port, double period); - public static native void spiSetAccumulatorCenter(int port, int center); + public static native void spiStartAutoTrigger(int port, int digitalSourceHandle, + int analogTriggerType, boolean triggerRising, + boolean triggerFalling); - public static native void spiSetAccumulatorDeadband(int port, int deadband); + public static native void spiStopAuto(int port); - public static native int spiGetAccumulatorLastValue(int port); + public static native void spiSetAutoTransmitData(int port, byte[] dataToSend, int zeroSize); - public static native long spiGetAccumulatorValue(int port); + public static native void spiForceAutoRead(int port); - public static native int spiGetAccumulatorCount(int port); + public static native int spiReadAutoReceivedData(int port, ByteBuffer buffer, int numToRead, + double timeout); - public static native double spiGetAccumulatorAverage(int port); + public static native int spiReadAutoReceivedData(int port, byte[] buffer, int numToRead, + double timeout); - public static native void spiGetAccumulatorOutput(int port, AccumulatorResult result); + public static native int spiGetAutoDroppedCount(int port); } diff --git a/wpilibj/src/main/native/cpp/SPIJNI.cpp b/wpilibj/src/main/native/cpp/SPIJNI.cpp index b38c17fb1b..e9d21e4b20 100644 --- a/wpilibj/src/main/native/cpp/SPIJNI.cpp +++ b/wpilibj/src/main/native/cpp/SPIJNI.cpp @@ -258,189 +258,181 @@ Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiSetChipSelectActiveLow( /* * Class: edu_wpi_first_wpilibj_hal_SPIJNI - * Method: spiInitAccumulator - * Signature: (IIIBIIBBZZ)V + * Method: spiInitAuto + * Signature: (II)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiInitAccumulator( - JNIEnv *env, jclass, jint port, jint period, jint cmd, jbyte xferSize, - jint validMask, jint validValue, jbyte dataShift, jbyte dataSize, - jboolean isSigned, jboolean bigEndian) { - SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiInitAccumulator"; - SPIJNI_LOG(logDEBUG) << "Port = " << (jint)port; +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiInitAuto + (JNIEnv *env, jclass, jint port, jint bufferSize) { + SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiInitAuto"; + SPIJNI_LOG(logDEBUG) << "Port = " << port; + SPIJNI_LOG(logDEBUG) << "BufferSize = " << bufferSize; + int32_t status = 0; + HAL_InitSPIAuto(static_cast(port), bufferSize, &status); + SPIJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); +} + +/* + * Class: edu_wpi_first_wpilibj_hal_SPIJNI + * Method: spiFreeAuto + * Signature: (I)V + */ +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiFreeAuto + (JNIEnv *env, jclass, jint port) { + SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiFreeAuto"; + SPIJNI_LOG(logDEBUG) << "Port = " << port; + int32_t status = 0; + HAL_FreeSPIAuto(static_cast(port), &status); + SPIJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); +} + +/* + * Class: edu_wpi_first_wpilibj_hal_SPIJNI + * Method: spiStartAutoRate + * Signature: (ID)V + */ +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiStartAutoRate + (JNIEnv *env, jclass, jint port, jdouble period) { + SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiStartAutoRate"; + SPIJNI_LOG(logDEBUG) << "Port = " << port; SPIJNI_LOG(logDEBUG) << "Period = " << period; - SPIJNI_LOG(logDEBUG) << "Cmd = " << cmd; - SPIJNI_LOG(logDEBUG) << "XferSize = " << (jint)xferSize; - SPIJNI_LOG(logDEBUG) << "ValidMask = " << validMask; - SPIJNI_LOG(logDEBUG) << "ValidValue = " << validValue; - SPIJNI_LOG(logDEBUG) << "DataShift = " << (jint)dataShift; - SPIJNI_LOG(logDEBUG) << "DataSize = " << (jint)dataSize; - SPIJNI_LOG(logDEBUG) << "IsSigned = " << (jint)isSigned; - SPIJNI_LOG(logDEBUG) << "BigEndian = " << (jint)bigEndian; int32_t status = 0; - HAL_InitSPIAccumulator(static_cast(port), period, cmd, xferSize, validMask, validValue, - dataShift, dataSize, isSigned, bigEndian, &status); + HAL_StartSPIAutoRate(static_cast(port), period, &status); SPIJNI_LOG(logDEBUG) << "Status = " << status; CheckStatus(env, status); } /* * Class: edu_wpi_first_wpilibj_hal_SPIJNI - * Method: spiFreeAccumulator + * Method: spiStartAutoTrigger + * Signature: (IIIZZ)V + */ +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiStartAutoTrigger + (JNIEnv *env, jclass, jint port, jint digitalSourceHandle, jint analogTriggerType, jboolean triggerRising, jboolean triggerFalling) { + SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiStartAutoTrigger"; + SPIJNI_LOG(logDEBUG) << "Port = " << port; + SPIJNI_LOG(logDEBUG) << "DigitalSourceHandle = " << digitalSourceHandle; + SPIJNI_LOG(logDEBUG) << "AnalogTriggerType = " << analogTriggerType; + SPIJNI_LOG(logDEBUG) << "TriggerRising = " << (jint)triggerRising; + SPIJNI_LOG(logDEBUG) << "TriggerFalling = " << (jint)triggerFalling; + int32_t status = 0; + HAL_StartSPIAutoTrigger(static_cast(port), digitalSourceHandle, + static_cast(analogTriggerType), triggerRising, + triggerFalling, &status); + SPIJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); +} + +/* + * Class: edu_wpi_first_wpilibj_hal_SPIJNI + * Method: spiStopAuto * Signature: (I)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiFreeAccumulator( - JNIEnv *env, jclass, jint port) { - SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiFreeAccumulator"; - SPIJNI_LOG(logDEBUG) << "Port = " << (jint)port; +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiStopAuto + (JNIEnv *env, jclass, jint port) { + SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiStopAuto"; + SPIJNI_LOG(logDEBUG) << "Port = " << port; int32_t status = 0; - HAL_FreeSPIAccumulator(static_cast(port), &status); + HAL_StopSPIAuto(static_cast(port), &status); SPIJNI_LOG(logDEBUG) << "Status = " << status; CheckStatus(env, status); } /* * Class: edu_wpi_first_wpilibj_hal_SPIJNI - * Method: spiResetAccumulator + * Method: spiSetAutoTransmitData + * Signature: (I[BI)V + */ +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiSetAutoTransmitData + (JNIEnv *env, jclass, jint port, jbyteArray dataToSend, jint zeroSize) { + SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiSetAutoTransmitData"; + SPIJNI_LOG(logDEBUG) << "Port = " << port; + SPIJNI_LOG(logDEBUG) << "ZeroSize = " << zeroSize; + JByteArrayRef jarr(env, dataToSend); + int32_t status = 0; + HAL_SetSPIAutoTransmitData(static_cast(port), + reinterpret_cast(jarr.array().data()), + jarr.array().size(), zeroSize, &status); + SPIJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); +} + +/* + * Class: edu_wpi_first_wpilibj_hal_SPIJNI + * Method: spiForceAutoRead * Signature: (I)V */ -JNIEXPORT void JNICALL -Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiResetAccumulator( - JNIEnv *env, jclass, jint port) { - SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiResetAccumulator"; - SPIJNI_LOG(logDEBUG) << "Port = " << (jint)port; +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiForceAutoRead + (JNIEnv *env, jclass, jint port) { + SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiForceAutoRead"; + SPIJNI_LOG(logDEBUG) << "Port = " << port; int32_t status = 0; - HAL_ResetSPIAccumulator(static_cast(port), &status); + HAL_ForceSPIAutoRead(static_cast(port), &status); SPIJNI_LOG(logDEBUG) << "Status = " << status; CheckStatus(env, status); } /* * Class: edu_wpi_first_wpilibj_hal_SPIJNI - * Method: spiSetAccumulatorCenter - * Signature: (II)V + * Method: spiReadAutoReceivedData + * Signature: (ILjava/nio/ByteBuffer;ID)I */ -JNIEXPORT void JNICALL -Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiSetAccumulatorCenter( - JNIEnv *env, jclass, jint port, jint center) { - SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiSetAccumulatorCenter"; - SPIJNI_LOG(logDEBUG) << "Port = " << (jint)port; - SPIJNI_LOG(logDEBUG) << "Center = " << center; +JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiReadAutoReceivedData__ILjava_nio_ByteBuffer_2ID + (JNIEnv *env, jclass, jint port, jobject buffer, jint numToRead, jdouble timeout) { + SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiReadAutoReceivedData"; + SPIJNI_LOG(logDEBUG) << "Port = " << port; + SPIJNI_LOG(logDEBUG) << "NumToRead = " << numToRead; + SPIJNI_LOG(logDEBUG) << "Timeout = " << timeout; + uint8_t *recvBuf = (uint8_t *)env->GetDirectBufferAddress(buffer); int32_t status = 0; - HAL_SetSPIAccumulatorCenter(static_cast(port), center, &status); + jint retval = HAL_ReadSPIAutoReceivedData(static_cast(port), recvBuf, numToRead, timeout, &status); SPIJNI_LOG(logDEBUG) << "Status = " << status; + SPIJNI_LOG(logDEBUG) << "Return = " << retval; CheckStatus(env, status); + return retval; } /* * Class: edu_wpi_first_wpilibj_hal_SPIJNI - * Method: spiSetAccumulatorDeadband - * Signature: (II)V + * Method: spiReadAutoReceivedData + * Signature: (I[BID)I */ -JNIEXPORT void JNICALL -Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiSetAccumulatorDeadband( - JNIEnv *env, jclass, jint port, jint deadband) { - SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiSetAccumulatorDeadband"; - SPIJNI_LOG(logDEBUG) << "Port = " << (jint)port; - SPIJNI_LOG(logDEBUG) << "Deadband = " << deadband; +JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiReadAutoReceivedData__I_3BID + (JNIEnv *env, jclass, jint port, jbyteArray buffer, jint numToRead, jdouble timeout) { + SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiReadAutoReceivedData"; + SPIJNI_LOG(logDEBUG) << "Port = " << port; + SPIJNI_LOG(logDEBUG) << "NumToRead = " << numToRead; + SPIJNI_LOG(logDEBUG) << "Timeout = " << timeout; + llvm::SmallVector recvBuf; + recvBuf.resize(numToRead); int32_t status = 0; - HAL_SetSPIAccumulatorDeadband(static_cast(port), deadband, &status); + jint retval = HAL_ReadSPIAutoReceivedData(static_cast(port), recvBuf.data(), numToRead, timeout, &status); SPIJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); + SPIJNI_LOG(logDEBUG) << "Return = " << retval; + if (!CheckStatus(env, status)) return retval; + if (numToRead > 0) { + env->SetByteArrayRegion(buffer, 0, numToRead, + reinterpret_cast(recvBuf.data())); + } + return retval; } /* * Class: edu_wpi_first_wpilibj_hal_SPIJNI - * Method: spiGetAccumulatorLastValue + * Method: spiGetAutoDroppedCount * Signature: (I)I */ -JNIEXPORT jint JNICALL -Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiGetAccumulatorLastValue( - JNIEnv *env, jclass, jint port) { - SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiGetAccumulatorLastValue"; - SPIJNI_LOG(logDEBUG) << "Port = " << (jint)port; +JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiGetAutoDroppedCount + (JNIEnv *env, jclass, jint port) { + SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiGetAutoDroppedCount"; + SPIJNI_LOG(logDEBUG) << "Port = " << port; int32_t status = 0; - jint retVal = HAL_GetSPIAccumulatorLastValue(static_cast(port), &status); + auto retval = HAL_GetSPIAutoDroppedCount(static_cast(port), &status); SPIJNI_LOG(logDEBUG) << "Status = " << status; - SPIJNI_LOG(logDEBUG) << "ReturnValue = " << retVal; - CheckStatus(env, status); - return retVal; -} - -/* - * Class: edu_wpi_first_wpilibj_hal_SPIJNI - * Method: spiGetAccumulatorValue - * Signature: (I)J - */ -JNIEXPORT jlong JNICALL -Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiGetAccumulatorValue( - JNIEnv *env, jclass, jint port) { - SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiGetAccumulatorValue"; - SPIJNI_LOG(logDEBUG) << "Port = " << (jint)port; - int32_t status = 0; - jlong retVal = HAL_GetSPIAccumulatorValue(static_cast(port), &status); - SPIJNI_LOG(logDEBUG) << "Status = " << status; - SPIJNI_LOG(logDEBUG) << "ReturnValue = " << retVal; - CheckStatus(env, status); - return retVal; -} - -/* - * Class: edu_wpi_first_wpilibj_hal_SPIJNI - * Method: spiGetAccumulatorCount - * Signature: (I)I - */ -JNIEXPORT jint JNICALL -Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiGetAccumulatorCount( - JNIEnv *env, jclass, jint port) { - SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiGetAccumulatorCount"; - SPIJNI_LOG(logDEBUG) << "Port = " << (jint)port; - int32_t status = 0; - jint retVal = HAL_GetSPIAccumulatorCount(static_cast(port), &status); - SPIJNI_LOG(logDEBUG) << "Status = " << status; - SPIJNI_LOG(logDEBUG) << "ReturnValue = " << retVal; - CheckStatus(env, status); - return retVal; -} - -/* - * Class: edu_wpi_first_wpilibj_hal_SPIJNI - * Method: spiGetAccumulatorAverage - * Signature: (I)D - */ -JNIEXPORT jdouble JNICALL -Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiGetAccumulatorAverage( - JNIEnv *env, jclass, jint port) { - SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiGetAccumulatorAverage"; - SPIJNI_LOG(logDEBUG) << "Port = " << (jint)port; - int32_t status = 0; - jdouble retVal = HAL_GetSPIAccumulatorAverage(static_cast(port), &status); - SPIJNI_LOG(logDEBUG) << "Status = " << status; - SPIJNI_LOG(logDEBUG) << "ReturnValue = " << retVal; - CheckStatus(env, status); - return retVal; -} - -/* - * Class: edu_wpi_first_wpilibj_hal_SPIJNI - * Method: spiGetAccumulatorOutput - * Signature: (ILedu/wpi/first/wpilibj/AccumulatorResult;)V - */ -JNIEXPORT void JNICALL -Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiGetAccumulatorOutput( - JNIEnv *env, jclass, jint port, jobject accumulatorResult) { - SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiGetAccumulatorOutput"; - SPIJNI_LOG(logDEBUG) << "Port = " << (jint)port; - int32_t status = 0; - int64_t value = 0; - int64_t count = 0; - HAL_GetSPIAccumulatorOutput(static_cast(port), &value, &count, &status); - - SetAccumulatorResultObject(env, accumulatorResult, value, count); - - SPIJNI_LOG(logDEBUG) << "Status = " << status; - SPIJNI_LOG(logDEBUG) << "Value = " << value; - SPIJNI_LOG(logDEBUG) << "Count = " << count; + SPIJNI_LOG(logDEBUG) << "Return = " << retval; CheckStatus(env, status); + return retval; } } // extern "C"