From de219055f04567fd63cb6ca72202c8817a3c3430 Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Sun, 22 Nov 2015 11:50:49 -0800 Subject: [PATCH] HAL: Add software-based accumulator for SPI devices. Change-Id: I154c4c8f438163edf3ebc2c38f67a976d8cfbfd7 --- hal/include/HAL/Digital.hpp | 16 + hal/lib/Athena/Digital.cpp | 274 ++++++++++++++++++ wpilibc/Athena/include/SPI.h | 14 + wpilibc/Athena/src/SPI.cpp | 129 +++++++++ wpilibj/src/athena/cpp/lib/SPIJNI.cpp | 187 ++++++++++++ .../java/edu/wpi/first/wpilibj/SPI.java | 118 ++++++++ .../edu/wpi/first/wpilibj/hal/SPIJNI.java | 25 ++ 7 files changed, 763 insertions(+) diff --git a/hal/include/HAL/Digital.hpp b/hal/include/HAL/Digital.hpp index fe7ee953fc..64851f18f8 100644 --- a/hal/include/HAL/Digital.hpp +++ b/hal/include/HAL/Digital.hpp @@ -110,6 +110,22 @@ extern "C" int32_t spiGetHandle(uint8_t port); void spiSetHandle(uint8_t port, int32_t handle); + void spiInitAccumulator(uint8_t port, uint32_t period, uint32_t cmd, + uint8_t xfer_size, uint32_t valid_mask, + uint32_t valid_value, uint8_t data_shift, + uint8_t data_size, bool is_signed, bool big_endian, + int32_t *status); + void spiFreeAccumulator(uint8_t port, int32_t *status); + void spiResetAccumulator(uint8_t port, int32_t *status); + void spiSetAccumulatorCenter(uint8_t port, int32_t center, int32_t *status); + void spiSetAccumulatorDeadband(uint8_t port, int32_t deadband, int32_t *status); + int32_t spiGetAccumulatorLastValue(uint8_t port, int32_t *status); + int64_t spiGetAccumulatorValue(uint8_t port, int32_t *status); + uint32_t spiGetAccumulatorCount(uint8_t port, int32_t *status); + double spiGetAccumulatorAverage(uint8_t port, int32_t *status); + void spiGetAccumulatorOutput(uint8_t port, int64_t *value, uint32_t *count, + int32_t *status); + void i2CInitialize(uint8_t port, int32_t *status); int32_t i2CTransaction(uint8_t port, uint8_t deviceAddress, uint8_t *dataToSend, uint8_t sendSize, uint8_t *dataReceived, uint8_t receiveSize); int32_t i2CWrite(uint8_t port, uint8_t deviceAddress, uint8_t *dataToSend, uint8_t sendSize); diff --git a/hal/lib/Athena/Digital.cpp b/hal/lib/Athena/Digital.cpp index 0a088798fb..0d4ff5f175 100644 --- a/hal/lib/Athena/Digital.cpp +++ b/hal/lib/Athena/Digital.cpp @@ -86,6 +86,31 @@ priority_recursive_mutex spiOnboardSemaphore; priority_recursive_mutex spiMXPSemaphore; tSPI *spiSystem; +struct SPIAccumulator { + void* notifier = nullptr; + uint32_t triggerTime; + uint32_t period; + + int64_t value = 0; + uint32_t count = 0; + int32_t last_value = 0; + + int32_t center = 0; + int32_t deadband = 0; + + uint8_t cmd[4]; // command to send (up to 4 bytes) + uint32_t valid_mask; + uint32_t valid_value; + int32_t data_max; // one more than max data value + int32_t data_msb_mask; // data field MSB mask (for signed) + uint8_t data_shift; // data field shift right amount, in bits + uint8_t xfer_size; // SPI transfer size, in bytes (up to 4) + uint8_t port; + bool is_signed; // is data field signed? + bool big_endian; // is response big endian? +}; +SPIAccumulator* spiAccumulators[5] = {nullptr, nullptr, nullptr, nullptr, nullptr}; + /** * Initialize the digital system. */ @@ -1341,6 +1366,10 @@ int32_t spiRead(uint8_t port, uint8_t *buffer, uint8_t count) */ void spiClose(uint8_t port) { std::lock_guard sync(spiGetSemaphore(port)); + if (spiAccumulators[port]) { + int32_t status = 0; + spiFreeAccumulator(port, &status); + } spilib_close(spiGetHandle(port)); spiSetHandle(port, 0); return; @@ -1470,6 +1499,251 @@ priority_recursive_mutex& spiGetSemaphore(uint8_t port) { return spiMXPSemaphore; } +static void spiAccumulatorProcess(uint32_t currentTime, void *param) { + SPIAccumulator* accum = (SPIAccumulator*)param; + + // perform SPI transaction + uint8_t resp_b[4]; + std::lock_guard sync(spiGetSemaphore(accum->port)); + spilib_writeread(spiGetHandle(accum->port), (const char*) accum->cmd, (char*) resp_b, (int32_t) accum->xfer_size); + + // convert from bytes + uint32_t resp = 0; + if (accum->big_endian) { + for (int i=0; i < accum->xfer_size; ++i) { + resp <<= 8; + resp |= resp_b[i] & 0xff; + } + } else { + for (int i = accum->xfer_size - 1; i >= 0; --i) { + resp <<= 8; + resp |= resp_b[i] & 0xff; + } + } + + // process response + if ((resp & accum->valid_mask) == accum->valid_value) { + // valid sensor data; extract data field + int32_t data = (int32_t)(resp >> accum->data_shift); + data &= accum->data_max - 1; + // 2s complement conversion if signed MSB is set + if (accum->is_signed && (data & accum->data_msb_mask) != 0) + data -= accum->data_max; + // center offset + data -= accum->center; + // only accumulate if outside deadband + if (data < -accum->deadband || data > accum->deadband) + accum->value += data; + ++accum->count; + accum->last_value = data; + } else { + // no data from the sensor; just clear the last value + accum->last_value = 0; + } + + // reschedule timer + accum->triggerTime += accum->period; + // handle timer slip + if (accum->triggerTime < currentTime) + accum->triggerTime = currentTime + accum->period; + int32_t status = 0; + updateNotifierAlarm(accum->notifier, accum->triggerTime, &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 xfer_size SPI transfer size, in bytes + * @param valid_mask Mask to apply to received data for validity checking + * @param valid_data After valid_mask is applied, required matching value for + * validity checking + * @param data_shift Bit shift to apply to received data to get actual data + * value + * @param data_size Size (in bits) of data field + * @param is_signed Is data field signed? + * @param big_endian Is device big endian? + */ +void spiInitAccumulator(uint8_t port, uint32_t period, uint32_t cmd, + uint8_t xfer_size, uint32_t valid_mask, uint32_t valid_value, + uint8_t data_shift, uint8_t data_size, bool is_signed, + bool big_endian, int32_t *status) { + std::lock_guard sync(spiGetSemaphore(port)); + if (port > 4) return; + if (!spiAccumulators[port]) + spiAccumulators[port] = new SPIAccumulator(); + SPIAccumulator* accum = spiAccumulators[port]; + if (big_endian) { + for (int i = xfer_size - 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->xfer_size = xfer_size; + accum->valid_mask = valid_mask; + accum->valid_value = valid_value; + accum->data_shift = data_shift; + accum->data_max = (1 << data_size); + accum->data_msb_mask = (1 << (data_size - 1)); + accum->is_signed = is_signed; + accum->big_endian = big_endian; + if (!accum->notifier) { + accum->notifier = initializeNotifier(spiAccumulatorProcess, accum, status); + accum->triggerTime = getFPGATime(status) + period; + if (*status != 0) return; + updateNotifierAlarm(accum->notifier, accum->triggerTime, status); + } +} + +/** + * Frees a SPI accumulator. + */ +void spiFreeAccumulator(uint8_t port, int32_t *status) { + std::lock_guard sync(spiGetSemaphore(port)); + SPIAccumulator* accum = spiAccumulators[port]; + if (!accum) { + *status = NULL_PARAMETER; + return; + } + cleanNotifier(accum->notifier, status); + delete accum; + spiAccumulators[port] = nullptr; +} + +/** + * Resets the accumulator to zero. + */ +void spiResetAccumulator(uint8_t port, int32_t *status) { + std::lock_guard sync(spiGetSemaphore(port)); + SPIAccumulator* accum = spiAccumulators[port]; + if (!accum) { + *status = NULL_PARAMETER; + return; + } + accum->value = 0; + accum->count = 0; + accum->last_value = 0; +} + +/** + * 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 spiSetAccumulatorCenter(uint8_t port, int32_t center, int32_t *status) { + std::lock_guard sync(spiGetSemaphore(port)); + SPIAccumulator* accum = spiAccumulators[port]; + if (!accum) { + *status = NULL_PARAMETER; + return; + } + accum->center = center; +} + +/** + * Set the accumulator's deadband. + */ +void spiSetAccumulatorDeadband(uint8_t port, int32_t deadband, int32_t *status) { + std::lock_guard sync(spiGetSemaphore(port)); + SPIAccumulator* accum = spiAccumulators[port]; + if (!accum) { + *status = NULL_PARAMETER; + return; + } + accum->deadband = deadband; +} + +/** + * Read the last value read by the accumulator engine. + */ +int32_t spiGetAccumulatorLastValue(uint8_t port, int32_t *status) { + std::lock_guard sync(spiGetSemaphore(port)); + SPIAccumulator* accum = spiAccumulators[port]; + if (!accum) { + *status = NULL_PARAMETER; + return 0; + } + return accum->last_value; +} + +/** + * Read the accumulated value. + * + * @return The 64-bit value accumulated since the last Reset(). + */ +int64_t spiGetAccumulatorValue(uint8_t port, int32_t *status) { + std::lock_guard sync(spiGetSemaphore(port)); + SPIAccumulator* accum = spiAccumulators[port]; + 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. + */ +uint32_t spiGetAccumulatorCount(uint8_t port, int32_t *status) { + std::lock_guard sync(spiGetSemaphore(port)); + SPIAccumulator* accum = spiAccumulators[port]; + 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 spiGetAccumulatorAverage(uint8_t port, int32_t *status) { + int64_t value; + uint32_t count; + spiGetAccumulatorOutput(port, &value, &count, status); + if (count == 0) return 0.0; + return ((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 spiGetAccumulatorOutput(uint8_t port, int64_t *value, uint32_t *count, + int32_t *status) { + std::lock_guard sync(spiGetSemaphore(port)); + SPIAccumulator* accum = spiAccumulators[port]; + if (!accum) { + *status = NULL_PARAMETER; + *value = 0; + *count = 0; + return; + } + *value = accum->value; + *count = accum->count; +} + /* * Initialize the I2C port. Opens the port if necessary and saves the handle. * If opening the MXP port, also sets up the pin functions appropriately diff --git a/wpilibc/Athena/include/SPI.h b/wpilibc/Athena/include/SPI.h index 1e7d53d0b5..7b33dd7e6b 100644 --- a/wpilibc/Athena/include/SPI.h +++ b/wpilibc/Athena/include/SPI.h @@ -47,6 +47,20 @@ class SPI : public SensorBase { virtual int32_t Transaction(uint8_t* dataToSend, uint8_t* dataReceived, uint8_t size); + void InitAccumulator(double period, uint32_t cmd, uint8_t xfer_size, + uint32_t valid_mask, uint32_t valid_value, + uint8_t data_shift, uint8_t data_size, bool is_signed, + bool big_endian); + void FreeAccumulator(); + void ResetAccumulator(); + void SetAccumulatorCenter(int32_t center); + void SetAccumulatorDeadband(int32_t deadband); + int32_t GetAccumulatorLastValue() const; + int64_t GetAccumulatorValue() const; + uint32_t GetAccumulatorCount() const; + double GetAccumulatorAverage() const; + void GetAccumulatorOutput(int64_t &value, uint32_t &count) const; + protected: uint8_t m_port; bool m_msbFirst = false; // default little-endian diff --git a/wpilibc/Athena/src/SPI.cpp b/wpilibc/Athena/src/SPI.cpp index c0ac0975d1..a470597539 100644 --- a/wpilibc/Athena/src/SPI.cpp +++ b/wpilibc/Athena/src/SPI.cpp @@ -172,3 +172,132 @@ int32_t SPI::Transaction(uint8_t* dataToSend, uint8_t* dataReceived, retVal = spiTransaction(m_port, dataToSend, dataReceived, size); return retVal; } + +/** + * Initialize the accumulator. + * + * @param period Time between reads + * @param cmd SPI command to send to request data + * @param xfer_size SPI transfer size, in bytes + * @param valid_mask Mask to apply to received data for validity checking + * @param valid_data After valid_mask is applied, required matching value for + * validity checking + * @param data_shift Bit shift to apply to received data to get actual data + * value + * @param data_size Size (in bits) of data field + * @param is_signed Is data field signed? + * @param big_endian Is device big endian? + */ +void SPI::InitAccumulator(double period, uint32_t cmd, uint8_t xfer_size, + uint32_t valid_mask, uint32_t valid_value, + uint8_t data_shift, uint8_t data_size, bool is_signed, + bool big_endian) { + int32_t status = 0; + spiInitAccumulator(m_port, (uint32_t)(period * 1e6), cmd, xfer_size, + valid_mask, valid_value, data_shift, data_size, is_signed, + big_endian, &status); + wpi_setErrorWithContext(status, getHALErrorMessage(status)); +} + +/** + * Frees the accumulator. + */ +void SPI::FreeAccumulator() { + int32_t status = 0; + spiFreeAccumulator(m_port, &status); + wpi_setErrorWithContext(status, getHALErrorMessage(status)); +} + +/** + * Resets the accumulator to zero. + */ +void SPI::ResetAccumulator() { + int32_t status = 0; + spiResetAccumulator(m_port, &status); + wpi_setErrorWithContext(status, getHALErrorMessage(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 SPI::SetAccumulatorCenter(int32_t center) { + int32_t status = 0; + spiSetAccumulatorCenter(m_port, center, &status); + wpi_setErrorWithContext(status, getHALErrorMessage(status)); +} + +/** + * Set the accumulator's deadband. + */ +void SPI::SetAccumulatorDeadband(int32_t deadband) { + int32_t status = 0; + spiSetAccumulatorDeadband(m_port, deadband, &status); + wpi_setErrorWithContext(status, getHALErrorMessage(status)); +} + +/** + * Read the last value read by the accumulator engine. + */ +int32_t SPI::GetAccumulatorLastValue() const { + int32_t status = 0; + int32_t retVal = spiGetAccumulatorLastValue(m_port, &status); + wpi_setErrorWithContext(status, getHALErrorMessage(status)); + return retVal; +} + +/** + * Read the accumulated value. + * + * @return The 64-bit value accumulated since the last Reset(). + */ +int64_t SPI::GetAccumulatorValue() const { + int32_t status = 0; + int64_t retVal = spiGetAccumulatorValue(m_port, &status); + wpi_setErrorWithContext(status, getHALErrorMessage(status)); + return retVal; +} + +/** + * 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. + */ +uint32_t SPI::GetAccumulatorCount() const { + int32_t status = 0; + uint32_t retVal = spiGetAccumulatorCount(m_port, &status); + wpi_setErrorWithContext(status, getHALErrorMessage(status)); + return retVal; +} + +/** + * Read the average of the accumulated value. + * + * @return The accumulated average value (value / count). + */ +double SPI::GetAccumulatorAverage() const { + int32_t status = 0; + double retVal = spiGetAccumulatorAverage(m_port, &status); + wpi_setErrorWithContext(status, getHALErrorMessage(status)); + return retVal; +} + +/** + * 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 SPI::GetAccumulatorOutput(int64_t &value, uint32_t &count) const { + int32_t status = 0; + spiGetAccumulatorOutput(m_port, &value, &count, &status); + wpi_setErrorWithContext(status, getHALErrorMessage(status)); +} diff --git a/wpilibj/src/athena/cpp/lib/SPIJNI.cpp b/wpilibj/src/athena/cpp/lib/SPIJNI.cpp index e35d93f90a..3dbb8f69d2 100644 --- a/wpilibj/src/athena/cpp/lib/SPIJNI.cpp +++ b/wpilibj/src/athena/cpp/lib/SPIJNI.cpp @@ -171,4 +171,191 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiSetChipSelectAct CheckStatus(env, status); } +/* + * Class: edu_wpi_first_wpilibj_hal_SPIJNI + * Method: spiInitAccumulator + * Signature: (BIIBIIBBZZ)V + */ +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiInitAccumulator + (JNIEnv *env, jclass, jbyte 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; + 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; + spiInitAccumulator(port, period, cmd, xferSize, validMask, validValue, + dataShift, dataSize, isSigned, bigEndian, &status); + SPIJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); +} + +/* + * Class: edu_wpi_first_wpilibj_hal_SPIJNI + * Method: spiFreeAccumulator + * Signature: (B)V + */ +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiFreeAccumulator + (JNIEnv *env, jclass, jbyte port) +{ + SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiFreeAccumulator"; + SPIJNI_LOG(logDEBUG) << "Port = " << (jint) port; + int32_t status = 0; + spiFreeAccumulator(port, &status); + SPIJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); +} + +/* + * Class: edu_wpi_first_wpilibj_hal_SPIJNI + * Method: spiResetAccumulator + * Signature: (B)V + */ +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiResetAccumulator + (JNIEnv *env, jclass, jbyte port) +{ + SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiResetAccumulator"; + SPIJNI_LOG(logDEBUG) << "Port = " << (jint) port; + int32_t status = 0; + spiResetAccumulator(port, &status); + SPIJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); +} + +/* + * Class: edu_wpi_first_wpilibj_hal_SPIJNI + * Method: spiSetAccumulatorCenter + * Signature: (BI)V + */ +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiSetAccumulatorCenter + (JNIEnv *env, jclass, jbyte port, jint center) +{ + SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiSetAccumulatorCenter"; + SPIJNI_LOG(logDEBUG) << "Port = " << (jint) port; + SPIJNI_LOG(logDEBUG) << "Center = " << center; + int32_t status = 0; + spiSetAccumulatorCenter(port, center, &status); + SPIJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); +} + +/* + * Class: edu_wpi_first_wpilibj_hal_SPIJNI + * Method: spiSetAccumulatorDeadband + * Signature: (BI)V + */ +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiSetAccumulatorDeadband + (JNIEnv *env, jclass, jbyte port, jint deadband) +{ + SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiSetAccumulatorDeadband"; + SPIJNI_LOG(logDEBUG) << "Port = " << (jint) port; + SPIJNI_LOG(logDEBUG) << "Deadband = " << deadband; + int32_t status = 0; + spiSetAccumulatorDeadband(port, deadband, &status); + SPIJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); +} + +/* + * Class: edu_wpi_first_wpilibj_hal_SPIJNI + * Method: spiGetAccumulatorLastValue + * Signature: (B)I + */ +JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiGetAccumulatorLastValue + (JNIEnv *env, jclass, jbyte port) +{ + SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiGetAccumulatorLastValue"; + SPIJNI_LOG(logDEBUG) << "Port = " << (jint) port; + int32_t status = 0; + jint retVal = spiGetAccumulatorLastValue(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: (B)J + */ +JNIEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiGetAccumulatorValue + (JNIEnv *env, jclass, jbyte port) +{ + SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiGetAccumulatorValue"; + SPIJNI_LOG(logDEBUG) << "Port = " << (jint) port; + int32_t status = 0; + jlong retVal = spiGetAccumulatorValue(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: (B)I + */ +JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiGetAccumulatorCount + (JNIEnv *env, jclass, jbyte port) +{ + SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiGetAccumulatorCount"; + SPIJNI_LOG(logDEBUG) << "Port = " << (jint) port; + int32_t status = 0; + jint retVal = spiGetAccumulatorCount(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: (B)D + */ +JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiGetAccumulatorAverage + (JNIEnv *env, jclass, jbyte port) +{ + SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiGetAccumulatorAverage"; + SPIJNI_LOG(logDEBUG) << "Port = " << (jint) port; + int32_t status = 0; + jdouble retVal = spiGetAccumulatorAverage(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: (BLjava/nio/LongBuffer;Ljava/nio/IntBuffer;)V + */ +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiGetAccumulatorOutput + (JNIEnv *env, jclass, jbyte port, jobject value, jobject count) +{ + SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiGetAccumulatorOutput"; + SPIJNI_LOG(logDEBUG) << "Port = " << (jint) port; + int32_t status = 0; + + jlong * valuePtr = (jlong*)env->GetDirectBufferAddress(value); + uint32_t * countPtr = (uint32_t*)env->GetDirectBufferAddress(count); + + spiGetAccumulatorOutput(port, valuePtr, countPtr, &status); + + SPIJNI_LOG(logDEBUG) << "Status = " << status; + SPIJNI_LOG(logDEBUG) << "Value = " << *valuePtr; + SPIJNI_LOG(logDEBUG) << "Count = " << *countPtr; + CheckStatus(env, status); +} + } // extern "C" diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/SPI.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/SPI.java index 108c640cae..b92a359e59 100644 --- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/SPI.java +++ b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/SPI.java @@ -1,6 +1,9 @@ package edu.wpi.first.wpilibj; +import java.nio.ByteOrder; import java.nio.ByteBuffer; +import java.nio.IntBuffer; +import java.nio.LongBuffer; import edu.wpi.first.wpilibj.communication.FRCNetworkCommunicationsLibrary.tResourceType; import edu.wpi.first.wpilibj.communication.UsageReporting; @@ -255,4 +258,119 @@ public class SPI extends SensorBase { throw new IllegalArgumentException("dataReceived is too small, must be at least " + size); return SPIJNI.spiTransaction(m_port, dataToSend, dataReceived, (byte) size); } + + /** + * Initialize the accumulator. + * + * @param period Time between reads + * @param cmd SPI command to send to request data + * @param xfer_size SPI transfer size, in bytes + * @param valid_mask Mask to apply to received data for validity checking + * @param valid_data After valid_mask is applied, required matching value for + * validity checking + * @param data_shift Bit shift to apply to received data to get actual data + * value + * @param data_size Size (in bits) of data field + * @param is_signed Is data field signed? + * @param big_endian Is device big endian? + */ + public void initAccumulator(double period, int cmd, int xfer_size, + int valid_mask, int valid_value, + int data_shift, int data_size, + boolean is_signed, boolean big_endian) { + SPIJNI.spiInitAccumulator(m_port, (int)(period * 1.0e6), cmd, + (byte)xfer_size, valid_mask, valid_value, (byte)data_shift, + (byte)data_size, is_signed, big_endian); + } + + /** + * Frees the accumulator. + */ + public void freeAccumulator() { + SPIJNI.spiFreeAccumulator(m_port); + } + + /** + * Resets the accumulator to zero. + */ + public void resetAccumulator() { + SPIJNI.spiResetAccumulator(m_port); + } + + /** + * 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. + */ + public void setAccumulatorCenter(int center) { + SPIJNI.spiSetAccumulatorCenter(m_port, center); + } + + /** + * Set the accumulator's deadband. + */ + public void setAccumulatorDeadband(int deadband) { + SPIJNI.spiSetAccumulatorDeadband(m_port, deadband); + } + + /** + * Read the last value read by the accumulator engine. + */ + public int getAccumulatorLastValue() { + return SPIJNI.spiGetAccumulatorLastValue(m_port); + } + + /** + * Read the accumulated value. + * + * @return The 64-bit value accumulated since the last Reset(). + */ + public long getAccumulatorValue() { + return SPIJNI.spiGetAccumulatorValue(m_port); + } + + /** + * 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. + */ + public int getAccumulatorCount() { + return SPIJNI.spiGetAccumulatorCount(m_port); + } + + /** + * Read the average of the accumulated value. + * + * @return The accumulated average value (value / count). + */ + public double getAccumulatorAverage() { + return SPIJNI.spiGetAccumulatorAverage(m_port); + } + + /** + * 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 result AccumulatorResult object to store the results in. + */ + public void getAccumulatorOutput(AccumulatorResult result) { + if (result == null) { + throw new IllegalArgumentException("Null parameter `result'"); + } + ByteBuffer value = ByteBuffer.allocateDirect(8); + // set the byte order + value.order(ByteOrder.LITTLE_ENDIAN); + ByteBuffer count = ByteBuffer.allocateDirect(4); + // set the byte order + count.order(ByteOrder.LITTLE_ENDIAN); + SPIJNI.spiGetAccumulatorOutput(m_port, value.asLongBuffer(), count.asIntBuffer()); + result.value = value.asLongBuffer().get(0); + result.count = count.asIntBuffer().get(0); + } } diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/hal/SPIJNI.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/hal/SPIJNI.java index 8e3442d617..cc800544dc 100644 --- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/hal/SPIJNI.java +++ b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/hal/SPIJNI.java @@ -1,6 +1,8 @@ package edu.wpi.first.wpilibj.hal; import java.nio.ByteBuffer; +import java.nio.IntBuffer; +import java.nio.LongBuffer; public class SPIJNI extends JNIWrapper { public static native void spiInitialize(byte port); @@ -22,4 +24,27 @@ public class SPIJNI extends JNIWrapper { public static native void spiSetChipSelectActiveHigh(byte port); public static native void spiSetChipSelectActiveLow(byte port); + + public static native void spiInitAccumulator(byte port, int period, int cmd, + byte xferSize, int validMask, int validValue, byte dataShift, + byte dataSize, boolean isSigned, boolean bigEndian); + + public static native void spiFreeAccumulator(byte port); + + public static native void spiResetAccumulator(byte port); + + public static native void spiSetAccumulatorCenter(byte port, int center); + + public static native void spiSetAccumulatorDeadband(byte port, int deadband); + + public static native int spiGetAccumulatorLastValue(byte port); + + public static native long spiGetAccumulatorValue(byte port); + + public static native int spiGetAccumulatorCount(byte port); + + public static native double spiGetAccumulatorAverage(byte port); + + public static native void spiGetAccumulatorOutput(byte port, LongBuffer value, + IntBuffer count); }