Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-24 01:31:46 +00:00
Code Issues Packages Projects Releases Wiki Activity

Update auto SPI for timestamp changes (#1457)

Browse Source 
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.
This commit is contained in:
Peter Johnson
2018-12-06 22:29:20 -08:00
committed by GitHub
parent 7e1ec28839
commit dcbf02a1ec
19 changed files with 271 additions and 88 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

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

2
hal/src/main/java/edu/wpi/first/hal/sim/SpiReadAutoReceiveBufferCallback.java
View File

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

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

@@ -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<wpi::mutex> lock(spiAutoMutex);

16
hal/src/main/native/cpp/jni/SPIJNI.cpp
View File

@@ -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<uint8_t*>(env->GetDirectBufferAddress(buffer));
uint32_t* recvBuf =
reinterpret_cast<uint32_t*>(env->GetDirectBufferAddress(buffer));
int32_t status = 0;
jint retval = HAL_ReadSPIAutoReceivedData(
static_cast<HAL_SPIPort>(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<uint8_t, 128> recvBuf;
wpi::SmallVector<uint32_t, 128> 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<const jbyte*>(recvBuf.data()));
env->SetIntArrayRegion(buffer, 0, numToRead,
reinterpret_cast<const jint*>(recvBuf.data()));
}
return retval;
}

12
hal/src/main/native/include/hal/SPI.h
View File

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

2
hal/src/main/native/include/mockdata/SPIData.h
View File

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

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

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

2
hal/src/main/native/sim/jni/SimulatorJNI.cpp
View File

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

11
hal/src/main/native/sim/jni/SpiReadAutoReceiveBufferCallbackStore.cpp
View File

@@ -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<const char*>(buffer),
static_cast<size_t>(numToRead)});
auto toCallbackArr = MakeJIntArray(
env, wpi::ArrayRef<uint32_t>{buffer, static_cast<size_t>(numToRead)});
jint ret = env->CallIntMethod(m_call, sim::GetBufferCallback(),
MakeJString(env, name), toCallbackArr,
(jint)numToRead);
jbyte* fromCallbackArr = reinterpret_cast<jbyte*>(
jint* fromCallbackArr = reinterpret_cast<jint*>(
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<uintptr_t>(param);
SIM_JniHandle handle = static_cast<SIM_JniHandle>(handleTmp);

2
hal/src/main/native/sim/jni/SpiReadAutoReceiveBufferCallbackStore.h
View File

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

2
hal/src/main/native/sim/mockdata/SPIData.cpp
View File

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

2
hal/src/main/native/sim/mockdata/SPIDataInternal.h
View File

@@ -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<HAL_Bool, MakeBoolean, GetInitializedName> initialized{false};

47
simulation/halsim_adx_gyro_accelerometer/src/main/native/cpp/ADXRS450_SpiGyroWrapperData.cpp
View File

@@ -11,6 +11,7 @@
#include <cmath>
#include <cstring>
#include <hal/HALBase.h>
#include <mockdata/SPIData.h>
#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 <class T>
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<ADXRS450_SpiGyroWrapper*>(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<wpi::recursive_spinlock> 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<int32_t>(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<uint32_t>(valueToSend) & 0xffu;
valueToSend >>= 8;
}
m_angleDiff -= cappedDiff;
msgCtr += 1;
timestamp += kSamplePeriod * 1e6; // fpga time is in us
}
}

2
simulation/halsim_adx_gyro_accelerometer/src/main/native/include/ADXRS450_SpiGyroWrapperData.h
View File

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

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

@@ -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<int>(m_spi.GetAccumulatorAverage()));
m_spi.SetAccumulatorIntegratedCenter(m_spi.GetAccumulatorIntegratedAverage());
m_spi.ResetAccumulator();
}

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

@@ -31,13 +31,13 @@ class SPI::Accumulator {
std::lock_guard<wpi::mutex> 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<int32_t>(timestamp - m_lastTimestamp) * 1e-6 -
m_integratedCenter;
else
m_integratedValue +=
dataNoCenter *
static_cast<int32_t>((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<wpi::mutex> lock(m_accum->m_mutex);
m_accum->m_integratedCenter = center;
}
double SPI::GetAccumulatorIntegratedValue() const {
if (!m_accum) return 0;
std::lock_guard<wpi::mutex> 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<wpi::mutex> 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);
}

43
wpilibc/src/main/native/include/frc/SPI.h
View File

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

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

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

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

@@ -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 {
* <p>Transfers may be made a byte at a time, so it's necessary for the caller
* to handle cases where an entire transfer has not been completed.
*
* <p>Blocks until numToRead bytes have been read or timeout expires.
* May be called with numToRead=0 to retrieve how many bytes are available.
* <p>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
* <p>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 {
* <p>Transfers may be made a byte at a time, so it's necessary for the caller
* to handle cases where an entire transfer has not been completed.
*
* <p>Blocks until numToRead bytes have been read or timeout expires.
* May be called with numToRead=0 to retrieve how many bytes are available.
* <p>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
* <p>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.
*
* <p>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);
}
}
}