Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-29 02:21:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

Fix bugs in simulation libraries (#853)

Browse Source 
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
Download Patch File Download Diff File Expand all files Collapse all files

13
simulation/adx_gyro_accelerometer/src/main/native/cpp/ADXL345_I2CAccelerometerData.cpp
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) {

13
simulation/adx_gyro_accelerometer/src/main/native/cpp/ADXL345_SpiAccelerometerData.cpp
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) {

13
simulation/adx_gyro_accelerometer/src/main/native/cpp/ADXL362_SpiAccelerometerData.cpp
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) {

96
simulation/adx_gyro_accelerometer/src/main/native/cpp/ADXRS450_SpiGyroWrapperData.cpp
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;
}
}

4
simulation/adx_gyro_accelerometer/src/main/native/include/ADXL345_I2CAccelerometerData.h
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;

4
simulation/adx_gyro_accelerometer/src/main/native/include/ADXL345_SpiAccelerometerData.h
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;

4
simulation/adx_gyro_accelerometer/src/main/native/include/ADXL362_SpiAccelerometerData.h
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;

16
simulation/adx_gyro_accelerometer/src/main/native/include/ADXRS450_SpiGyroWrapperData.h
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