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