diff --git a/hal/src/main/native/include/MockData/SPIData.h b/hal/src/main/native/include/MockData/SPIData.h index 5cbf355a52..df72206b61 100644 --- a/hal/src/main/native/include/MockData/SPIData.h +++ b/hal/src/main/native/include/MockData/SPIData.h @@ -12,6 +12,12 @@ #include "HAL/HAL.h" #include "NotifyListener.h" +typedef void (*HAL_SpiReadAutoReceiveBufferCallback)(const char* name, + void* param, + unsigned char* buffer, + int32_t numToRead, + int32_t* outputCount); + #ifdef __cplusplus extern "C" { #endif @@ -36,18 +42,9 @@ int32_t HALSIM_RegisterSPIWriteCallback(int32_t index, void* param); void HALSIM_CancelSPIWriteCallback(int32_t index, int32_t uid); -int32_t HALSIM_RegisterSPIResetAccumulatorCallback(int32_t index, - HAL_NotifyCallback callback, - void* param, - HAL_Bool initialNotify); -void HALSIM_CancelSPIResetAccumulatorCallback(int32_t index, int32_t uid); - -int32_t HALSIM_RegisterSPISetAccumulatorCallback(int32_t index, - HAL_BufferCallback callback, - void* param); -void HALSIM_CancelSPISetAccumulatorCallback(int32_t index, int32_t uid); -void HALSIM_SetSPISetAccumulatorValue(int32_t index, int64_t value); -int64_t HALSIM_GetSPIGetAccumulatorValue(int32_t index); +int32_t HALSIM_RegisterSPIReadAutoReceivedDataCallback( + int32_t index, HAL_SpiReadAutoReceiveBufferCallback callback, void* param); +void HALSIM_CancelSPIReadAutoReceivedDataCallback(int32_t index, int32_t uid); #ifdef __cplusplus } // extern "C" diff --git a/hal/src/main/native/sim/HAL.cpp b/hal/src/main/native/sim/HAL.cpp index b8c2bf27cd..b9a51e049f 100644 --- a/hal/src/main/native/sim/HAL.cpp +++ b/hal/src/main/native/sim/HAL.cpp @@ -55,6 +55,7 @@ void InitializeHAL() { InitializeDigitalInternal(); InitializeDIO(); InitializeDriverStation(); + InitializeEncoder(); InitializeExtensions(); InitializeI2C(); InitializeInterrupts(); diff --git a/hal/src/main/native/sim/HALInitializer.h b/hal/src/main/native/sim/HALInitializer.h index 77fb78dd6f..8cd0db4407 100644 --- a/hal/src/main/native/sim/HALInitializer.h +++ b/hal/src/main/native/sim/HALInitializer.h @@ -41,6 +41,7 @@ extern void InitializeCounter(); extern void InitializeDigitalInternal(); extern void InitializeDIO(); extern void InitializeDriverStation(); +extern void InitializeEncoder(); extern void InitializeExtensions(); extern void InitializeHAL(); extern void InitializeI2C(); diff --git a/hal/src/main/native/sim/MockData/SPIData.cpp b/hal/src/main/native/sim/MockData/SPIData.cpp index fed0bd286c..94c39d3a5e 100644 --- a/hal/src/main/native/sim/MockData/SPIData.cpp +++ b/hal/src/main/native/sim/MockData/SPIData.cpp @@ -13,6 +13,21 @@ using namespace hal; +void InvokeCallback( + std::shared_ptr currentVector, + const char* name, uint8_t* buffer, int32_t numToRead, + int32_t* outputCount) { + // Return if no callbacks are assigned + if (currentVector == nullptr) return; + // Get a copy of the shared_ptr, then iterate and callback listeners + auto newCallbacks = currentVector; + for (size_t i = 0; i < newCallbacks->size(); ++i) { + if (!(*newCallbacks)[i]) continue; // callback was removed + auto listener = (*newCallbacks)[i]; + listener.callback(name, listener.param, buffer, numToRead, outputCount); + } +} + namespace hal { namespace init { void InitializeSPIData() { @@ -25,12 +40,10 @@ void InitializeSPIData() { SPIData* hal::SimSPIData; void SPIData::ResetData() { m_initialized = false; - m_accumulatorValue = 0; m_initializedCallbacks = nullptr; m_readCallbacks = nullptr; m_writeCallbacks = nullptr; - m_resetAccumulatorCallback = nullptr; - m_setAccumulatorCallback = nullptr; + m_autoReceiveDataCallbacks = nullptr; } SPIData::SPIData() {} @@ -108,66 +121,26 @@ void SPIData::CancelWriteCallback(int32_t uid) { m_writeCallbacks = CancelCallback(m_writeCallbacks, uid); } -int32_t SPIData::RegisterResetAccumulatorCallback(HAL_NotifyCallback callback, - void* param, - HAL_Bool initialNotify) { +int32_t SPIData::RegisterReadAutoReceivedDataCallback( + HAL_SpiReadAutoReceiveBufferCallback callback, void* param) { // Must return -1 on a null callback for error handling if (callback == nullptr) return -1; int32_t newUid = 0; { std::lock_guard lock(m_registerMutex); - m_resetAccumulatorCallback = - RegisterCallback(m_resetAccumulatorCallback, "ResetAccumulator", - callback, param, &newUid); - } - if (initialNotify) { - // We know that the callback is not null because of earlier null check - HAL_Value value = MakeBoolean(GetInitialized()); - callback("ResetAccumulator", param, &value); + m_autoReceiveDataCallbacks = RegisterCallbackImpl( + m_autoReceiveDataCallbacks, "AutoReceive", callback, param, &newUid); } return newUid; } -void SPIData::CancelResetAccumulatorCallback(int32_t uid) { - m_resetAccumulatorCallback = CancelCallback(m_resetAccumulatorCallback, uid); +void SPIData::CancelReadAutoReceivedDataCallback(int32_t uid) { + m_autoReceiveDataCallbacks = + CancelCallbackImpl( + m_autoReceiveDataCallbacks, uid); } -int32_t SPIData::RegisterAccumulatorCallback(HAL_NotifyCallback callback, - void* param, - HAL_Bool initialNotify) { - // Must return -1 on a null callback for error handling - if (callback == nullptr) return -1; - int32_t newUid = 0; - { - std::lock_guard lock(m_registerMutex); - m_setAccumulatorCallback = RegisterCallback( - m_setAccumulatorCallback, "SetAccumulator", callback, param, &newUid); - } - if (initialNotify) { - // We know that the callback is not null because of earlier null check - HAL_Value value = MakeInt(GetAccumulatorValue()); - callback("SetAccumulator", param, &value); - } - return newUid; -} - -void SPIData::CancelAccumulatorCallback(int32_t uid) { - m_setAccumulatorCallback = CancelCallback(m_setAccumulatorCallback, uid); -} - -void SPIData::InvokeSetAccumulatorCallback(HAL_Value value) { - InvokeCallback(m_setAccumulatorCallback, "SetAccumulator", &value); -} - -void SPIData::SetAccumulatorValue(int64_t value) { - int64_t oldValue = m_accumulatorValue.exchange(value); - if (oldValue != value) { - InvokeSetAccumulatorCallback(MakeLong(value)); - } -} - -int64_t SPIData::GetAccumulatorValue() { return m_accumulatorValue; } - int32_t SPIData::Read(uint8_t* buffer, int32_t count) { std::lock_guard lock(m_dataMutex); InvokeCallback(m_readCallbacks, "Read", buffer, count); @@ -192,9 +165,13 @@ int32_t SPIData::Transaction(const uint8_t* dataToSend, uint8_t* dataReceived, return size; } -void SPIData::ResetAccumulator() { - HAL_Value value = MakeInt(0); - InvokeCallback(m_resetAccumulatorCallback, "ResetAccumulator", &value); +int32_t SPIData::ReadAutoReceivedData(uint8_t* buffer, int32_t numToRead, + double timeout, int32_t* status) { + int32_t outputCount = 0; + InvokeCallback(m_autoReceiveDataCallbacks, "AutoReceive", + const_cast(buffer), numToRead, &outputCount); + + return outputCount; } extern "C" { @@ -238,22 +215,14 @@ void HALSIM_CancelSPIWriteCallback(int32_t index, int32_t uid) { SimSPIData[index].CancelWriteCallback(uid); } -int32_t HALSIM_RegisterSPIResetAccumulatorCallback(int32_t index, - HAL_NotifyCallback callback, - void* param, - HAL_Bool initialNotify) { - return SimSPIData[index].RegisterResetAccumulatorCallback(callback, param, - initialNotify); -} -void HALSIM_CancelSPIResetAccumulatorCallback(int32_t index, int32_t uid) { - SimSPIData[index].CancelResetAccumulatorCallback(uid); +int32_t HALSIM_RegisterSPIReadAutoReceivedDataCallback( + int32_t index, HAL_SpiReadAutoReceiveBufferCallback callback, void* param) { + return SimSPIData[index].RegisterReadAutoReceivedDataCallback(callback, + param); } -void HALSIM_SetSPISetAccumulatorValue(int32_t index, int64_t value) { - SimSPIData[index].SetAccumulatorValue(value); -} -int64_t HALSIM_GetSPIGetAccumulatorValue(int32_t index) { - return SimSPIData[index].GetAccumulatorValue(); +void HALSIM_CancelSPIReadAutoReceivedDataCallback(int32_t index, int32_t uid) { + SimSPIData[index].CancelReadAutoReceivedDataCallback(uid); } } // extern "C" diff --git a/hal/src/main/native/sim/MockData/SPIDataInternal.h b/hal/src/main/native/sim/MockData/SPIDataInternal.h index 673baf8919..37aa1a73d5 100644 --- a/hal/src/main/native/sim/MockData/SPIDataInternal.h +++ b/hal/src/main/native/sim/MockData/SPIDataInternal.h @@ -17,6 +17,10 @@ #include "MockData/SPIData.h" namespace hal { + +typedef HalCallbackListenerVectorImpl + SpiAutoReceiveDataListenerVector; + class SPIData { public: SPIData(); @@ -35,22 +39,17 @@ class SPIData { int32_t RegisterWriteCallback(HAL_ConstBufferCallback callback, void* param); void CancelWriteCallback(int32_t uid); - int32_t RegisterResetAccumulatorCallback(HAL_NotifyCallback callback, - void* param, HAL_Bool initialNotify); - void CancelResetAccumulatorCallback(int32_t uid); - - int32_t RegisterAccumulatorCallback(HAL_NotifyCallback callback, void* param, - HAL_Bool initialNotify); - void CancelAccumulatorCallback(int32_t uid); - void InvokeSetAccumulatorCallback(HAL_Value value); - void SetAccumulatorValue(int64_t value); - int64_t GetAccumulatorValue(); + int32_t RegisterReadAutoReceivedDataCallback( + HAL_SpiReadAutoReceiveBufferCallback callback, void* param); + void CancelReadAutoReceivedDataCallback(int32_t uid); int32_t Read(uint8_t* buffer, int32_t count); int32_t Write(const uint8_t* dataToSend, int32_t sendSize); int32_t Transaction(const uint8_t* dataToSend, uint8_t* dataReceived, int32_t size); - void ResetAccumulator(); + + int32_t ReadAutoReceivedData(uint8_t* buffer, int32_t numToRead, + double timeout, int32_t* status); void ResetData(); @@ -58,12 +57,11 @@ class SPIData { wpi::mutex m_registerMutex; wpi::mutex m_dataMutex; std::atomic m_initialized{false}; - std::atomic m_accumulatorValue{false}; std::shared_ptr m_initializedCallbacks = nullptr; std::shared_ptr m_readCallbacks = nullptr; std::shared_ptr m_writeCallbacks = nullptr; - std::shared_ptr m_resetAccumulatorCallback = nullptr; - std::shared_ptr m_setAccumulatorCallback = nullptr; + std::shared_ptr m_autoReceiveDataCallbacks = + nullptr; }; extern SPIData* SimSPIData; } // namespace hal diff --git a/hal/src/main/native/sim/SPI.cpp b/hal/src/main/native/sim/SPI.cpp index 76b2b8076a..72d22eb790 100644 --- a/hal/src/main/native/sim/SPI.cpp +++ b/hal/src/main/native/sim/SPI.cpp @@ -55,7 +55,8 @@ void HAL_ForceSPIAutoRead(HAL_SPIPort port, int32_t* status) {} int32_t HAL_ReadSPIAutoReceivedData(HAL_SPIPort port, uint8_t* buffer, int32_t numToRead, double timeout, int32_t* status) { - return 0; + return SimSPIData[port].ReadAutoReceivedData(buffer, numToRead, timeout, + status); } int32_t HAL_GetSPIAutoDroppedCount(HAL_SPIPort port, int32_t* status) { return 0; diff --git a/simulation/adx_gyro_accelerometer/src/main/native/cpp/ADXL345_I2CAccelerometerData.cpp b/simulation/adx_gyro_accelerometer/src/main/native/cpp/ADXL345_I2CAccelerometerData.cpp index f7a543d436..50c44863c0 100644 --- a/simulation/adx_gyro_accelerometer/src/main/native/cpp/ADXL345_I2CAccelerometerData.cpp +++ b/simulation/adx_gyro_accelerometer/src/main/native/cpp/ADXL345_I2CAccelerometerData.cpp @@ -28,12 +28,17 @@ static void ADXL345I2C_WriteBufferCallback(const char* name, void* param, sim->HandleWrite(buffer, count); } -ADXL345_I2CData::ADXL345_I2CData(int port) { - HALSIM_RegisterI2CReadCallback(port, ADXL345I2C_ReadBufferCallback, this); - HALSIM_RegisterI2CWriteCallback(port, ADXL345I2C_WriteBufferCallback, this); +ADXL345_I2CData::ADXL345_I2CData(int port) : m_port(port) { + m_readCallbackId = + HALSIM_RegisterI2CReadCallback(port, ADXL345I2C_ReadBufferCallback, this); + m_writeCallbackId = HALSIM_RegisterI2CWriteCallback( + port, ADXL345I2C_WriteBufferCallback, this); } -ADXL345_I2CData::~ADXL345_I2CData() {} +ADXL345_I2CData::~ADXL345_I2CData() { + HALSIM_CancelI2CReadCallback(m_port, m_readCallbackId); + HALSIM_CancelI2CWriteCallback(m_port, m_writeCallbackId); +} void ADXL345_I2CData::ADXL345_I2CData::HandleWrite(const uint8_t* buffer, uint32_t count) { diff --git a/simulation/adx_gyro_accelerometer/src/main/native/cpp/ADXL345_SpiAccelerometerData.cpp b/simulation/adx_gyro_accelerometer/src/main/native/cpp/ADXL345_SpiAccelerometerData.cpp index f3ec9d6f5d..dfb4ca4db5 100644 --- a/simulation/adx_gyro_accelerometer/src/main/native/cpp/ADXL345_SpiAccelerometerData.cpp +++ b/simulation/adx_gyro_accelerometer/src/main/native/cpp/ADXL345_SpiAccelerometerData.cpp @@ -28,12 +28,17 @@ static void ADXL345SPI_WriteBufferCallback(const char* name, void* param, sim->HandleWrite(buffer, count); } -ADXL345_SpiAccelerometer::ADXL345_SpiAccelerometer(int port) { - HALSIM_RegisterSPIReadCallback(port, ADXL345SPI_ReadBufferCallback, this); - HALSIM_RegisterSPIWriteCallback(port, ADXL345SPI_WriteBufferCallback, this); +ADXL345_SpiAccelerometer::ADXL345_SpiAccelerometer(int port) : m_port(port) { + m_readCallbackId = + HALSIM_RegisterSPIReadCallback(port, ADXL345SPI_ReadBufferCallback, this); + m_writeCallbackId = HALSIM_RegisterSPIWriteCallback( + port, ADXL345SPI_WriteBufferCallback, this); } -ADXL345_SpiAccelerometer::~ADXL345_SpiAccelerometer() {} +ADXL345_SpiAccelerometer::~ADXL345_SpiAccelerometer() { + HALSIM_CancelSPIReadCallback(m_port, m_readCallbackId); + HALSIM_CancelSPIWriteCallback(m_port, m_writeCallbackId); +} void ADXL345_SpiAccelerometer::HandleWrite(const uint8_t* buffer, uint32_t count) { diff --git a/simulation/adx_gyro_accelerometer/src/main/native/cpp/ADXL362_SpiAccelerometerData.cpp b/simulation/adx_gyro_accelerometer/src/main/native/cpp/ADXL362_SpiAccelerometerData.cpp index 4981c5b9c6..107d5d6c8a 100644 --- a/simulation/adx_gyro_accelerometer/src/main/native/cpp/ADXL362_SpiAccelerometerData.cpp +++ b/simulation/adx_gyro_accelerometer/src/main/native/cpp/ADXL362_SpiAccelerometerData.cpp @@ -28,12 +28,17 @@ static void ADXL362SPI_WriteBufferCallback(const char* name, void* param, sim->HandleWrite(buffer, count); } -ADXL362_SpiAccelerometer::ADXL362_SpiAccelerometer(int port) { - HALSIM_RegisterSPIReadCallback(port, ADXL362SPI_ReadBufferCallback, this); - HALSIM_RegisterSPIWriteCallback(port, ADXL362SPI_WriteBufferCallback, this); +ADXL362_SpiAccelerometer::ADXL362_SpiAccelerometer(int port) : m_port(port) { + m_readCallbackId = + HALSIM_RegisterSPIReadCallback(port, ADXL362SPI_ReadBufferCallback, this); + m_writeCallbackId = HALSIM_RegisterSPIWriteCallback( + port, ADXL362SPI_WriteBufferCallback, this); } -ADXL362_SpiAccelerometer::~ADXL362_SpiAccelerometer() {} +ADXL362_SpiAccelerometer::~ADXL362_SpiAccelerometer() { + HALSIM_CancelSPIReadCallback(m_port, m_readCallbackId); + HALSIM_CancelSPIWriteCallback(m_port, m_writeCallbackId); +} void ADXL362_SpiAccelerometer::HandleWrite(const uint8_t* buffer, uint32_t count) { diff --git a/simulation/adx_gyro_accelerometer/src/main/native/cpp/ADXRS450_SpiGyroWrapperData.cpp b/simulation/adx_gyro_accelerometer/src/main/native/cpp/ADXRS450_SpiGyroWrapperData.cpp index 45ccb5235f..7a5024cfdd 100644 --- a/simulation/adx_gyro_accelerometer/src/main/native/cpp/ADXRS450_SpiGyroWrapperData.cpp +++ b/simulation/adx_gyro_accelerometer/src/main/native/cpp/ADXRS450_SpiGyroWrapperData.cpp @@ -7,29 +7,61 @@ #include "ADXRS450_SpiGyroWrapperData.h" +#include +#include #include #include "MockData/NotifyCallbackHelpers.h" #include "MockData/SPIData.h" +#ifdef _WIN32 +#include "Winsock2.h" +#pragma comment(lib, "ws2_32.lib") +#else +#include +#endif + using namespace hal; -const double ADXRS450_SpiGyroWrapper::ANGLE_LSB = 1 / 0.0125 / 0.001; +const double ADXRS450_SpiGyroWrapper::kAngleLsb = 1 / 0.0125 / 0.0005; +const double ADXRS450_SpiGyroWrapper::kMaxAngleDeltaPerMessage = 0.1875; +const int ADXRS450_SpiGyroWrapper::kPacketSize = 4; + +template +constexpr const T& clamp(const T& value, const T& low, const T& high) { + return std::max(low, std::min(value, high)); +} static void ADXRS450SPI_ReadBufferCallback(const char* name, void* param, uint8_t* buffer, uint32_t count) { - ADXRS450_SpiGyroWrapper* sim = static_cast(param); + auto sim = static_cast(param); sim->HandleRead(buffer, count); } -ADXRS450_SpiGyroWrapper::ADXRS450_SpiGyroWrapper(int port) : m_port(port) { - HALSIM_RegisterSPIReadCallback(port, ADXRS450SPI_ReadBufferCallback, this); +static void ADXRS450SPI_ReadAutoReceivedData(const char* name, void* param, + uint8_t* buffer, int32_t numToRead, + int32_t* outputCount) { + auto sim = static_cast(param); + sim->HandleAutoReceiveData(buffer, numToRead, *outputCount); } -ADXRS450_SpiGyroWrapper::~ADXRS450_SpiGyroWrapper() {} +ADXRS450_SpiGyroWrapper::ADXRS450_SpiGyroWrapper(int port) : m_port(port) { + m_readCallbackId = HALSIM_RegisterSPIReadCallback( + port, ADXRS450SPI_ReadBufferCallback, this); + m_autoReceiveReadCallbackId = HALSIM_RegisterSPIReadAutoReceivedDataCallback( + port, ADXRS450SPI_ReadAutoReceivedData, this); +} + +ADXRS450_SpiGyroWrapper::~ADXRS450_SpiGyroWrapper() { + HALSIM_CancelSPIReadCallback(m_port, m_readCallbackId); + HALSIM_CancelSPIReadAutoReceivedDataCallback(m_port, + m_autoReceiveReadCallbackId); +} void ADXRS450_SpiGyroWrapper::ResetData() { + std::lock_guard lock(m_dataMutex); m_angle = 0; + m_angleDiff = 0; m_angleCallbacks = nullptr; } @@ -38,6 +70,43 @@ void ADXRS450_SpiGyroWrapper::HandleRead(uint8_t* buffer, uint32_t count) { std::memcpy(&buffer[0], &returnCode, sizeof(returnCode)); } +void ADXRS450_SpiGyroWrapper::HandleAutoReceiveData(uint8_t* buffer, + int32_t numToRead, + int32_t& outputCount) { + std::lock_guard lock(m_dataMutex); + int32_t messagesToSend = 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 + if (numToRead == 0) { + outputCount = messagesToSend * kPacketSize; + return; + } + + int valuesToRead = numToRead / kPacketSize; + std::memset(&buffer[0], 0, numToRead); + + int msgCtr = 0; + + while (msgCtr < valuesToRead) { + double cappedDiff = + clamp(m_angleDiff, -kMaxAngleDeltaPerMessage, kMaxAngleDeltaPerMessage); + + int32_t valueToSend = + ((static_cast(cappedDiff * kAngleLsb) << 10) & (~0x0C00000E)) | + 0x04000000; + valueToSend = ntohl(valueToSend); + + std::memcpy(&buffer[msgCtr * kPacketSize], &valueToSend, + sizeof(valueToSend)); + + m_angleDiff -= cappedDiff; + msgCtr += 1; + } +} + int32_t ADXRS450_SpiGyroWrapper::RegisterAngleCallback( HAL_NotifyCallback callback, void* param, HAL_Bool initialNotify) { // Must return -1 on a null callback for error handling @@ -55,19 +124,26 @@ int32_t ADXRS450_SpiGyroWrapper::RegisterAngleCallback( } return newUid; } + void ADXRS450_SpiGyroWrapper::CancelAngleCallback(int32_t uid) { m_angleCallbacks = CancelCallback(m_angleCallbacks, uid); } + void ADXRS450_SpiGyroWrapper::InvokeAngleCallback(HAL_Value value) { InvokeCallback(m_angleCallbacks, "Angle", &value); } -double ADXRS450_SpiGyroWrapper::GetAngle() { return m_angle; } + +double ADXRS450_SpiGyroWrapper::GetAngle() { + std::lock_guard lock(m_dataMutex); + return m_angle; +} + void ADXRS450_SpiGyroWrapper::SetAngle(double angle) { - int32_t oldValue = m_angle.exchange(angle); - if (oldValue != angle) { + std::lock_guard lock(m_dataMutex); + if (m_angle != angle) { InvokeAngleCallback(MakeDouble(angle)); - int64_t accumValue = angle * ANGLE_LSB; - HALSIM_SetSPISetAccumulatorValue(m_port, accumValue); + m_angleDiff += angle - m_angle; + m_angle = angle; } } diff --git a/simulation/adx_gyro_accelerometer/src/main/native/include/ADXL345_I2CAccelerometerData.h b/simulation/adx_gyro_accelerometer/src/main/native/include/ADXL345_I2CAccelerometerData.h index dc7c05524e..91a316c542 100644 --- a/simulation/adx_gyro_accelerometer/src/main/native/include/ADXL345_I2CAccelerometerData.h +++ b/simulation/adx_gyro_accelerometer/src/main/native/include/ADXL345_I2CAccelerometerData.h @@ -19,6 +19,10 @@ class ADXL345_I2CData : public ThreeAxisAccelerometerData { void HandleRead(uint8_t* buffer, uint32_t count); private: + int m_port; + int m_writeCallbackId; + int m_readCallbackId; + int m_lastWriteAddress; static const double LSB; diff --git a/simulation/adx_gyro_accelerometer/src/main/native/include/ADXL345_SpiAccelerometerData.h b/simulation/adx_gyro_accelerometer/src/main/native/include/ADXL345_SpiAccelerometerData.h index 8073f7b21f..efd88c4d7c 100644 --- a/simulation/adx_gyro_accelerometer/src/main/native/include/ADXL345_SpiAccelerometerData.h +++ b/simulation/adx_gyro_accelerometer/src/main/native/include/ADXL345_SpiAccelerometerData.h @@ -19,6 +19,10 @@ class ADXL345_SpiAccelerometer : public ThreeAxisAccelerometerData { void HandleRead(uint8_t* buffer, uint32_t count); private: + int m_port; + int m_writeCallbackId; + int m_readCallbackId; + int m_lastWriteAddress; static const double LSB; diff --git a/simulation/adx_gyro_accelerometer/src/main/native/include/ADXL362_SpiAccelerometerData.h b/simulation/adx_gyro_accelerometer/src/main/native/include/ADXL362_SpiAccelerometerData.h index 0d1ba6d52f..39f7b53c58 100644 --- a/simulation/adx_gyro_accelerometer/src/main/native/include/ADXL362_SpiAccelerometerData.h +++ b/simulation/adx_gyro_accelerometer/src/main/native/include/ADXL362_SpiAccelerometerData.h @@ -19,6 +19,10 @@ class ADXL362_SpiAccelerometer : public ThreeAxisAccelerometerData { void HandleRead(uint8_t* buffer, uint32_t count); private: + int m_port; + int m_writeCallbackId; + int m_readCallbackId; + int m_lastWriteAddress; static const double LSB; diff --git a/simulation/adx_gyro_accelerometer/src/main/native/include/ADXRS450_SpiGyroWrapperData.h b/simulation/adx_gyro_accelerometer/src/main/native/include/ADXRS450_SpiGyroWrapperData.h index 5cc7733c25..0c2e514405 100644 --- a/simulation/adx_gyro_accelerometer/src/main/native/include/ADXRS450_SpiGyroWrapperData.h +++ b/simulation/adx_gyro_accelerometer/src/main/native/include/ADXRS450_SpiGyroWrapperData.h @@ -21,6 +21,8 @@ class ADXRS450_SpiGyroWrapper { virtual ~ADXRS450_SpiGyroWrapper(); void HandleRead(uint8_t* buffer, uint32_t count); + void HandleAutoReceiveData(uint8_t* buffer, int32_t numToRead, + int32_t& outputCount); virtual void ResetData(); @@ -32,12 +34,20 @@ class ADXRS450_SpiGyroWrapper { void SetAngle(double angle); private: - const int m_port; + int m_port; + int m_readCallbackId; + int m_autoReceiveReadCallbackId; wpi::mutex m_registerMutex; - std::atomic m_angle{0.0}; + wpi::mutex m_dataMutex; + double m_angle = 0.0; + double m_angleDiff = 0.0; std::shared_ptr m_angleCallbacks = nullptr; - static const double ANGLE_LSB; + static const double kAngleLsb; + // The maximum difference that can fit inside of the shifted and masked data + // field, per transaction + static const double kMaxAngleDeltaPerMessage; + static const int kPacketSize; }; } // namespace hal