mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-07-01 02:41:48 +00:00
Fix bugs in simulation libraries (#853)
Some thing got broken in the past couple of weeks with updates to the HAL.
This commit is contained in:
committed by
Peter Johnson
parent
2c4faee667
commit
55b6764d50
@@ -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"
|
||||
|
||||
@@ -55,6 +55,7 @@ void InitializeHAL() {
|
||||
InitializeDigitalInternal();
|
||||
InitializeDIO();
|
||||
InitializeDriverStation();
|
||||
InitializeEncoder();
|
||||
InitializeExtensions();
|
||||
InitializeI2C();
|
||||
InitializeInterrupts();
|
||||
|
||||
@@ -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();
|
||||
|
||||
@@ -13,6 +13,21 @@
|
||||
|
||||
using namespace hal;
|
||||
|
||||
void InvokeCallback(
|
||||
std::shared_ptr<SpiAutoReceiveDataListenerVector> 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<wpi::mutex> 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<SpiAutoReceiveDataListenerVector,
|
||||
HAL_SpiReadAutoReceiveBufferCallback>(
|
||||
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<wpi::mutex> 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<wpi::mutex> 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<uint8_t*>(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"
|
||||
|
||||
@@ -17,6 +17,10 @@
|
||||
#include "MockData/SPIData.h"
|
||||
|
||||
namespace hal {
|
||||
|
||||
typedef HalCallbackListenerVectorImpl<HAL_SpiReadAutoReceiveBufferCallback>
|
||||
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<HAL_Bool> m_initialized{false};
|
||||
std::atomic<int64_t> m_accumulatorValue{false};
|
||||
std::shared_ptr<NotifyListenerVector> m_initializedCallbacks = nullptr;
|
||||
std::shared_ptr<BufferListenerVector> m_readCallbacks = nullptr;
|
||||
std::shared_ptr<ConstBufferListenerVector> m_writeCallbacks = nullptr;
|
||||
std::shared_ptr<NotifyListenerVector> m_resetAccumulatorCallback = nullptr;
|
||||
std::shared_ptr<NotifyListenerVector> m_setAccumulatorCallback = nullptr;
|
||||
std::shared_ptr<SpiAutoReceiveDataListenerVector> m_autoReceiveDataCallbacks =
|
||||
nullptr;
|
||||
};
|
||||
extern SPIData* SimSPIData;
|
||||
} // namespace hal
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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) {
|
||||
|
||||
@@ -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) {
|
||||
|
||||
@@ -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) {
|
||||
|
||||
@@ -7,29 +7,61 @@
|
||||
|
||||
#include "ADXRS450_SpiGyroWrapperData.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <cstring>
|
||||
|
||||
#include "MockData/NotifyCallbackHelpers.h"
|
||||
#include "MockData/SPIData.h"
|
||||
|
||||
#ifdef _WIN32
|
||||
#include "Winsock2.h"
|
||||
#pragma comment(lib, "ws2_32.lib")
|
||||
#else
|
||||
#include <arpa/inet.h>
|
||||
#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 <class T>
|
||||
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<ADXRS450_SpiGyroWrapper*>(param);
|
||||
auto sim = static_cast<ADXRS450_SpiGyroWrapper*>(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<ADXRS450_SpiGyroWrapper*>(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<wpi::mutex> 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<wpi::mutex> 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<int32_t>(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<wpi::mutex> 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<wpi::mutex> 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;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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<double> m_angle{0.0};
|
||||
wpi::mutex m_dataMutex;
|
||||
double m_angle = 0.0;
|
||||
double m_angleDiff = 0.0;
|
||||
std::shared_ptr<NotifyListenerVector> 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
|
||||
|
||||
Reference in New Issue
Block a user