Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-28 02:11:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

Add support for automatic SPI transfer engine. (#836)

Browse Source 
The SPI Accumulator functions have been moved from HAL to wpilib and rewritten
to use the automatic transfer engine.
This commit is contained in:
Peter Johnson
2017-12-13 23:41:37 -08:00
committed by GitHub
parent d3dd586362
commit 7f074563d0
10 changed files with 1003 additions and 535 deletions
Download Patch File Download Diff File Expand all files Collapse all files

475
hal/src/main/native/athena/SPI.cpp
View File

@@ -15,7 +15,6 @@
#include <array>
#include <atomic>
#include <cstring>
#include <thread>
#include <llvm/raw_ostream.h>
#include <support/mutex.h>
@@ -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<wpi::mutex, kSpiMaxHandles> spiApiMutexes;
static std::array<wpi::mutex, kSpiMaxHandles> spiAccumulatorMutexes;
// MXP SPI does not count towards this
std::atomic<int32_t> spiPortCount{0};
static std::atomic<int32_t> spiPortCount{0};
static HAL_DigitalHandle digitalHandles[9]{HAL_kInvalidHandle};
struct SPIAccumulator {
std::atomic<HAL_NotifierHandle> 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<tDMAManager> 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<SPIAccumulator> 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<wpi::mutex> 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<wpi::mutex> 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<wpi::mutex> 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<uint32_t>(accum->validValue)) {
// valid sensor data; extract data field
int32_t data = static_cast<int32_t>(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<tDMAManager>(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<wpi::mutex> lock(spiAccumulatorMutexes[port]);
if (!spiAccumulators[port])
spiAccumulators[port] = std::make_unique<SPIAccumulator>();
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<wpi::mutex> 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<wpi::mutex> 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<wpi::mutex> 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<wpi::mutex> 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<wpi::mutex> 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<wpi::mutex> lock(spiApiMutexes[port]);
SPIAccumulator* accum = spiAccumulators[port].get();
if (!accum) {
*status = NULL_PARAMETER;
std::lock_guard<wpi::mutex> 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<unsigned>(zeroSize) & 0x7f;
config.TxByteCount = static_cast<unsigned>(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<wpi::mutex> lock(spiAutoMutex);
// FPGA only has one auto SPI engine
if (port != spiAutoPort) {
*status = INCOMPATIBLE_STATE;
return;
}
std::lock_guard<wpi::mutex> 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<wpi::mutex> 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<wpi::mutex> lock(spiAutoMutex);
// FPGA only has one auto SPI engine
if (port != spiAutoPort) {
*status = INCOMPATIBLE_STATE;
return 0;
}
std::lock_guard<wpi::mutex> 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<wpi::mutex> lock(spiAutoMutex);
// FPGA only has one auto SPI engine
if (port != spiAutoPort) {
*status = INCOMPATIBLE_STATE;
return 0;
}
std::lock_guard<wpi::mutex> 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<wpi::mutex> 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<double>(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<wpi::mutex> 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"

35
hal/src/main/native/include/HAL/SPI.h
View File

@@ -9,6 +9,7 @@
#include <stdint.h>
#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

39
hal/src/main/native/sim/SPI.cpp
View File

@@ -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) {}

2
wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp
View File

@@ -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;

332
wpilibc/src/main/native/cpp/SPI.cpp
View File

@@ -12,11 +12,120 @@
#include <HAL/HAL.h>
#include <HAL/SPI.h>
#include <llvm/SmallVector.h>
#include <support/mutex.h>
#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<wpi::mutex> 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<uint32_t>(m_validValue)) {
// valid sensor data; extract data field
int32_t data = static_cast<int32_t>(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<uint8_t> 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<int32_t>(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<wpi::mutex> 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<wpi::mutex> 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<wpi::mutex> 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<wpi::mutex> 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<wpi::mutex> 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<wpi::mutex> 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<wpi::mutex> lock(m_accum->m_mutex);
m_accum->Update();
if (m_accum->m_count == 0) return 0.0;
return static_cast<double>(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<wpi::mutex> lock(m_accum->m_mutex);
m_accum->Update();
value = m_accum->m_value;
count = m_accum->m_count;
}

19
wpilibc/src/main/native/include/SPI.h
View File

@@ -9,12 +9,18 @@
#include <stdint.h>
#include <memory>
#include <llvm/ArrayRef.h>
#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<uint8_t> 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<Accumulator> m_accum;
};
} // namespace frc

2
wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java
View File

@@ -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;

343
wpilibj/src/main/java/edu/wpi/first/wpilibj/SPI.java
View File

@@ -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.
*
* <p>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.
*
* <p>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.
*
* <p>{@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.
*
* <p>{@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.
*
* <p>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.
*
* <p>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.
*
* <p>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.
*
* <p>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;
}
}
}

27
wpilibj/src/main/java/edu/wpi/first/wpilibj/hal/SPIJNI.java
View File

@@ -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);
}

264
wpilibj/src/main/native/cpp/SPIJNI.cpp
View File

@@ -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<HAL_SPIPort>(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<HAL_SPIPort>(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<HAL_SPIPort>(port), period, cmd, xferSize, validMask, validValue,
dataShift, dataSize, isSigned, bigEndian, &status);
HAL_StartSPIAutoRate(static_cast<HAL_SPIPort>(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<HAL_SPIPort>(port), digitalSourceHandle,
static_cast<HAL_AnalogTriggerType>(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<HAL_SPIPort>(port), &status);
HAL_StopSPIAuto(static_cast<HAL_SPIPort>(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<HAL_SPIPort>(port),
reinterpret_cast<const uint8_t*>(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<HAL_SPIPort>(port), &status);
HAL_ForceSPIAutoRead(static_cast<HAL_SPIPort>(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<HAL_SPIPort>(port), center, &status);
jint retval = HAL_ReadSPIAutoReceivedData(static_cast<HAL_SPIPort>(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<uint8_t, 128> recvBuf;
recvBuf.resize(numToRead);
int32_t status = 0;
HAL_SetSPIAccumulatorDeadband(static_cast<HAL_SPIPort>(port), deadband, &status);
jint retval = HAL_ReadSPIAutoReceivedData(static_cast<HAL_SPIPort>(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<const jbyte *>(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<HAL_SPIPort>(port), &status);
auto retval = HAL_GetSPIAutoDroppedCount(static_cast<HAL_SPIPort>(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<HAL_SPIPort>(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<HAL_SPIPort>(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<HAL_SPIPort>(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<HAL_SPIPort>(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"