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:
PJ Reiniger
2017-12-31 15:37:14 -05:00
committed by Peter Johnson
parent 2c4faee667
commit 55b6764d50
14 changed files with 201 additions and 121 deletions

View File

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

View File

@@ -55,6 +55,7 @@ void InitializeHAL() {
InitializeDigitalInternal();
InitializeDIO();
InitializeDriverStation();
InitializeEncoder();
InitializeExtensions();
InitializeI2C();
InitializeInterrupts();

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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