diff --git a/hal/src/main/native/sim/MockData/NotifyCallbackHelpers.h b/hal/src/main/native/include/MockData/NotifyCallbackHelpers.h similarity index 100% rename from hal/src/main/native/sim/MockData/NotifyCallbackHelpers.h rename to hal/src/main/native/include/MockData/NotifyCallbackHelpers.h diff --git a/hal/src/main/native/sim/MockData/AccelerometerData.cpp b/hal/src/main/native/sim/MockData/AccelerometerData.cpp index ae12c2a6b2..b43961312b 100644 --- a/hal/src/main/native/sim/MockData/AccelerometerData.cpp +++ b/hal/src/main/native/sim/MockData/AccelerometerData.cpp @@ -7,7 +7,7 @@ #include "../PortsInternal.h" #include "AccelerometerDataInternal.h" -#include "NotifyCallbackHelpers.h" +#include "MockData/NotifyCallbackHelpers.h" using namespace hal; diff --git a/hal/src/main/native/sim/MockData/AnalogGyroData.cpp b/hal/src/main/native/sim/MockData/AnalogGyroData.cpp index b379265725..a0e7610a18 100644 --- a/hal/src/main/native/sim/MockData/AnalogGyroData.cpp +++ b/hal/src/main/native/sim/MockData/AnalogGyroData.cpp @@ -7,7 +7,7 @@ #include "../PortsInternal.h" #include "AnalogGyroDataInternal.h" -#include "NotifyCallbackHelpers.h" +#include "MockData/NotifyCallbackHelpers.h" using namespace hal; diff --git a/hal/src/main/native/sim/MockData/AnalogInData.cpp b/hal/src/main/native/sim/MockData/AnalogInData.cpp index 46f895ceea..62ff48953f 100644 --- a/hal/src/main/native/sim/MockData/AnalogInData.cpp +++ b/hal/src/main/native/sim/MockData/AnalogInData.cpp @@ -7,7 +7,7 @@ #include "../PortsInternal.h" #include "AnalogInDataInternal.h" -#include "NotifyCallbackHelpers.h" +#include "MockData/NotifyCallbackHelpers.h" using namespace hal; diff --git a/hal/src/main/native/sim/MockData/AnalogOutData.cpp b/hal/src/main/native/sim/MockData/AnalogOutData.cpp index 24c140c9ee..5488a60053 100644 --- a/hal/src/main/native/sim/MockData/AnalogOutData.cpp +++ b/hal/src/main/native/sim/MockData/AnalogOutData.cpp @@ -7,7 +7,7 @@ #include "../PortsInternal.h" #include "AnalogOutDataInternal.h" -#include "NotifyCallbackHelpers.h" +#include "MockData/NotifyCallbackHelpers.h" using namespace hal; diff --git a/hal/src/main/native/sim/MockData/AnalogTriggerData.cpp b/hal/src/main/native/sim/MockData/AnalogTriggerData.cpp index 53889fd449..98cdad2057 100644 --- a/hal/src/main/native/sim/MockData/AnalogTriggerData.cpp +++ b/hal/src/main/native/sim/MockData/AnalogTriggerData.cpp @@ -7,7 +7,7 @@ #include "../PortsInternal.h" #include "AnalogTriggerDataInternal.h" -#include "NotifyCallbackHelpers.h" +#include "MockData/NotifyCallbackHelpers.h" using namespace hal; diff --git a/hal/src/main/native/sim/MockData/DIOData.cpp b/hal/src/main/native/sim/MockData/DIOData.cpp index 80ca7f27da..6191ed7ae8 100644 --- a/hal/src/main/native/sim/MockData/DIOData.cpp +++ b/hal/src/main/native/sim/MockData/DIOData.cpp @@ -7,7 +7,7 @@ #include "../PortsInternal.h" #include "DIODataInternal.h" -#include "NotifyCallbackHelpers.h" +#include "MockData/NotifyCallbackHelpers.h" using namespace hal; diff --git a/hal/src/main/native/sim/MockData/DigitalPWMData.cpp b/hal/src/main/native/sim/MockData/DigitalPWMData.cpp index ba1c98a531..3cf175a859 100644 --- a/hal/src/main/native/sim/MockData/DigitalPWMData.cpp +++ b/hal/src/main/native/sim/MockData/DigitalPWMData.cpp @@ -7,7 +7,7 @@ #include "../PortsInternal.h" #include "DigitalPWMDataInternal.h" -#include "NotifyCallbackHelpers.h" +#include "MockData/NotifyCallbackHelpers.h" using namespace hal; diff --git a/hal/src/main/native/sim/MockData/DriverStationData.cpp b/hal/src/main/native/sim/MockData/DriverStationData.cpp index d3e5f0f533..ffb4ebfe20 100644 --- a/hal/src/main/native/sim/MockData/DriverStationData.cpp +++ b/hal/src/main/native/sim/MockData/DriverStationData.cpp @@ -11,7 +11,7 @@ #include "DriverStationDataInternal.h" #include "HAL/cpp/make_unique.h" -#include "NotifyCallbackHelpers.h" +#include "MockData/NotifyCallbackHelpers.h" namespace hal { struct JoystickOutputStore { diff --git a/hal/src/main/native/sim/MockData/EncoderData.cpp b/hal/src/main/native/sim/MockData/EncoderData.cpp index 383e976bbd..fdd1b7c55b 100644 --- a/hal/src/main/native/sim/MockData/EncoderData.cpp +++ b/hal/src/main/native/sim/MockData/EncoderData.cpp @@ -7,7 +7,7 @@ #include "../PortsInternal.h" #include "EncoderDataInternal.h" -#include "NotifyCallbackHelpers.h" +#include "MockData/NotifyCallbackHelpers.h" using namespace hal; diff --git a/hal/src/main/native/sim/MockData/I2CData.cpp b/hal/src/main/native/sim/MockData/I2CData.cpp index dc21866aef..44144f5afd 100644 --- a/hal/src/main/native/sim/MockData/I2CData.cpp +++ b/hal/src/main/native/sim/MockData/I2CData.cpp @@ -9,7 +9,7 @@ #include "../PortsInternal.h" #include "I2CDataInternal.h" -#include "NotifyCallbackHelpers.h" +#include "MockData/NotifyCallbackHelpers.h" using namespace hal; diff --git a/hal/src/main/native/sim/MockData/NotifyCallbackHelpers.cpp b/hal/src/main/native/sim/MockData/NotifyCallbackHelpers.cpp index 4edf646460..bd76d0f5ec 100644 --- a/hal/src/main/native/sim/MockData/NotifyCallbackHelpers.cpp +++ b/hal/src/main/native/sim/MockData/NotifyCallbackHelpers.cpp @@ -5,7 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -#include "NotifyCallbackHelpers.h" +#include "MockData/NotifyCallbackHelpers.h" using namespace hal; diff --git a/hal/src/main/native/sim/MockData/PCMData.cpp b/hal/src/main/native/sim/MockData/PCMData.cpp index da93555628..67d2fa773d 100644 --- a/hal/src/main/native/sim/MockData/PCMData.cpp +++ b/hal/src/main/native/sim/MockData/PCMData.cpp @@ -6,7 +6,7 @@ /*----------------------------------------------------------------------------*/ #include "../PortsInternal.h" -#include "NotifyCallbackHelpers.h" +#include "MockData/NotifyCallbackHelpers.h" #include "PCMDataInternal.h" using namespace hal; diff --git a/hal/src/main/native/sim/MockData/PDPData.cpp b/hal/src/main/native/sim/MockData/PDPData.cpp index 7e20eeb9f8..30f4be95a6 100644 --- a/hal/src/main/native/sim/MockData/PDPData.cpp +++ b/hal/src/main/native/sim/MockData/PDPData.cpp @@ -6,7 +6,7 @@ /*----------------------------------------------------------------------------*/ #include "../PortsInternal.h" -#include "NotifyCallbackHelpers.h" +#include "MockData/NotifyCallbackHelpers.h" #include "PDPDataInternal.h" using namespace hal; diff --git a/hal/src/main/native/sim/MockData/PWMData.cpp b/hal/src/main/native/sim/MockData/PWMData.cpp index c3e790fbda..c81c5f9441 100644 --- a/hal/src/main/native/sim/MockData/PWMData.cpp +++ b/hal/src/main/native/sim/MockData/PWMData.cpp @@ -6,7 +6,7 @@ /*----------------------------------------------------------------------------*/ #include "../PortsInternal.h" -#include "NotifyCallbackHelpers.h" +#include "MockData/NotifyCallbackHelpers.h" #include "PWMDataInternal.h" using namespace hal; diff --git a/hal/src/main/native/sim/MockData/RelayData.cpp b/hal/src/main/native/sim/MockData/RelayData.cpp index 0adc11a2a5..22b024c44f 100644 --- a/hal/src/main/native/sim/MockData/RelayData.cpp +++ b/hal/src/main/native/sim/MockData/RelayData.cpp @@ -6,7 +6,7 @@ /*----------------------------------------------------------------------------*/ #include "../PortsInternal.h" -#include "NotifyCallbackHelpers.h" +#include "MockData/NotifyCallbackHelpers.h" #include "RelayDataInternal.h" using namespace hal; diff --git a/hal/src/main/native/sim/MockData/RoboRioData.cpp b/hal/src/main/native/sim/MockData/RoboRioData.cpp index c50f48e7ec..088080b807 100644 --- a/hal/src/main/native/sim/MockData/RoboRioData.cpp +++ b/hal/src/main/native/sim/MockData/RoboRioData.cpp @@ -6,7 +6,7 @@ /*----------------------------------------------------------------------------*/ #include "../PortsInternal.h" -#include "NotifyCallbackHelpers.h" +#include "MockData/NotifyCallbackHelpers.h" #include "RoboRioDataInternal.h" using namespace hal; diff --git a/hal/src/main/native/sim/MockData/SPIAccelerometerData.cpp b/hal/src/main/native/sim/MockData/SPIAccelerometerData.cpp index b99979b68b..e509f27ca2 100644 --- a/hal/src/main/native/sim/MockData/SPIAccelerometerData.cpp +++ b/hal/src/main/native/sim/MockData/SPIAccelerometerData.cpp @@ -6,7 +6,7 @@ /*----------------------------------------------------------------------------*/ #include "../PortsInternal.h" -#include "NotifyCallbackHelpers.h" +#include "MockData/NotifyCallbackHelpers.h" #include "SPIAccelerometerDataInternal.h" using namespace hal; diff --git a/hal/src/main/native/sim/MockData/SPIData.cpp b/hal/src/main/native/sim/MockData/SPIData.cpp index 13150a548e..dc2a1c5166 100644 --- a/hal/src/main/native/sim/MockData/SPIData.cpp +++ b/hal/src/main/native/sim/MockData/SPIData.cpp @@ -8,7 +8,7 @@ #include #include "../PortsInternal.h" -#include "NotifyCallbackHelpers.h" +#include "MockData/NotifyCallbackHelpers.h" #include "SPIDataInternal.h" using namespace hal; @@ -178,6 +178,9 @@ int32_t SPIData::Write(const uint8_t* dataToSend, int32_t sendSize) { int32_t SPIData::Transaction(const uint8_t* dataToSend, uint8_t* dataReceived, int32_t size) { std::lock_guard lock(m_dataMutex); + InvokeCallback(m_writeCallbacks, "Write", dataToSend, size); + InvokeCallback(m_readCallbacks, "Read", dataReceived, size); + return size; } diff --git a/settings.gradle b/settings.gradle index da3ce6061c..a0345054e4 100644 --- a/settings.gradle +++ b/settings.gradle @@ -9,3 +9,4 @@ include 'wpilibjIntegrationTests' include 'wpilibjExamples' include 'myRobot' include 'simulation:halsim_print' +include 'simulation:adx_gyro_accelerometer' diff --git a/simulation/adx_gyro_accelerometer/build.gradle b/simulation/adx_gyro_accelerometer/build.gradle new file mode 100644 index 0000000000..26dbf21041 --- /dev/null +++ b/simulation/adx_gyro_accelerometer/build.gradle @@ -0,0 +1,60 @@ +description = "A simulation library for the ADXL345 I2C and SPI Accelerometer, ADXL362 SPI Accelerometer, and ADXRS450 SPI Gyro" + +apply plugin: 'edu.wpi.first.NativeUtils' +apply plugin: 'cpp' + + +if (!project.hasProperty('onlyAthena')) { + ext.skipAthena = true + + apply from: "../../config.gradle" + + + model { + dependencyConfigs { + wpiutil(DependencyConfig) { + groupId = 'edu.wpi.first.wpiutil' + artifactId = 'wpiutil-cpp' + headerClassifier = 'headers' + ext = 'zip' + version = '3.+' + sharedConfigs = [ halsim_adx_gyro_accelerometer: [] ] + } + } + exportsConfigs { + halsim_adx_gyro_accelerometer(ExportsConfig) { + x86ExcludeSymbols = [ '_CT??_R0?AV_System_error', '_CT??_R0?AVexception', '_CT??_R0?AVfailure', + '_CT??_R0?AVbad_cast', + '_CT??_R0?AVruntime_error', '_CT??_R0?AVsystem_error', '_CTA5?AVfailure', + '_TI5?AVfailure' ] + x64ExcludeSymbols = [ '_CT??_R0?AV_System_error', '_CT??_R0?AVexception', '_CT??_R0?AVfailure', + '_CT??_R0?AVbad_cast', + '_CT??_R0?AVruntime_error', '_CT??_R0?AVsystem_error', '_CTA5?AVfailure', + '_TI5?AVfailure' ] + } + } + components { + halsim_adx_gyro_accelerometer(NativeLibrarySpec) { + sources { + cpp { + source { + srcDirs = [ 'src/main/native/cpp' ] + includes = ["**/*.cpp"] + } + exportedHeaders { + srcDirs = ["src/main/native/include"] + } + } + } + } + } + + binaries { + all { + project(':hal').addHalCompilerArguments(it) + project(':hal').addHalToLinker(it) + } + } + } + apply from: 'publish.gradle' +} diff --git a/simulation/adx_gyro_accelerometer/publish.gradle b/simulation/adx_gyro_accelerometer/publish.gradle new file mode 100644 index 0000000000..f2acb36888 --- /dev/null +++ b/simulation/adx_gyro_accelerometer/publish.gradle @@ -0,0 +1,68 @@ +apply plugin: 'maven-publish' +apply plugin: 'edu.wpi.first.wpilib.versioning.WPILibVersioningPlugin' + +if (!hasProperty('releaseType')) { + WPILibVersion { + releaseType = 'dev' + } +} + +def pubVersion = '' +if (project.hasProperty("publishVersion")) { + pubVersion = project.publishVersion +} else { + pubVersion = WPILibVersion.version +} + +def baseArtifactId = 'halsim-adx_gyro_accelerometer' +def artifactGroupId = 'edu.wpi.first.halsim' + +def outputsFolder = file("$project.buildDir/outputs") + +task cppHeadersZip(type: Zip) { + destinationDir = outputsFolder + baseName = 'halsim-adx_gyro_accelerometer' + classifier = "headers" + + from(licenseFile) { + into '/' + } + + from('src/main/native/include') { + into '/' + } +} + +task cppSourcesZip(type: Zip) { + destinationDir = outputsFolder + baseName = 'halsim-adx_gyro_accelerometer' + classifier = "sources" + + from(licenseFile) { + into '/' + } + + from('src/main/native/cpp') { + into '/' + } +} + +model { + publishing { + def libSpec = createComponentZipTasks($.components, 'halsim_adx_gyro_accelerometer', 'zipcpp', Zip, project, includeStandardZipFormat) + + publications { + cpp(MavenPublication) { + artifactId = baseArtifactId + groupId artifactGroupId + version pubVersion + + libSpec.each { + artifact it + } + artifact cppHeadersZip + artifact cppSourcesZip + } + } + } +} 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 new file mode 100644 index 0000000000..f7a543d436 --- /dev/null +++ b/simulation/adx_gyro_accelerometer/src/main/native/cpp/ADXL345_I2CAccelerometerData.cpp @@ -0,0 +1,64 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "ADXL345_I2CAccelerometerData.h" + +#include + +#include "MockData/I2CData.h" + +using namespace hal; + +const double ADXL345_I2CData::LSB = 1 / 0.00390625; + +static void ADXL345I2C_ReadBufferCallback(const char* name, void* param, + uint8_t* buffer, uint32_t count) { + ADXL345_I2CData* sim = static_cast(param); + sim->HandleRead(buffer, count); +} + +static void ADXL345I2C_WriteBufferCallback(const char* name, void* param, + const uint8_t* buffer, + uint32_t count) { + ADXL345_I2CData* sim = static_cast(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() {} + +void ADXL345_I2CData::ADXL345_I2CData::HandleWrite(const uint8_t* buffer, + uint32_t count) { + m_lastWriteAddress = buffer[0]; +} + +void ADXL345_I2CData::HandleRead(uint8_t* buffer, uint32_t count) { + bool writeAll = count == 6; + int byteIndex = 0; + + if (writeAll || m_lastWriteAddress == 0x32) { + int16_t val = static_cast(GetX() * LSB); + std::memcpy(&buffer[byteIndex], &val, sizeof(val)); + byteIndex += sizeof(val); + } + + if (writeAll || m_lastWriteAddress == 0x34) { + int16_t val = static_cast(GetY() * LSB); + std::memcpy(&buffer[byteIndex], &val, sizeof(val)); + byteIndex += sizeof(val); + } + + if (writeAll || m_lastWriteAddress == 0x36) { + int16_t val = static_cast(GetZ() * LSB); + std::memcpy(&buffer[byteIndex], &val, sizeof(val)); + byteIndex += sizeof(val); + } +} 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 new file mode 100644 index 0000000000..f3ec9d6f5d --- /dev/null +++ b/simulation/adx_gyro_accelerometer/src/main/native/cpp/ADXL345_SpiAccelerometerData.cpp @@ -0,0 +1,64 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "ADXL345_SpiAccelerometerData.h" + +#include + +#include "MockData/SPIData.h" + +using namespace hal; + +const double ADXL345_SpiAccelerometer::LSB = 1 / 0.00390625; + +static void ADXL345SPI_ReadBufferCallback(const char* name, void* param, + uint8_t* buffer, uint32_t count) { + ADXL345_SpiAccelerometer* sim = static_cast(param); + sim->HandleRead(buffer, count); +} + +static void ADXL345SPI_WriteBufferCallback(const char* name, void* param, + const uint8_t* buffer, + uint32_t count) { + ADXL345_SpiAccelerometer* sim = static_cast(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() {} + +void ADXL345_SpiAccelerometer::HandleWrite(const uint8_t* buffer, + uint32_t count) { + m_lastWriteAddress = buffer[0] & 0xF; +} + +void ADXL345_SpiAccelerometer::HandleRead(uint8_t* buffer, uint32_t count) { + bool writeAll = count == 7; + int byteIndex = 1; + + if (writeAll || m_lastWriteAddress == 0x02) { + int16_t val = static_cast(GetX() * LSB); + std::memcpy(&buffer[byteIndex], &val, sizeof(val)); + byteIndex += sizeof(val); + } + + if (writeAll || m_lastWriteAddress == 0x04) { + int16_t val = static_cast(GetY() * LSB); + std::memcpy(&buffer[byteIndex], &val, sizeof(val)); + byteIndex += sizeof(val); + } + + if (writeAll || m_lastWriteAddress == 0x06) { + int16_t val = static_cast(GetZ() * LSB); + std::memcpy(&buffer[byteIndex], &val, sizeof(val)); + byteIndex += sizeof(val); + } +} 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 new file mode 100644 index 0000000000..4981c5b9c6 --- /dev/null +++ b/simulation/adx_gyro_accelerometer/src/main/native/cpp/ADXL362_SpiAccelerometerData.cpp @@ -0,0 +1,71 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "ADXL362_SpiAccelerometerData.h" + +#include + +#include "MockData/SPIData.h" + +using namespace hal; + +const double ADXL362_SpiAccelerometer::LSB = 1 / 0.001; + +static void ADXL362SPI_ReadBufferCallback(const char* name, void* param, + uint8_t* buffer, uint32_t count) { + ADXL362_SpiAccelerometer* sim = static_cast(param); + sim->HandleRead(buffer, count); +} + +static void ADXL362SPI_WriteBufferCallback(const char* name, void* param, + const uint8_t* buffer, + uint32_t count) { + ADXL362_SpiAccelerometer* sim = static_cast(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() {} + +void ADXL362_SpiAccelerometer::HandleWrite(const uint8_t* buffer, + uint32_t count) { + m_lastWriteAddress = buffer[1]; +} + +void ADXL362_SpiAccelerometer::HandleRead(uint8_t* buffer, uint32_t count) { + // Init check + if (m_lastWriteAddress == 0x02) { + uint32_t numToPut = 0xF20000; + std::memcpy(&buffer[0], &numToPut, sizeof(numToPut)); + } else { + // Get Accelerations + bool writeAll = count == 8; + int byteIndex = 2; + + if (writeAll || m_lastWriteAddress == 0x0E) { + int16_t val = static_cast(GetX() * LSB); + std::memcpy(&buffer[byteIndex], &val, sizeof(val)); + byteIndex += sizeof(val); + } + + if (writeAll || m_lastWriteAddress == 0x10) { + int16_t val = static_cast(GetY() * LSB); + std::memcpy(&buffer[byteIndex], &val, sizeof(val)); + byteIndex += sizeof(val); + } + + if (writeAll || m_lastWriteAddress == 0x12) { + int16_t val = static_cast(GetZ() * LSB); + std::memcpy(&buffer[byteIndex], &val, sizeof(val)); + byteIndex += sizeof(val); + } + } +} 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 new file mode 100644 index 0000000000..45ccb5235f --- /dev/null +++ b/simulation/adx_gyro_accelerometer/src/main/native/cpp/ADXRS450_SpiGyroWrapperData.cpp @@ -0,0 +1,73 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "ADXRS450_SpiGyroWrapperData.h" + +#include + +#include "MockData/NotifyCallbackHelpers.h" +#include "MockData/SPIData.h" + +using namespace hal; + +const double ADXRS450_SpiGyroWrapper::ANGLE_LSB = 1 / 0.0125 / 0.001; + +static void ADXRS450SPI_ReadBufferCallback(const char* name, void* param, + uint8_t* buffer, uint32_t count) { + ADXRS450_SpiGyroWrapper* sim = static_cast(param); + sim->HandleRead(buffer, count); +} + +ADXRS450_SpiGyroWrapper::ADXRS450_SpiGyroWrapper(int port) : m_port(port) { + HALSIM_RegisterSPIReadCallback(port, ADXRS450SPI_ReadBufferCallback, this); +} + +ADXRS450_SpiGyroWrapper::~ADXRS450_SpiGyroWrapper() {} + +void ADXRS450_SpiGyroWrapper::ResetData() { + m_angle = 0; + m_angleCallbacks = nullptr; +} + +void ADXRS450_SpiGyroWrapper::HandleRead(uint8_t* buffer, uint32_t count) { + int returnCode = 0x00400AE0; + std::memcpy(&buffer[0], &returnCode, sizeof(returnCode)); +} + +int32_t ADXRS450_SpiGyroWrapper::RegisterAngleCallback( + 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_angleCallbacks = + RegisterCallback(m_angleCallbacks, "Angle", callback, param, &newUid); + } + if (initialNotify) { + // We know that the callback is not null because of earlier null check + HAL_Value value = MakeDouble(GetAngle()); + callback("Angle", param, &value); + } + 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; } +void ADXRS450_SpiGyroWrapper::SetAngle(double angle) { + int32_t oldValue = m_angle.exchange(angle); + if (oldValue != angle) { + InvokeAngleCallback(MakeDouble(angle)); + + int64_t accumValue = angle * ANGLE_LSB; + HALSIM_SetSPISetAccumulatorValue(m_port, accumValue); + } +} diff --git a/simulation/adx_gyro_accelerometer/src/main/native/cpp/ThreeAxisAccelerometerData.cpp b/simulation/adx_gyro_accelerometer/src/main/native/cpp/ThreeAxisAccelerometerData.cpp new file mode 100644 index 0000000000..9dba074790 --- /dev/null +++ b/simulation/adx_gyro_accelerometer/src/main/native/cpp/ThreeAxisAccelerometerData.cpp @@ -0,0 +1,129 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "ThreeAxisAccelerometerData.h" + +#include "MockData/NotifyCallbackHelpers.h" + +using namespace hal; + +ThreeAxisAccelerometerData::ThreeAxisAccelerometerData() {} + +ThreeAxisAccelerometerData::~ThreeAxisAccelerometerData() {} +void ThreeAxisAccelerometerData::ResetData() { + m_x = 0.0; + m_xCallbacks = nullptr; + m_y = 0.0; + m_yCallbacks = nullptr; + m_z = 0.0; + m_zCallbacks = nullptr; +} + +int32_t ThreeAxisAccelerometerData::RegisterXCallback( + 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_xCallbacks = + RegisterCallback(m_xCallbacks, "X", callback, param, &newUid); + } + if (initialNotify) { + // We know that the callback is not null because of earlier null check + HAL_Value value = MakeDouble(GetX()); + callback("X", param, &value); + } + return newUid; +} + +void ThreeAxisAccelerometerData::CancelXCallback(int32_t uid) { + m_xCallbacks = CancelCallback(m_xCallbacks, uid); +} + +void ThreeAxisAccelerometerData::InvokeXCallback(HAL_Value value) { + InvokeCallback(m_xCallbacks, "X", &value); +} + +double ThreeAxisAccelerometerData::GetX() { return m_x; } + +void ThreeAxisAccelerometerData::SetX(double x) { + double oldValue = m_x.exchange(x); + if (oldValue != x) { + InvokeXCallback(MakeDouble(x)); + } +} + +int32_t ThreeAxisAccelerometerData::RegisterYCallback( + 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_yCallbacks = + RegisterCallback(m_yCallbacks, "Y", callback, param, &newUid); + } + if (initialNotify) { + // We know that the callback is not null because of earlier null check + HAL_Value value = MakeDouble(GetY()); + callback("Y", param, &value); + } + return newUid; +} + +void ThreeAxisAccelerometerData::CancelYCallback(int32_t uid) { + m_yCallbacks = CancelCallback(m_yCallbacks, uid); +} + +void ThreeAxisAccelerometerData::InvokeYCallback(HAL_Value value) { + InvokeCallback(m_yCallbacks, "Y", &value); +} + +double ThreeAxisAccelerometerData::GetY() { return m_y; } + +void ThreeAxisAccelerometerData::SetY(double y) { + double oldValue = m_y.exchange(y); + if (oldValue != y) { + InvokeYCallback(MakeDouble(y)); + } +} + +int32_t ThreeAxisAccelerometerData::RegisterZCallback( + 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_zCallbacks = + RegisterCallback(m_zCallbacks, "Z", callback, param, &newUid); + } + if (initialNotify) { + // We know that the callback is not null because of earlier null check + HAL_Value value = MakeDouble(GetZ()); + callback("Z", param, &value); + } + return newUid; +} + +void ThreeAxisAccelerometerData::CancelZCallback(int32_t uid) { + m_zCallbacks = CancelCallback(m_zCallbacks, uid); +} + +void ThreeAxisAccelerometerData::InvokeZCallback(HAL_Value value) { + InvokeCallback(m_zCallbacks, "Z", &value); +} + +double ThreeAxisAccelerometerData::GetZ() { return m_z; } + +void ThreeAxisAccelerometerData::SetZ(double z) { + double oldValue = m_z.exchange(z); + if (oldValue != z) { + InvokeZCallback(MakeDouble(z)); + } +} 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 new file mode 100644 index 0000000000..dc7c05524e --- /dev/null +++ b/simulation/adx_gyro_accelerometer/src/main/native/include/ADXL345_I2CAccelerometerData.h @@ -0,0 +1,26 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include "ThreeAxisAccelerometerData.h" + +namespace hal { +class ADXL345_I2CData : public ThreeAxisAccelerometerData { + public: + explicit ADXL345_I2CData(int port); + virtual ~ADXL345_I2CData(); + + void HandleWrite(const uint8_t* buffer, uint32_t count); + void HandleRead(uint8_t* buffer, uint32_t count); + + private: + int m_lastWriteAddress; + + static const double LSB; +}; +} // namespace hal 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 new file mode 100644 index 0000000000..8073f7b21f --- /dev/null +++ b/simulation/adx_gyro_accelerometer/src/main/native/include/ADXL345_SpiAccelerometerData.h @@ -0,0 +1,26 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include "ThreeAxisAccelerometerData.h" + +namespace hal { +class ADXL345_SpiAccelerometer : public ThreeAxisAccelerometerData { + public: + explicit ADXL345_SpiAccelerometer(int port); + virtual ~ADXL345_SpiAccelerometer(); + + void HandleWrite(const uint8_t* buffer, uint32_t count); + void HandleRead(uint8_t* buffer, uint32_t count); + + private: + int m_lastWriteAddress; + + static const double LSB; +}; +} // namespace hal 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 new file mode 100644 index 0000000000..0d1ba6d52f --- /dev/null +++ b/simulation/adx_gyro_accelerometer/src/main/native/include/ADXL362_SpiAccelerometerData.h @@ -0,0 +1,26 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include "ThreeAxisAccelerometerData.h" + +namespace hal { +class ADXL362_SpiAccelerometer : public ThreeAxisAccelerometerData { + public: + explicit ADXL362_SpiAccelerometer(int port); + virtual ~ADXL362_SpiAccelerometer(); + + void HandleWrite(const uint8_t* buffer, uint32_t count); + void HandleRead(uint8_t* buffer, uint32_t count); + + private: + int m_lastWriteAddress; + + static const double LSB; +}; +} // namespace hal 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 new file mode 100644 index 0000000000..5cc7733c25 --- /dev/null +++ b/simulation/adx_gyro_accelerometer/src/main/native/include/ADXRS450_SpiGyroWrapperData.h @@ -0,0 +1,43 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include + +#include "MockData/NotifyListenerVector.h" + +namespace hal { +class ADXRS450_SpiGyroWrapper { + public: + explicit ADXRS450_SpiGyroWrapper(int port); + virtual ~ADXRS450_SpiGyroWrapper(); + + void HandleRead(uint8_t* buffer, uint32_t count); + + virtual void ResetData(); + + int32_t RegisterAngleCallback(HAL_NotifyCallback callback, void* param, + HAL_Bool initialNotify); + void CancelAngleCallback(int32_t uid); + void InvokeAngleCallback(HAL_Value value); + double GetAngle(); + void SetAngle(double angle); + + private: + const int m_port; + + wpi::mutex m_registerMutex; + std::atomic m_angle{0.0}; + std::shared_ptr m_angleCallbacks = nullptr; + + static const double ANGLE_LSB; +}; +} // namespace hal diff --git a/simulation/adx_gyro_accelerometer/src/main/native/include/ThreeAxisAccelerometerData.h b/simulation/adx_gyro_accelerometer/src/main/native/include/ThreeAxisAccelerometerData.h new file mode 100644 index 0000000000..229a874d0a --- /dev/null +++ b/simulation/adx_gyro_accelerometer/src/main/native/include/ThreeAxisAccelerometerData.h @@ -0,0 +1,55 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include + +#include "MockData/NotifyListenerVector.h" + +namespace hal { +class ThreeAxisAccelerometerData { + public: + ThreeAxisAccelerometerData(); + virtual ~ThreeAxisAccelerometerData(); + + int32_t RegisterXCallback(HAL_NotifyCallback callback, void* param, + HAL_Bool initialNotify); + void CancelXCallback(int32_t uid); + void InvokeXCallback(HAL_Value value); + double GetX(); + void SetX(double x); + + int32_t RegisterYCallback(HAL_NotifyCallback callback, void* param, + HAL_Bool initialNotify); + void CancelYCallback(int32_t uid); + void InvokeYCallback(HAL_Value value); + double GetY(); + void SetY(double y); + + int32_t RegisterZCallback(HAL_NotifyCallback callback, void* param, + HAL_Bool initialNotify); + void CancelZCallback(int32_t uid); + void InvokeZCallback(HAL_Value value); + double GetZ(); + void SetZ(double z); + + virtual void ResetData(); + + protected: + wpi::mutex m_registerMutex; + std::atomic m_x{0.0}; + std::shared_ptr m_xCallbacks = nullptr; + std::atomic m_y{0.0}; + std::shared_ptr m_yCallbacks = nullptr; + std::atomic m_z{0.0}; + std::shared_ptr m_zCallbacks = nullptr; +}; +} // namespace hal