Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-01 02:41:48 +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

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,