From dcbf02a1ecfc1730f48753acf4ba1682c0f3ebd3 Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Thu, 6 Dec 2018 22:29:20 -0800 Subject: [PATCH] Update auto SPI for timestamp changes (#1457) The 2019 FPGA image switched the output of auto SPI from plain bytes to a sequence of 32-bit words (timestamp, then words with the byte values in the least significant byte of each word). In addition to changing the HAL and simulators to reflect this, add piecewise integration support to wpilibc/wpilibj SPI to take advantage of the timestamps and use it in the ADXRS450 gyro. --- .../main/java/edu/wpi/first/hal/SPIJNI.java | 2 +- .../sim/SpiReadAutoReceiveBufferCallback.java | 2 +- hal/src/main/native/athena/SPI.cpp | 2 +- hal/src/main/native/cpp/jni/SPIJNI.cpp | 16 +-- hal/src/main/native/include/hal/SPI.h | 12 +- .../main/native/include/mockdata/SPIData.h | 2 +- hal/src/main/native/sim/SPI.cpp | 2 +- hal/src/main/native/sim/jni/SimulatorJNI.cpp | 2 +- .../SpiReadAutoReceiveBufferCallbackStore.cpp | 11 +- .../SpiReadAutoReceiveBufferCallbackStore.h | 2 +- hal/src/main/native/sim/mockdata/SPIData.cpp | 2 +- .../native/sim/mockdata/SPIDataInternal.h | 2 +- .../cpp/ADXRS450_SpiGyroWrapperData.cpp | 47 ++++--- .../include/ADXRS450_SpiGyroWrapperData.h | 2 +- wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp | 6 +- wpilibc/src/main/native/cpp/SPI.cpp | 65 +++++++-- wpilibc/src/main/native/include/frc/SPI.h | 43 +++++- .../edu/wpi/first/wpilibj/ADXRS450_Gyro.java | 6 +- .../main/java/edu/wpi/first/wpilibj/SPI.java | 133 ++++++++++++++---- 19 files changed, 271 insertions(+), 88 deletions(-) diff --git a/hal/src/main/java/edu/wpi/first/hal/SPIJNI.java b/hal/src/main/java/edu/wpi/first/hal/SPIJNI.java index 164c6d149b..c473181ad7 100644 --- a/hal/src/main/java/edu/wpi/first/hal/SPIJNI.java +++ b/hal/src/main/java/edu/wpi/first/hal/SPIJNI.java @@ -57,7 +57,7 @@ public class SPIJNI extends JNIWrapper { public static native int spiReadAutoReceivedData(int port, ByteBuffer buffer, int numToRead, double timeout); - public static native int spiReadAutoReceivedData(int port, byte[] buffer, int numToRead, + public static native int spiReadAutoReceivedData(int port, int[] buffer, int numToRead, double timeout); public static native int spiGetAutoDroppedCount(int port); diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/SpiReadAutoReceiveBufferCallback.java b/hal/src/main/java/edu/wpi/first/hal/sim/SpiReadAutoReceiveBufferCallback.java index d14531aa4a..5083fabc7c 100644 --- a/hal/src/main/java/edu/wpi/first/hal/sim/SpiReadAutoReceiveBufferCallback.java +++ b/hal/src/main/java/edu/wpi/first/hal/sim/SpiReadAutoReceiveBufferCallback.java @@ -8,5 +8,5 @@ package edu.wpi.first.hal.sim; public interface SpiReadAutoReceiveBufferCallback { - int callback(String name, byte[] buffer, int numToRead); + int callback(String name, int[] buffer, int numToRead); } diff --git a/hal/src/main/native/athena/SPI.cpp b/hal/src/main/native/athena/SPI.cpp index ed5a713f41..ddced41b9a 100644 --- a/hal/src/main/native/athena/SPI.cpp +++ b/hal/src/main/native/athena/SPI.cpp @@ -604,7 +604,7 @@ void HAL_ForceSPIAutoRead(HAL_SPIPort port, int32_t* status) { spiSystem->strobeAutoForceOne(status); } -int32_t HAL_ReadSPIAutoReceivedData(HAL_SPIPort port, uint8_t* buffer, +int32_t HAL_ReadSPIAutoReceivedData(HAL_SPIPort port, uint32_t* buffer, int32_t numToRead, double timeout, int32_t* status) { std::lock_guard lock(spiAutoMutex); diff --git a/hal/src/main/native/cpp/jni/SPIJNI.cpp b/hal/src/main/native/cpp/jni/SPIJNI.cpp index d75f710f83..d5940584ec 100644 --- a/hal/src/main/native/cpp/jni/SPIJNI.cpp +++ b/hal/src/main/native/cpp/jni/SPIJNI.cpp @@ -436,8 +436,8 @@ Java_edu_wpi_first_hal_SPIJNI_spiReadAutoReceivedData__ILjava_nio_ByteBuffer_2ID SPIJNI_LOG(logDEBUG) << "Port = " << port; SPIJNI_LOG(logDEBUG) << "NumToRead = " << numToRead; SPIJNI_LOG(logDEBUG) << "Timeout = " << timeout; - uint8_t* recvBuf = - reinterpret_cast(env->GetDirectBufferAddress(buffer)); + uint32_t* recvBuf = + reinterpret_cast(env->GetDirectBufferAddress(buffer)); int32_t status = 0; jint retval = HAL_ReadSPIAutoReceivedData( static_cast(port), recvBuf, numToRead, timeout, &status); @@ -450,18 +450,18 @@ Java_edu_wpi_first_hal_SPIJNI_spiReadAutoReceivedData__ILjava_nio_ByteBuffer_2ID /* * Class: edu_wpi_first_hal_SPIJNI * Method: spiReadAutoReceivedData - * Signature: (I[BID)I + * Signature: (I[IID)I */ JNIEXPORT jint JNICALL -Java_edu_wpi_first_hal_SPIJNI_spiReadAutoReceivedData__I_3BID - (JNIEnv* env, jclass, jint port, jbyteArray buffer, jint numToRead, +Java_edu_wpi_first_hal_SPIJNI_spiReadAutoReceivedData__I_3IID + (JNIEnv* env, jclass, jint port, jintArray 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; - wpi::SmallVector recvBuf; + wpi::SmallVector recvBuf; recvBuf.resize(numToRead); int32_t status = 0; jint retval = @@ -471,8 +471,8 @@ Java_edu_wpi_first_hal_SPIJNI_spiReadAutoReceivedData__I_3BID SPIJNI_LOG(logDEBUG) << "Return = " << retval; if (!CheckStatus(env, status)) return retval; if (numToRead > 0) { - env->SetByteArrayRegion(buffer, 0, numToRead, - reinterpret_cast(recvBuf.data())); + env->SetIntArrayRegion(buffer, 0, numToRead, + reinterpret_cast(recvBuf.data())); } return retval; } diff --git a/hal/src/main/native/include/hal/SPI.h b/hal/src/main/native/include/hal/SPI.h index 657b20e7f2..4f1815f1bb 100644 --- a/hal/src/main/native/include/hal/SPI.h +++ b/hal/src/main/native/include/hal/SPI.h @@ -218,16 +218,20 @@ void HAL_SetSPIAutoTransmitData(HAL_SPIPort port, const uint8_t* dataToSend, void HAL_ForceSPIAutoRead(HAL_SPIPort port, int32_t* status); /** - * Reads data received by the SPI accumulator. + * Reads data received by the SPI accumulator. Each received data sequence + * consists of a timestamp followed by the received data bytes, one byte per + * word (in the least significant byte). The length of each received data + * sequence is the same as the combined dataSize + zeroSize set in + * HAL_SetSPIAutoTransmitData. * * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 * for MXP. * @param buffer The buffer to store the data into. - * @param numToRead The number of bytes to read. + * @param numToRead The number of words to read. * @param timeout The read timeout (in seconds). - * @return The number of bytes actually read. + * @return The number of words actually read. */ -int32_t HAL_ReadSPIAutoReceivedData(HAL_SPIPort port, uint8_t* buffer, +int32_t HAL_ReadSPIAutoReceivedData(HAL_SPIPort port, uint32_t* buffer, int32_t numToRead, double timeout, int32_t* status); diff --git a/hal/src/main/native/include/mockdata/SPIData.h b/hal/src/main/native/include/mockdata/SPIData.h index 4f0cc75870..54a2ec986d 100644 --- a/hal/src/main/native/include/mockdata/SPIData.h +++ b/hal/src/main/native/include/mockdata/SPIData.h @@ -14,7 +14,7 @@ typedef void (*HAL_SpiReadAutoReceiveBufferCallback)(const char* name, void* param, - unsigned char* buffer, + uint32_t* buffer, int32_t numToRead, int32_t* outputCount); diff --git a/hal/src/main/native/sim/SPI.cpp b/hal/src/main/native/sim/SPI.cpp index 6de383e39f..8c539d3ec0 100644 --- a/hal/src/main/native/sim/SPI.cpp +++ b/hal/src/main/native/sim/SPI.cpp @@ -54,7 +54,7 @@ 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 HAL_ReadSPIAutoReceivedData(HAL_SPIPort port, uint32_t* buffer, int32_t numToRead, double timeout, int32_t* status) { return SimSPIData[port].ReadAutoReceivedData(buffer, numToRead, timeout, diff --git a/hal/src/main/native/sim/jni/SimulatorJNI.cpp b/hal/src/main/native/sim/jni/SimulatorJNI.cpp index 8a0a005e55..d1e0f7d6bb 100644 --- a/hal/src/main/native/sim/jni/SimulatorJNI.cpp +++ b/hal/src/main/native/sim/jni/SimulatorJNI.cpp @@ -69,7 +69,7 @@ jint SimOnLoad(JavaVM* vm, void* reserved) { spiReadAutoReceiveBufferCallbackCallback = env->GetMethodID(spiReadAutoReceiveBufferCallbackCls, "callback", - "(Ljava/lang/String;[BI)I"); + "(Ljava/lang/String;[II)I"); if (!spiReadAutoReceiveBufferCallbackCallback) return JNI_ERR; InitializeStore(); diff --git a/hal/src/main/native/sim/jni/SpiReadAutoReceiveBufferCallbackStore.cpp b/hal/src/main/native/sim/jni/SpiReadAutoReceiveBufferCallbackStore.cpp index 164fe52cf0..935c8bf7d4 100644 --- a/hal/src/main/native/sim/jni/SpiReadAutoReceiveBufferCallbackStore.cpp +++ b/hal/src/main/native/sim/jni/SpiReadAutoReceiveBufferCallbackStore.cpp @@ -39,7 +39,7 @@ void SpiReadAutoReceiveBufferCallbackStore::create(JNIEnv* env, jobject obj) { } int32_t SpiReadAutoReceiveBufferCallbackStore::performCallback( - const char* name, unsigned char* buffer, int32_t numToRead) { + const char* name, uint32_t* buffer, int32_t numToRead) { JNIEnv* env; JavaVM* vm = sim::GetJVM(); bool didAttachThread = false; @@ -58,15 +58,14 @@ int32_t SpiReadAutoReceiveBufferCallbackStore::performCallback( wpi::outs().flush(); } - auto toCallbackArr = - MakeJByteArray(env, wpi::StringRef{reinterpret_cast(buffer), - static_cast(numToRead)}); + auto toCallbackArr = MakeJIntArray( + env, wpi::ArrayRef{buffer, static_cast(numToRead)}); jint ret = env->CallIntMethod(m_call, sim::GetBufferCallback(), MakeJString(env, name), toCallbackArr, (jint)numToRead); - jbyte* fromCallbackArr = reinterpret_cast( + jint* fromCallbackArr = reinterpret_cast( env->GetPrimitiveArrayCritical(toCallbackArr, nullptr)); for (int i = 0; i < ret; i++) { @@ -106,7 +105,7 @@ SIM_JniHandle sim::AllocateSpiBufferCallback( callbackStore->create(env, callback); - auto callbackFunc = [](const char* name, void* param, unsigned char* buffer, + auto callbackFunc = [](const char* name, void* param, uint32_t* buffer, int32_t numToRead, int32_t* outputCount) { uintptr_t handleTmp = reinterpret_cast(param); SIM_JniHandle handle = static_cast(handleTmp); diff --git a/hal/src/main/native/sim/jni/SpiReadAutoReceiveBufferCallbackStore.h b/hal/src/main/native/sim/jni/SpiReadAutoReceiveBufferCallbackStore.h index df8e2e04f8..643a874a66 100644 --- a/hal/src/main/native/sim/jni/SpiReadAutoReceiveBufferCallbackStore.h +++ b/hal/src/main/native/sim/jni/SpiReadAutoReceiveBufferCallbackStore.h @@ -22,7 +22,7 @@ namespace sim { class SpiReadAutoReceiveBufferCallbackStore { public: void create(JNIEnv* env, jobject obj); - int32_t performCallback(const char* name, unsigned char* buffer, + int32_t performCallback(const char* name, uint32_t* buffer, int32_t numToRead); void free(JNIEnv* env); void setCallbackId(int32_t id) { callbackId = id; } diff --git a/hal/src/main/native/sim/mockdata/SPIData.cpp b/hal/src/main/native/sim/mockdata/SPIData.cpp index ccc28e03bd..3afc606d41 100644 --- a/hal/src/main/native/sim/mockdata/SPIData.cpp +++ b/hal/src/main/native/sim/mockdata/SPIData.cpp @@ -46,7 +46,7 @@ int32_t SPIData::Transaction(const uint8_t* dataToSend, uint8_t* dataReceived, return size; } -int32_t SPIData::ReadAutoReceivedData(uint8_t* buffer, int32_t numToRead, +int32_t SPIData::ReadAutoReceivedData(uint32_t* buffer, int32_t numToRead, double timeout, int32_t* status) { int32_t outputCount = 0; autoReceivedData(buffer, numToRead, &outputCount); diff --git a/hal/src/main/native/sim/mockdata/SPIDataInternal.h b/hal/src/main/native/sim/mockdata/SPIDataInternal.h index c70bb0fa1d..b10e741ba4 100644 --- a/hal/src/main/native/sim/mockdata/SPIDataInternal.h +++ b/hal/src/main/native/sim/mockdata/SPIDataInternal.h @@ -24,7 +24,7 @@ class SPIData { int32_t Write(const uint8_t* dataToSend, int32_t sendSize); int32_t Transaction(const uint8_t* dataToSend, uint8_t* dataReceived, int32_t size); - int32_t ReadAutoReceivedData(uint8_t* buffer, int32_t numToRead, + int32_t ReadAutoReceivedData(uint32_t* buffer, int32_t numToRead, double timeout, int32_t* status); SimDataValue initialized{false}; diff --git a/simulation/halsim_adx_gyro_accelerometer/src/main/native/cpp/ADXRS450_SpiGyroWrapperData.cpp b/simulation/halsim_adx_gyro_accelerometer/src/main/native/cpp/ADXRS450_SpiGyroWrapperData.cpp index 110aec437a..5658b30ac4 100644 --- a/simulation/halsim_adx_gyro_accelerometer/src/main/native/cpp/ADXRS450_SpiGyroWrapperData.cpp +++ b/simulation/halsim_adx_gyro_accelerometer/src/main/native/cpp/ADXRS450_SpiGyroWrapperData.cpp @@ -11,6 +11,7 @@ #include #include +#include #include #ifdef _WIN32 @@ -22,9 +23,11 @@ using namespace hal; -const double ADXRS450_SpiGyroWrapper::kAngleLsb = 1 / 0.0125 / 0.0005; +static constexpr double kSamplePeriod = 0.0005; + +const double ADXRS450_SpiGyroWrapper::kAngleLsb = 1 / 0.0125 / kSamplePeriod; const double ADXRS450_SpiGyroWrapper::kMaxAngleDeltaPerMessage = 0.1875; -const int ADXRS450_SpiGyroWrapper::kPacketSize = 4; +const int ADXRS450_SpiGyroWrapper::kPacketSize = 4 + 1; // +1 for timestamp template constexpr const T& clamp(const T& value, const T& low, const T& high) { @@ -38,7 +41,8 @@ static void ADXRS450SPI_ReadBufferCallback(const char* name, void* param, } static void ADXRS450SPI_ReadAutoReceivedData(const char* name, void* param, - uint8_t* buffer, int32_t numToRead, + uint32_t* buffer, + int32_t numToRead, int32_t* outputCount) { auto sim = static_cast(param); sim->HandleAutoReceiveData(buffer, numToRead, *outputCount); @@ -71,13 +75,14 @@ void ADXRS450_SpiGyroWrapper::HandleRead(uint8_t* buffer, uint32_t count) { std::memcpy(&buffer[0], &returnCode, sizeof(returnCode)); } -void ADXRS450_SpiGyroWrapper::HandleAutoReceiveData(uint8_t* buffer, +void ADXRS450_SpiGyroWrapper::HandleAutoReceiveData(uint32_t* buffer, int32_t numToRead, int32_t& outputCount) { std::lock_guard lock(m_angle.GetMutex()); - int32_t messagesToSend = std::abs( - m_angleDiff > 0 ? std::ceil(m_angleDiff / kMaxAngleDeltaPerMessage) - : std::floor(m_angleDiff / kMaxAngleDeltaPerMessage)); + int32_t messagesToSend = + 1 + std::abs(m_angleDiff > 0 + ? std::ceil(m_angleDiff / kMaxAngleDeltaPerMessage) + : std::floor(m_angleDiff / kMaxAngleDeltaPerMessage)); // Zero gets passed in during the "How much data do I need to read" step. // Else it is actually reading the accumulator @@ -87,24 +92,34 @@ void ADXRS450_SpiGyroWrapper::HandleAutoReceiveData(uint8_t* buffer, } int valuesToRead = numToRead / kPacketSize; - std::memset(&buffer[0], 0, numToRead); + std::memset(&buffer[0], 0, numToRead * sizeof(uint32_t)); - int msgCtr = 0; + int32_t status = 0; + uint32_t timestamp = HAL_GetFPGATime(&status); - while (msgCtr < valuesToRead) { - double cappedDiff = - clamp(m_angleDiff, -kMaxAngleDeltaPerMessage, kMaxAngleDeltaPerMessage); + for (int msgCtr = 0; msgCtr < valuesToRead; ++msgCtr) { + // force the first message to be a rate of 0 to init the timestamp + double cappedDiff = (msgCtr == 0) + ? 0 + : clamp(m_angleDiff, -kMaxAngleDeltaPerMessage, + kMaxAngleDeltaPerMessage); + + // first word is timestamp + buffer[msgCtr * kPacketSize] = timestamp; int32_t valueToSend = ((static_cast(cappedDiff * kAngleLsb) << 10) & (~0x0C00000E)) | 0x04000000; - valueToSend = ntohl(valueToSend); - std::memcpy(&buffer[msgCtr * kPacketSize], &valueToSend, - sizeof(valueToSend)); + // following words have byte in LSB, in big endian order + for (int i = 4; i >= 1; --i) { + buffer[msgCtr * kPacketSize + i] = + static_cast(valueToSend) & 0xffu; + valueToSend >>= 8; + } m_angleDiff -= cappedDiff; - msgCtr += 1; + timestamp += kSamplePeriod * 1e6; // fpga time is in us } } diff --git a/simulation/halsim_adx_gyro_accelerometer/src/main/native/include/ADXRS450_SpiGyroWrapperData.h b/simulation/halsim_adx_gyro_accelerometer/src/main/native/include/ADXRS450_SpiGyroWrapperData.h index 0a30047723..25e83766fa 100644 --- a/simulation/halsim_adx_gyro_accelerometer/src/main/native/include/ADXRS450_SpiGyroWrapperData.h +++ b/simulation/halsim_adx_gyro_accelerometer/src/main/native/include/ADXRS450_SpiGyroWrapperData.h @@ -18,7 +18,7 @@ class ADXRS450_SpiGyroWrapper { bool GetInitialized() const; void HandleRead(uint8_t* buffer, uint32_t count); - void HandleAutoReceiveData(uint8_t* buffer, int32_t numToRead, + void HandleAutoReceiveData(uint32_t* buffer, int32_t numToRead, int32_t& outputCount); int32_t RegisterAngleCallback(HAL_NotifyCallback callback, void* param, diff --git a/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp b/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp index 0dbeed30ed..875f403b64 100644 --- a/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp +++ b/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp @@ -86,7 +86,7 @@ uint16_t ADXRS450_Gyro::ReadRegister(int reg) { } double ADXRS450_Gyro::GetAngle() const { - return m_spi.GetAccumulatorValue() * kDegreePerSecondPerLSB * kSamplePeriod; + return m_spi.GetAccumulatorIntegratedValue() * kDegreePerSecondPerLSB; } double ADXRS450_Gyro::GetRate() const { @@ -99,11 +99,11 @@ void ADXRS450_Gyro::Reset() { m_spi.ResetAccumulator(); } void ADXRS450_Gyro::Calibrate() { Wait(0.1); - m_spi.SetAccumulatorCenter(0); + m_spi.SetAccumulatorIntegratedCenter(0); m_spi.ResetAccumulator(); Wait(kCalibrationSampleTime); - m_spi.SetAccumulatorCenter(static_cast(m_spi.GetAccumulatorAverage())); + m_spi.SetAccumulatorIntegratedCenter(m_spi.GetAccumulatorIntegratedAverage()); m_spi.ResetAccumulator(); } diff --git a/wpilibc/src/main/native/cpp/SPI.cpp b/wpilibc/src/main/native/cpp/SPI.cpp index 25547e5200..6b41adb4b7 100644 --- a/wpilibc/src/main/native/cpp/SPI.cpp +++ b/wpilibc/src/main/native/cpp/SPI.cpp @@ -31,13 +31,13 @@ class SPI::Accumulator { std::lock_guard lock(m_mutex); Update(); }), - m_buf(new uint8_t[xferSize * kAccumulateDepth]), + m_buf(new uint32_t[(xferSize + 1) * kAccumulateDepth]), m_validMask(validMask), m_validValue(validValue), m_dataMax(1 << dataSize), m_dataMsbMask(1 << (dataSize - 1)), m_dataShift(dataShift), - m_xferSize(xferSize), + m_xferSize(xferSize + 1), // +1 for timestamp m_isSigned(isSigned), m_bigEndian(bigEndian), m_port(port) {} @@ -46,15 +46,18 @@ class SPI::Accumulator { void Update(); Notifier m_notifier; - uint8_t* m_buf; + uint32_t* m_buf; wpi::mutex m_mutex; int64_t m_value = 0; uint32_t m_count = 0; int32_t m_lastValue = 0; + uint32_t m_lastTimestamp = 0; + double m_integratedValue = 0; int32_t m_center = 0; int32_t m_deadband = 0; + double m_integratedCenter = 0; int32_t m_validMask; int32_t m_validValue; @@ -78,7 +81,7 @@ void SPI::Accumulator::Update() { HAL_ReadSPIAutoReceivedData(m_port, m_buf, 0, 0, &status); if (status != 0) return; // error reading - // only get whole responses + // only get whole responses; +1 is for timestamp numToRead -= numToRead % m_xferSize; if (numToRead > m_xferSize * kAccumulateDepth) { numToRead = m_xferSize * kAccumulateDepth; @@ -92,15 +95,18 @@ void SPI::Accumulator::Update() { // loop over all responses for (int32_t off = 0; off < numToRead; off += m_xferSize) { + // get timestamp from first word + uint32_t timestamp = m_buf[off]; + // convert from bytes uint32_t resp = 0; if (m_bigEndian) { - for (int32_t i = 0; i < m_xferSize; ++i) { + for (int32_t i = 1; i < m_xferSize; ++i) { resp <<= 8; resp |= m_buf[off + i] & 0xff; } } else { - for (int32_t i = m_xferSize - 1; i >= 0; --i) { + for (int32_t i = m_xferSize - 1; i >= 1; --i) { resp <<= 8; resp |= m_buf[off + i] & 0xff; } @@ -114,15 +120,34 @@ void SPI::Accumulator::Update() { // 2s complement conversion if signed MSB is set if (m_isSigned && (data & m_dataMsbMask) != 0) data -= m_dataMax; // center offset + int32_t dataNoCenter = data; data -= m_center; // only accumulate if outside deadband - if (data < -m_deadband || data > m_deadband) m_value += data; + if (data < -m_deadband || data > m_deadband) { + m_value += data; + if (m_count != 0) { + // timestamps use the 1us FPGA clock; also handle rollover + if (timestamp >= m_lastTimestamp) + m_integratedValue += + dataNoCenter * + static_cast(timestamp - m_lastTimestamp) * 1e-6 - + m_integratedCenter; + else + m_integratedValue += + dataNoCenter * + static_cast((1ULL << 32) - m_lastTimestamp + + timestamp) * + 1e-6 - + m_integratedCenter; + } + } ++m_count; m_lastValue = data; } else { // no data from the sensor; just clear the last value m_lastValue = 0; } + m_lastTimestamp = timestamp; } } while (!done); } @@ -284,7 +309,7 @@ void SPI::ForceAutoRead() { wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); } -int SPI::ReadAutoReceivedData(uint8_t* buffer, int numToRead, double timeout) { +int SPI::ReadAutoReceivedData(uint32_t* buffer, int numToRead, double timeout) { int32_t status = 0; int32_t val = HAL_ReadSPIAutoReceivedData(m_port, buffer, numToRead, timeout, &status); @@ -337,6 +362,8 @@ void SPI::ResetAccumulator() { m_accum->m_value = 0; m_accum->m_count = 0; m_accum->m_lastValue = 0; + m_accum->m_lastTimestamp = 0; + m_accum->m_integratedValue = 0; } void SPI::SetAccumulatorCenter(int center) { @@ -391,3 +418,25 @@ void SPI::GetAccumulatorOutput(int64_t& value, int64_t& count) const { value = m_accum->m_value; count = m_accum->m_count; } + +void SPI::SetAccumulatorIntegratedCenter(double center) { + if (!m_accum) return; + std::lock_guard lock(m_accum->m_mutex); + m_accum->m_integratedCenter = center; +} + +double SPI::GetAccumulatorIntegratedValue() const { + if (!m_accum) return 0; + std::lock_guard lock(m_accum->m_mutex); + m_accum->Update(); + return m_accum->m_integratedValue; +} + +double SPI::GetAccumulatorIntegratedAverage() const { + if (!m_accum) return 0; + std::lock_guard lock(m_accum->m_mutex); + m_accum->Update(); + if (m_accum->m_count <= 1) return 0.0; + // count-1 due to not integrating the first value received + return m_accum->m_integratedValue / (m_accum->m_count - 1); +} diff --git a/wpilibc/src/main/native/include/frc/SPI.h b/wpilibc/src/main/native/include/frc/SPI.h index c5963df984..88cd4d96c0 100644 --- a/wpilibc/src/main/native/include/frc/SPI.h +++ b/wpilibc/src/main/native/include/frc/SPI.h @@ -211,15 +211,20 @@ class SPI : public ErrorBase { * 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. + * Each received data sequence consists of a timestamp followed by the + * received data bytes, one byte per word (in the least significant byte). + * The length of each received data sequence is the same as the combined + * size of the data and zeroSize set in SetAutoTransmitData(). * - * @param buffer buffer where read bytes are stored - * @param numToRead number of bytes to read + * Blocks until numToRead words have been read or timeout expires. + * May be called with numToRead=0 to retrieve how many words are available. + * + * @param buffer buffer where read words are stored + * @param numToRead number of words to read * @param timeout timeout in seconds (ms resolution) - * @return Number of bytes remaining to be read + * @return Number of words remaining to be read */ - int ReadAutoReceivedData(uint8_t* buffer, int numToRead, double timeout); + int ReadAutoReceivedData(uint32_t* buffer, int numToRead, double timeout); /** * Get the number of bytes dropped by the automatic SPI transfer engine due @@ -313,6 +318,32 @@ class SPI : public ErrorBase { */ void GetAccumulatorOutput(int64_t& value, int64_t& count) const; + /** + * Set the center value of the accumulator integrator. + * + * The center value is subtracted from each value*dt before it is added to the + * integrated value. This is used for the center value of devices like gyros + * and accelerometers to take the device offset into account when integrating. + */ + void SetAccumulatorIntegratedCenter(double center); + + /** + * Read the integrated value. This is the sum of (each value * time between + * values). + * + * @return The integrated value accumulated since the last Reset(). + */ + double GetAccumulatorIntegratedValue() const; + + /** + * Read the average of the integrated value. This is the sum of (each value + * times the time between values), divided by the count. + * + * @return The average of the integrated value accumulated since the last + * Reset(). + */ + double GetAccumulatorIntegratedAverage() const; + protected: HAL_SPIPort m_port = HAL_SPI_kInvalid; bool m_msbFirst = false; // Default little-endian 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 a5a862d7f6..a34b373953 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 @@ -92,12 +92,12 @@ public class ADXRS450_Gyro extends GyroBase implements Gyro, PIDSource, Sendable Timer.delay(0.1); - m_spi.setAccumulatorCenter(0); + m_spi.setAccumulatorIntegratedCenter(0); m_spi.resetAccumulator(); Timer.delay(kCalibrationSampleTime); - m_spi.setAccumulatorCenter((int) m_spi.getAccumulatorAverage()); + m_spi.setAccumulatorIntegratedCenter(m_spi.getAccumulatorIntegratedAverage()); m_spi.resetAccumulator(); } @@ -154,7 +154,7 @@ public class ADXRS450_Gyro extends GyroBase implements Gyro, PIDSource, Sendable if (m_spi == null) { return 0.0; } - return m_spi.getAccumulatorValue() * kDegreePerSecondPerLSB * kSamplePeriod; + return m_spi.getAccumulatorIntegratedValue() * kDegreePerSecondPerLSB; } @Override 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 90c1f07f1e..70b9d82892 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SPI.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SPI.java @@ -8,6 +8,8 @@ package edu.wpi.first.wpilibj; import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import java.nio.IntBuffer; import edu.wpi.first.hal.AccumulatorResult; import edu.wpi.first.hal.FRCNetComm.tResourceType; @@ -371,24 +373,26 @@ public class SPI implements AutoCloseable { *

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. + *

Each received data sequence consists of a timestamp followed by the + * received data bytes, one byte per word (in the least significant byte). + * The length of each received data sequence is the same as the combined + * size of the data and zeroSize set in setAutoTransmitData(). * - * @param buffer buffer where read bytes are stored - * @param numToRead number of bytes to read + *

Blocks until numToRead words have been read or timeout expires. + * May be called with numToRead=0 to retrieve how many words are available. + * + * @param buffer buffer where read words are stored + * @param numToRead number of words to read * @param timeout timeout in seconds (ms resolution) - * @return Number of bytes remaining to be read + * @return Number of words remaining to be read */ - @SuppressWarnings("ByteBufferBackingArray") 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); + if (buffer.capacity() < numToRead * 4) { + throw new IllegalArgumentException("buffer is too small, must be at least " + + (numToRead * 4)); } return SPIJNI.spiReadAutoReceivedData(m_port, buffer, numToRead, timeout); } @@ -399,15 +403,20 @@ public class SPI implements AutoCloseable { *

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. + *

Each received data sequence consists of a timestamp followed by the + * received data bytes, one byte per word (in the least significant byte). + * The length of each received data sequence is the same as the combined + * size of the data and zeroSize set in setAutoTransmitData(). * - * @param buffer array where read bytes are stored - * @param numToRead number of bytes to read + *

Blocks until numToRead words have been read or timeout expires. + * May be called with numToRead=0 to retrieve how many words are available. + * + * @param buffer array where read words are stored + * @param numToRead number of words to read * @param timeout timeout in seconds (ms resolution) - * @return Number of bytes remaining to be read + * @return Number of words remaining to be read */ - public int readAutoReceivedData(byte[] buffer, int numToRead, double timeout) { + public int readAutoReceivedData(int[] buffer, int numToRead, double timeout) { if (buffer.length < numToRead) { throw new IllegalArgumentException("buffer is too small, must be at least " + numToRead); } @@ -431,8 +440,10 @@ public class SPI implements AutoCloseable { 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_buf = ByteBuffer.allocateDirect((xferSize + 1) * kAccumulateDepth * 4) + .order(ByteOrder.nativeOrder()); + m_intBuf = m_buf.asIntBuffer(); + m_xferSize = xferSize + 1; // +1 for timestamp m_validMask = validMask; m_validValue = validValue; m_dataShift = dataShift; @@ -450,14 +461,18 @@ public class SPI implements AutoCloseable { final Notifier m_notifier; final ByteBuffer m_buf; + final IntBuffer m_intBuf; final Object m_mutex = new Object(); long m_value; int m_count; int m_lastValue; + long m_lastTimestamp; + double m_integratedValue; int m_center; int m_deadband; + double m_integratedCenter; final int m_validMask; final int m_validValue; @@ -469,7 +484,7 @@ public class SPI implements AutoCloseable { final boolean m_bigEndian; // is response big endian? final int m_port; - @SuppressWarnings("PMD.CyclomaticComplexity") + @SuppressWarnings({"PMD.CyclomaticComplexity", "PMD.NPathComplexity"}) void update() { synchronized (m_mutex) { boolean done = false; @@ -494,17 +509,20 @@ public class SPI implements AutoCloseable { // loop over all responses for (int off = 0; off < numToRead; off += m_xferSize) { + // get timestamp from first word + long timestamp = m_intBuf.get(off) & 0xffffffffL; + // convert from bytes int resp = 0; if (m_bigEndian) { - for (int i = 0; i < m_xferSize; ++i) { + for (int i = 1; i < m_xferSize; ++i) { resp <<= 8; - resp |= ((int) m_buf.get(off + i)) & 0xff; + resp |= m_intBuf.get(off + i) & 0xff; } } else { - for (int i = m_xferSize - 1; i >= 0; --i) { + for (int i = m_xferSize - 1; i >= 1; --i) { resp <<= 8; - resp |= ((int) m_buf.get(off + i)) & 0xff; + resp |= m_intBuf.get(off + i) & 0xff; } } @@ -518,10 +536,21 @@ public class SPI implements AutoCloseable { data -= m_dataMax; } // center offset + int dataNoCenter = data; data -= m_center; // only accumulate if outside deadband if (data < -m_deadband || data > m_deadband) { m_value += data; + if (m_count != 0) { + // timestamps use the 1us FPGA clock; also handle rollover + if (timestamp >= m_lastTimestamp) { + m_integratedValue += dataNoCenter * (timestamp - m_lastTimestamp) + * 1e-6 - m_integratedCenter; + } else { + m_integratedValue += dataNoCenter * ((1L << 32) - m_lastTimestamp + timestamp) + * 1e-6 - m_integratedCenter; + } + } } ++m_count; m_lastValue = data; @@ -529,6 +558,7 @@ public class SPI implements AutoCloseable { // no data from the sensor; just clear the last value m_lastValue = 0; } + m_lastTimestamp = timestamp; } } } @@ -600,6 +630,8 @@ public class SPI implements AutoCloseable { m_accum.m_value = 0; m_accum.m_count = 0; m_accum.m_lastValue = 0; + m_accum.m_lastTimestamp = 0; + m_accum.m_integratedValue = 0; } } @@ -716,4 +748,57 @@ public class SPI implements AutoCloseable { result.count = m_accum.m_count; } } + + /** + * Set the center value of the accumulator integrator. + * + *

The center value is subtracted from each value*dt before it is added to the + * integrated value. This is used for the center value of devices like gyros + * and accelerometers to take the device offset into account when integrating. + */ + public void setAccumulatorIntegratedCenter(double center) { + if (m_accum == null) { + return; + } + synchronized (m_accum.m_mutex) { + m_accum.m_integratedCenter = center; + } + } + + /** + * Read the integrated value. This is the sum of (each value * time between + * values). + * + * @return The integrated value accumulated since the last Reset(). + */ + public double getAccumulatorIntegratedValue() { + if (m_accum == null) { + return 0; + } + synchronized (m_accum.m_mutex) { + m_accum.update(); + return m_accum.m_integratedValue; + } + } + + /** + * Read the average of the integrated value. This is the sum of (each value + * times the time between values), divided by the count. + * + * @return The average of the integrated value accumulated since the last + * Reset(). + */ + public double getAccumulatorIntegratedAverage() { + if (m_accum == null) { + return 0; + } + synchronized (m_accum.m_mutex) { + m_accum.update(); + if (m_accum.m_count <= 1) { + return 0.0; + } + // count-1 due to not integrating the first value received + return m_accum.m_integratedValue / (m_accum.m_count - 1); + } + } }