Rename our allwpilib (which is now 2020) to not have 2019 in the name
Change-Id: I3c07f85ed32ab8b97db765a9b43f2a6ce7da964a
diff --git a/hal/src/main/native/athena/Accelerometer.cpp b/hal/src/main/native/athena/Accelerometer.cpp
new file mode 100644
index 0000000..f83c06e
--- /dev/null
+++ b/hal/src/main/native/athena/Accelerometer.cpp
@@ -0,0 +1,243 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 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 "hal/Accelerometer.h"
+
+#include <stdint.h>
+
+#include <cassert>
+#include <cstdio>
+#include <memory>
+
+#include "HALInitializer.h"
+#include "hal/ChipObject.h"
+#include "hal/HAL.h"
+
+using namespace hal;
+
+// The 7-bit I2C address with a 0 "send" bit
+static constexpr uint8_t kSendAddress = (0x1c << 1) | 0;
+
+// The 7-bit I2C address with a 1 "receive" bit
+static constexpr uint8_t kReceiveAddress = (0x1c << 1) | 1;
+
+static constexpr uint8_t kControlTxRx = 1;
+static constexpr uint8_t kControlStart = 2;
+static constexpr uint8_t kControlStop = 4;
+
+static std::unique_ptr<tAccel> accel;
+static HAL_AccelerometerRange accelerometerRange;
+
+// Register addresses
+enum Register {
+ kReg_Status = 0x00,
+ kReg_OutXMSB = 0x01,
+ kReg_OutXLSB = 0x02,
+ kReg_OutYMSB = 0x03,
+ kReg_OutYLSB = 0x04,
+ kReg_OutZMSB = 0x05,
+ kReg_OutZLSB = 0x06,
+ kReg_Sysmod = 0x0B,
+ kReg_IntSource = 0x0C,
+ kReg_WhoAmI = 0x0D,
+ kReg_XYZDataCfg = 0x0E,
+ kReg_HPFilterCutoff = 0x0F,
+ kReg_PLStatus = 0x10,
+ kReg_PLCfg = 0x11,
+ kReg_PLCount = 0x12,
+ kReg_PLBfZcomp = 0x13,
+ kReg_PLThsReg = 0x14,
+ kReg_FFMtCfg = 0x15,
+ kReg_FFMtSrc = 0x16,
+ kReg_FFMtThs = 0x17,
+ kReg_FFMtCount = 0x18,
+ kReg_TransientCfg = 0x1D,
+ kReg_TransientSrc = 0x1E,
+ kReg_TransientThs = 0x1F,
+ kReg_TransientCount = 0x20,
+ kReg_PulseCfg = 0x21,
+ kReg_PulseSrc = 0x22,
+ kReg_PulseThsx = 0x23,
+ kReg_PulseThsy = 0x24,
+ kReg_PulseThsz = 0x25,
+ kReg_PulseTmlt = 0x26,
+ kReg_PulseLtcy = 0x27,
+ kReg_PulseWind = 0x28,
+ kReg_ASlpCount = 0x29,
+ kReg_CtrlReg1 = 0x2A,
+ kReg_CtrlReg2 = 0x2B,
+ kReg_CtrlReg3 = 0x2C,
+ kReg_CtrlReg4 = 0x2D,
+ kReg_CtrlReg5 = 0x2E,
+ kReg_OffX = 0x2F,
+ kReg_OffY = 0x30,
+ kReg_OffZ = 0x31
+};
+
+namespace hal {
+namespace init {
+void InitializeAccelerometer() {}
+} // namespace init
+} // namespace hal
+
+namespace hal {
+
+static void writeRegister(Register reg, uint8_t data);
+static uint8_t readRegister(Register reg);
+
+/**
+ * Initialize the accelerometer.
+ */
+static void initializeAccelerometer() {
+ hal::init::CheckInit();
+ int32_t status;
+
+ if (!accel) {
+ accel.reset(tAccel::create(&status));
+
+ accelerometerRange = HAL_AccelerometerRange::HAL_AccelerometerRange_k2G;
+
+ // Enable I2C
+ accel->writeCNFG(1, &status);
+
+ // Set the counter to 100 kbps
+ accel->writeCNTR(213, &status);
+
+ // The device identification number should be 0x2a
+ assert(readRegister(kReg_WhoAmI) == 0x2a);
+ }
+}
+
+static void writeRegister(Register reg, uint8_t data) {
+ int32_t status = 0;
+ uint64_t initialTime;
+
+ accel->writeADDR(kSendAddress, &status);
+
+ // Send a start transmit/receive message with the register address
+ accel->writeCNTL(kControlStart | kControlTxRx, &status);
+ accel->writeDATO(reg, &status);
+ accel->strobeGO(&status);
+
+ // Execute and wait until it's done (up to a millisecond)
+ initialTime = HAL_GetFPGATime(&status);
+ while (accel->readSTAT(&status) & 1) {
+ if (HAL_GetFPGATime(&status) > initialTime + 1000) break;
+ }
+
+ // Send a stop transmit/receive message with the data
+ accel->writeCNTL(kControlStop | kControlTxRx, &status);
+ accel->writeDATO(data, &status);
+ accel->strobeGO(&status);
+
+ // Execute and wait until it's done (up to a millisecond)
+ initialTime = HAL_GetFPGATime(&status);
+ while (accel->readSTAT(&status) & 1) {
+ if (HAL_GetFPGATime(&status) > initialTime + 1000) break;
+ }
+}
+
+static uint8_t readRegister(Register reg) {
+ int32_t status = 0;
+ uint64_t initialTime;
+
+ // Send a start transmit/receive message with the register address
+ accel->writeADDR(kSendAddress, &status);
+ accel->writeCNTL(kControlStart | kControlTxRx, &status);
+ accel->writeDATO(reg, &status);
+ accel->strobeGO(&status);
+
+ // Execute and wait until it's done (up to a millisecond)
+ initialTime = HAL_GetFPGATime(&status);
+ while (accel->readSTAT(&status) & 1) {
+ if (HAL_GetFPGATime(&status) > initialTime + 1000) break;
+ }
+
+ // Receive a message with the data and stop
+ accel->writeADDR(kReceiveAddress, &status);
+ accel->writeCNTL(kControlStart | kControlStop | kControlTxRx, &status);
+ accel->strobeGO(&status);
+
+ // Execute and wait until it's done (up to a millisecond)
+ initialTime = HAL_GetFPGATime(&status);
+ while (accel->readSTAT(&status) & 1) {
+ if (HAL_GetFPGATime(&status) > initialTime + 1000) break;
+ }
+
+ return accel->readDATI(&status);
+}
+
+/**
+ * Convert a 12-bit raw acceleration value into a scaled double in units of
+ * 1 g-force, taking into account the accelerometer range.
+ */
+static double unpackAxis(int16_t raw) {
+ // The raw value is actually 12 bits, not 16, so we need to propogate the
+ // 2's complement sign bit to the unused 4 bits for this to work with
+ // negative numbers.
+ raw <<= 4;
+ raw >>= 4;
+
+ switch (accelerometerRange) {
+ case HAL_AccelerometerRange_k2G:
+ return raw / 1024.0;
+ case HAL_AccelerometerRange_k4G:
+ return raw / 512.0;
+ case HAL_AccelerometerRange_k8G:
+ return raw / 256.0;
+ default:
+ return 0.0;
+ }
+}
+
+} // namespace hal
+
+extern "C" {
+
+void HAL_SetAccelerometerActive(HAL_Bool active) {
+ initializeAccelerometer();
+
+ uint8_t ctrlReg1 = readRegister(kReg_CtrlReg1);
+ ctrlReg1 &= ~1; // Clear the existing active bit
+ writeRegister(kReg_CtrlReg1, ctrlReg1 | (active ? 1 : 0));
+}
+
+void HAL_SetAccelerometerRange(HAL_AccelerometerRange range) {
+ initializeAccelerometer();
+
+ accelerometerRange = range;
+
+ uint8_t xyzDataCfg = readRegister(kReg_XYZDataCfg);
+ xyzDataCfg &= ~3; // Clear the existing two range bits
+ writeRegister(kReg_XYZDataCfg, xyzDataCfg | range);
+}
+
+double HAL_GetAccelerometerX(void) {
+ initializeAccelerometer();
+
+ int32_t raw =
+ (readRegister(kReg_OutXMSB) << 4) | (readRegister(kReg_OutXLSB) >> 4);
+ return unpackAxis(raw);
+}
+
+double HAL_GetAccelerometerY(void) {
+ initializeAccelerometer();
+
+ int32_t raw =
+ (readRegister(kReg_OutYMSB) << 4) | (readRegister(kReg_OutYLSB) >> 4);
+ return unpackAxis(raw);
+}
+
+double HAL_GetAccelerometerZ(void) {
+ initializeAccelerometer();
+
+ int32_t raw =
+ (readRegister(kReg_OutZMSB) << 4) | (readRegister(kReg_OutZLSB) >> 4);
+ return unpackAxis(raw);
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/athena/AddressableLED.cpp b/hal/src/main/native/athena/AddressableLED.cpp
new file mode 100644
index 0000000..64b6457
--- /dev/null
+++ b/hal/src/main/native/athena/AddressableLED.cpp
@@ -0,0 +1,243 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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 "hal/AddressableLED.h"
+
+#include <cstring>
+
+#include <FRC_FPGA_ChipObject/fpgainterfacecapi/NiFpga_HMB.h>
+
+#include "ConstantsInternal.h"
+#include "DigitalInternal.h"
+#include "HALInitializer.h"
+#include "PortsInternal.h"
+#include "hal/ChipObject.h"
+#include "hal/handles/HandlesInternal.h"
+#include "hal/handles/LimitedHandleResource.h"
+
+using namespace hal;
+
+namespace {
+struct AddressableLED {
+ std::unique_ptr<tLED> led;
+ void* ledBuffer;
+ size_t ledBufferSize;
+ int32_t stringLength = 1;
+};
+} // namespace
+
+static LimitedHandleResource<
+ HAL_AddressableLEDHandle, AddressableLED, kNumAddressableLEDs,
+ HAL_HandleEnum::AddressableLED>* addressableLEDHandles;
+
+namespace hal {
+namespace init {
+void InitializeAddressableLED() {
+ static LimitedHandleResource<HAL_AddressableLEDHandle, AddressableLED,
+ kNumAddressableLEDs,
+ HAL_HandleEnum::AddressableLED>
+ alH;
+ addressableLEDHandles = &alH;
+}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+
+HAL_AddressableLEDHandle HAL_InitializeAddressableLED(
+ HAL_DigitalHandle outputPort, int32_t* status) {
+ hal::init::CheckInit();
+
+ auto digitalPort =
+ hal::digitalChannelHandles->Get(outputPort, hal::HAL_HandleEnum::PWM);
+
+ if (!digitalPort) {
+ // If DIO was passed, channel error, else generic error
+ if (getHandleType(outputPort) == hal::HAL_HandleEnum::DIO) {
+ *status = HAL_LED_CHANNEL_ERROR;
+ } else {
+ *status = HAL_HANDLE_ERROR;
+ }
+ return HAL_kInvalidHandle;
+ }
+
+ if (digitalPort->channel >= kNumPWMHeaders) {
+ *status = HAL_LED_CHANNEL_ERROR;
+ return HAL_kInvalidHandle;
+ }
+
+ auto handle = addressableLEDHandles->Allocate();
+
+ if (handle == HAL_kInvalidHandle) {
+ *status = NO_AVAILABLE_RESOURCES;
+ return HAL_kInvalidHandle;
+ }
+
+ auto led = addressableLEDHandles->Get(handle);
+
+ if (!led) {
+ *status = HAL_HANDLE_ERROR;
+ return HAL_kInvalidHandle;
+ }
+
+ led->led.reset(tLED::create(status));
+
+ if (*status != 0) {
+ addressableLEDHandles->Free(handle);
+ return HAL_kInvalidHandle;
+ }
+
+ led->led->writeOutputSelect(digitalPort->channel, status);
+
+ if (*status != 0) {
+ addressableLEDHandles->Free(handle);
+ return HAL_kInvalidHandle;
+ }
+
+ led->ledBuffer = nullptr;
+ led->ledBufferSize = 0;
+
+ uint32_t session = led->led->getSystemInterface()->getHandle();
+
+ *status = NiFpga_OpenHostMemoryBuffer(session, "HMB_0_LED", &led->ledBuffer,
+ &led->ledBufferSize);
+
+ if (*status != 0) {
+ addressableLEDHandles->Free(handle);
+ return HAL_kInvalidHandle;
+ }
+
+ return handle;
+}
+
+void HAL_FreeAddressableLED(HAL_AddressableLEDHandle handle) {
+ addressableLEDHandles->Free(handle);
+}
+
+void HAL_SetAddressableLEDOutputPort(HAL_AddressableLEDHandle handle,
+ HAL_DigitalHandle outputPort,
+ int32_t* status) {
+ auto digitalPort =
+ hal::digitalChannelHandles->Get(outputPort, hal::HAL_HandleEnum::PWM);
+
+ if (!digitalPort) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ auto led = addressableLEDHandles->Get(handle);
+ if (!led) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ led->led->writeOutputSelect(digitalPort->channel, status);
+}
+
+void HAL_SetAddressableLEDLength(HAL_AddressableLEDHandle handle,
+ int32_t length, int32_t* status) {
+ auto led = addressableLEDHandles->Get(handle);
+ if (!led) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (length > HAL_kAddressableLEDMaxLength) {
+ *status = PARAMETER_OUT_OF_RANGE;
+ return;
+ }
+
+ led->led->strobeReset(status);
+
+ while (led->led->readPixelWriteIndex(status) != 0) {
+ }
+
+ if (*status != 0) {
+ return;
+ }
+
+ led->led->writeStringLength(length, status);
+
+ led->stringLength = length;
+}
+
+static_assert(sizeof(HAL_AddressableLEDData) == sizeof(uint32_t),
+ "LED Data must be 32 bit");
+
+void HAL_WriteAddressableLEDData(HAL_AddressableLEDHandle handle,
+ const struct HAL_AddressableLEDData* data,
+ int32_t length, int32_t* status) {
+ auto led = addressableLEDHandles->Get(handle);
+ if (!led) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (length > led->stringLength) {
+ *status = PARAMETER_OUT_OF_RANGE;
+ return;
+ }
+
+ std::memcpy(led->ledBuffer, data, length * sizeof(HAL_AddressableLEDData));
+
+ asm("dmb");
+
+ led->led->strobeLoad(status);
+}
+
+void HAL_SetAddressableLEDBitTiming(HAL_AddressableLEDHandle handle,
+ int32_t lowTime0NanoSeconds,
+ int32_t highTime0NanoSeconds,
+ int32_t lowTime1NanoSeconds,
+ int32_t highTime1NanoSeconds,
+ int32_t* status) {
+ auto led = addressableLEDHandles->Get(handle);
+ if (!led) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ led->led->writeLowBitTickTiming(1, highTime0NanoSeconds / 25, status);
+ led->led->writeLowBitTickTiming(0, lowTime0NanoSeconds / 25, status);
+ led->led->writeHighBitTickTiming(1, highTime1NanoSeconds / 25, status);
+ led->led->writeHighBitTickTiming(0, lowTime1NanoSeconds / 25, status);
+}
+
+void HAL_SetAddressableLEDSyncTime(HAL_AddressableLEDHandle handle,
+ int32_t syncTimeMicroSeconds,
+ int32_t* status) {
+ auto led = addressableLEDHandles->Get(handle);
+ if (!led) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ led->led->writeSyncTiming(syncTimeMicroSeconds, status);
+}
+
+void HAL_StartAddressableLEDOutput(HAL_AddressableLEDHandle handle,
+ int32_t* status) {
+ auto led = addressableLEDHandles->Get(handle);
+ if (!led) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ led->led->strobeStart(status);
+}
+
+void HAL_StopAddressableLEDOutput(HAL_AddressableLEDHandle handle,
+ int32_t* status) {
+ auto led = addressableLEDHandles->Get(handle);
+ if (!led) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ led->led->strobeAbort(status);
+}
+} // extern "C"
diff --git a/hal/src/main/native/athena/AnalogAccumulator.cpp b/hal/src/main/native/athena/AnalogAccumulator.cpp
new file mode 100644
index 0000000..6664c52
--- /dev/null
+++ b/hal/src/main/native/athena/AnalogAccumulator.cpp
@@ -0,0 +1,139 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 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 "hal/AnalogAccumulator.h"
+
+#include "AnalogInternal.h"
+#include "hal/HAL.h"
+
+using namespace hal;
+
+namespace hal {
+namespace init {
+void InitializeAnalogAccumulator() {}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+
+HAL_Bool HAL_IsAccumulatorChannel(HAL_AnalogInputHandle analogPortHandle,
+ int32_t* status) {
+ auto port = analogInputHandles->Get(analogPortHandle);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return false;
+ }
+ for (int32_t i = 0; i < kNumAccumulators; i++) {
+ if (port->channel == kAccumulatorChannels[i]) return true;
+ }
+ return false;
+}
+
+void HAL_InitAccumulator(HAL_AnalogInputHandle analogPortHandle,
+ int32_t* status) {
+ if (!HAL_IsAccumulatorChannel(analogPortHandle, status)) {
+ *status = HAL_INVALID_ACCUMULATOR_CHANNEL;
+ return;
+ }
+ HAL_SetAccumulatorCenter(analogPortHandle, 0, status);
+ HAL_ResetAccumulator(analogPortHandle, status);
+}
+
+void HAL_ResetAccumulator(HAL_AnalogInputHandle analogPortHandle,
+ int32_t* status) {
+ auto port = analogInputHandles->Get(analogPortHandle);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ if (port->accumulator == nullptr) {
+ *status = NULL_PARAMETER;
+ return;
+ }
+ port->accumulator->strobeReset(status);
+}
+
+void HAL_SetAccumulatorCenter(HAL_AnalogInputHandle analogPortHandle,
+ int32_t center, int32_t* status) {
+ auto port = analogInputHandles->Get(analogPortHandle);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ if (port->accumulator == nullptr) {
+ *status = NULL_PARAMETER;
+ return;
+ }
+ port->accumulator->writeCenter(center, status);
+}
+
+void HAL_SetAccumulatorDeadband(HAL_AnalogInputHandle analogPortHandle,
+ int32_t deadband, int32_t* status) {
+ auto port = analogInputHandles->Get(analogPortHandle);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ if (port->accumulator == nullptr) {
+ *status = NULL_PARAMETER;
+ return;
+ }
+ port->accumulator->writeDeadband(deadband, status);
+}
+
+int64_t HAL_GetAccumulatorValue(HAL_AnalogInputHandle analogPortHandle,
+ int32_t* status) {
+ auto port = analogInputHandles->Get(analogPortHandle);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+ if (port->accumulator == nullptr) {
+ *status = NULL_PARAMETER;
+ return 0;
+ }
+ int64_t value = port->accumulator->readOutput_Value(status);
+ return value;
+}
+
+int64_t HAL_GetAccumulatorCount(HAL_AnalogInputHandle analogPortHandle,
+ int32_t* status) {
+ auto port = analogInputHandles->Get(analogPortHandle);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+ if (port->accumulator == nullptr) {
+ *status = NULL_PARAMETER;
+ return 0;
+ }
+ return port->accumulator->readOutput_Count(status);
+}
+
+void HAL_GetAccumulatorOutput(HAL_AnalogInputHandle analogPortHandle,
+ int64_t* value, int64_t* count, int32_t* status) {
+ auto port = analogInputHandles->Get(analogPortHandle);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ if (port->accumulator == nullptr) {
+ *status = NULL_PARAMETER;
+ return;
+ }
+ if (value == nullptr || count == nullptr) {
+ *status = NULL_PARAMETER;
+ return;
+ }
+
+ tAccumulator::tOutput output = port->accumulator->readOutput(status);
+
+ *value = output.Value;
+ *count = output.Count;
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/athena/AnalogGyro.cpp b/hal/src/main/native/athena/AnalogGyro.cpp
new file mode 100644
index 0000000..12d688d
--- /dev/null
+++ b/hal/src/main/native/athena/AnalogGyro.cpp
@@ -0,0 +1,261 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 "hal/AnalogGyro.h"
+
+#include <thread>
+
+#include <wpi/raw_ostream.h>
+
+#include "AnalogInternal.h"
+#include "HALInitializer.h"
+#include "hal/AnalogAccumulator.h"
+#include "hal/AnalogInput.h"
+#include "hal/handles/IndexedHandleResource.h"
+
+namespace {
+
+struct AnalogGyro {
+ HAL_AnalogInputHandle handle;
+ double voltsPerDegreePerSecond;
+ double offset;
+ int32_t center;
+};
+
+} // namespace
+
+static constexpr uint32_t kOversampleBits = 10;
+static constexpr uint32_t kAverageBits = 0;
+static constexpr double kSamplesPerSecond = 50.0;
+static constexpr double kCalibrationSampleTime = 5.0;
+static constexpr double kDefaultVoltsPerDegreePerSecond = 0.007;
+
+using namespace hal;
+
+static IndexedHandleResource<HAL_GyroHandle, AnalogGyro, kNumAccumulators,
+ HAL_HandleEnum::AnalogGyro>* analogGyroHandles;
+
+namespace hal {
+namespace init {
+void InitializeAnalogGyro() {
+ static IndexedHandleResource<HAL_GyroHandle, AnalogGyro, kNumAccumulators,
+ HAL_HandleEnum::AnalogGyro>
+ agHandles;
+ analogGyroHandles = &agHandles;
+}
+} // namespace init
+} // namespace hal
+
+static void Wait(double seconds) {
+ if (seconds < 0.0) return;
+ std::this_thread::sleep_for(std::chrono::duration<double>(seconds));
+}
+
+extern "C" {
+
+HAL_GyroHandle HAL_InitializeAnalogGyro(HAL_AnalogInputHandle analogHandle,
+ int32_t* status) {
+ hal::init::CheckInit();
+ if (!HAL_IsAccumulatorChannel(analogHandle, status)) {
+ if (*status == 0) {
+ *status = HAL_INVALID_ACCUMULATOR_CHANNEL;
+ }
+ return HAL_kInvalidHandle;
+ }
+
+ // handle known to be correct, so no need to type check
+ int16_t channel = getHandleIndex(analogHandle);
+
+ auto handle = analogGyroHandles->Allocate(channel, status);
+
+ if (*status != 0)
+ return HAL_kInvalidHandle; // failed to allocate. Pass error back.
+
+ // Initialize port structure
+ auto gyro = analogGyroHandles->Get(handle);
+ if (gyro == nullptr) { // would only error on thread issue
+ *status = HAL_HANDLE_ERROR;
+ return HAL_kInvalidHandle;
+ }
+
+ gyro->handle = analogHandle;
+ gyro->voltsPerDegreePerSecond = 0;
+ gyro->offset = 0;
+ gyro->center = 0;
+
+ return handle;
+}
+
+void HAL_SetupAnalogGyro(HAL_GyroHandle handle, int32_t* status) {
+ auto gyro = analogGyroHandles->Get(handle);
+ if (gyro == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ gyro->voltsPerDegreePerSecond = kDefaultVoltsPerDegreePerSecond;
+
+ HAL_SetAnalogAverageBits(gyro->handle, kAverageBits, status);
+ if (*status != 0) return;
+ HAL_SetAnalogOversampleBits(gyro->handle, kOversampleBits, status);
+ if (*status != 0) return;
+ double sampleRate =
+ kSamplesPerSecond * (1 << (kAverageBits + kOversampleBits));
+ HAL_SetAnalogSampleRate(sampleRate, status);
+ if (*status != 0) return;
+ Wait(0.1);
+
+ HAL_SetAnalogGyroDeadband(handle, 0.0, status);
+ if (*status != 0) return;
+}
+
+void HAL_FreeAnalogGyro(HAL_GyroHandle handle) {
+ analogGyroHandles->Free(handle);
+}
+
+void HAL_SetAnalogGyroParameters(HAL_GyroHandle handle,
+ double voltsPerDegreePerSecond, double offset,
+ int32_t center, int32_t* status) {
+ auto gyro = analogGyroHandles->Get(handle);
+ if (gyro == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ gyro->voltsPerDegreePerSecond = voltsPerDegreePerSecond;
+ gyro->offset = offset;
+ gyro->center = center;
+ HAL_SetAccumulatorCenter(gyro->handle, center, status);
+}
+
+void HAL_SetAnalogGyroVoltsPerDegreePerSecond(HAL_GyroHandle handle,
+ double voltsPerDegreePerSecond,
+ int32_t* status) {
+ auto gyro = analogGyroHandles->Get(handle);
+ if (gyro == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ gyro->voltsPerDegreePerSecond = voltsPerDegreePerSecond;
+}
+
+void HAL_ResetAnalogGyro(HAL_GyroHandle handle, int32_t* status) {
+ auto gyro = analogGyroHandles->Get(handle);
+ if (gyro == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ HAL_ResetAccumulator(gyro->handle, status);
+ if (*status != 0) return;
+
+ const double sampleTime = 1.0 / HAL_GetAnalogSampleRate(status);
+ const double overSamples =
+ 1 << HAL_GetAnalogOversampleBits(gyro->handle, status);
+ const double averageSamples =
+ 1 << HAL_GetAnalogAverageBits(gyro->handle, status);
+ if (*status != 0) return;
+ Wait(sampleTime * overSamples * averageSamples);
+}
+
+void HAL_CalibrateAnalogGyro(HAL_GyroHandle handle, int32_t* status) {
+ auto gyro = analogGyroHandles->Get(handle);
+ if (gyro == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ HAL_InitAccumulator(gyro->handle, status);
+ if (*status != 0) return;
+ wpi::outs() << "Calibrating analog gyro for " << kCalibrationSampleTime
+ << " seconds." << '\n';
+ Wait(kCalibrationSampleTime);
+
+ int64_t value;
+ int64_t count;
+ HAL_GetAccumulatorOutput(gyro->handle, &value, &count, status);
+ if (*status != 0) return;
+
+ gyro->center = static_cast<int32_t>(
+ static_cast<double>(value) / static_cast<double>(count) + 0.5);
+
+ gyro->offset = static_cast<double>(value) / static_cast<double>(count) -
+ static_cast<double>(gyro->center);
+ HAL_SetAccumulatorCenter(gyro->handle, gyro->center, status);
+ if (*status != 0) return;
+ HAL_ResetAnalogGyro(handle, status);
+}
+
+void HAL_SetAnalogGyroDeadband(HAL_GyroHandle handle, double volts,
+ int32_t* status) {
+ auto gyro = analogGyroHandles->Get(handle);
+ if (gyro == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ int32_t deadband = static_cast<int32_t>(
+ volts * 1e9 / HAL_GetAnalogLSBWeight(gyro->handle, status) *
+ (1 << HAL_GetAnalogOversampleBits(gyro->handle, status)));
+ if (*status != 0) return;
+ HAL_SetAccumulatorDeadband(gyro->handle, deadband, status);
+}
+
+double HAL_GetAnalogGyroAngle(HAL_GyroHandle handle, int32_t* status) {
+ auto gyro = analogGyroHandles->Get(handle);
+ if (gyro == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+ int64_t rawValue = 0;
+ int64_t count = 0;
+ HAL_GetAccumulatorOutput(gyro->handle, &rawValue, &count, status);
+
+ int64_t value = rawValue - static_cast<int64_t>(static_cast<double>(count) *
+ gyro->offset);
+
+ double scaledValue =
+ value * 1e-9 *
+ static_cast<double>(HAL_GetAnalogLSBWeight(gyro->handle, status)) *
+ static_cast<double>(1 << HAL_GetAnalogAverageBits(gyro->handle, status)) /
+ (HAL_GetAnalogSampleRate(status) * gyro->voltsPerDegreePerSecond);
+
+ return scaledValue;
+}
+
+double HAL_GetAnalogGyroRate(HAL_GyroHandle handle, int32_t* status) {
+ auto gyro = analogGyroHandles->Get(handle);
+ if (gyro == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+
+ return (HAL_GetAnalogAverageValue(gyro->handle, status) -
+ (static_cast<double>(gyro->center) + gyro->offset)) *
+ 1e-9 * HAL_GetAnalogLSBWeight(gyro->handle, status) /
+ ((1 << HAL_GetAnalogOversampleBits(gyro->handle, status)) *
+ gyro->voltsPerDegreePerSecond);
+}
+
+double HAL_GetAnalogGyroOffset(HAL_GyroHandle handle, int32_t* status) {
+ auto gyro = analogGyroHandles->Get(handle);
+ if (gyro == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+ return gyro->offset;
+}
+
+int32_t HAL_GetAnalogGyroCenter(HAL_GyroHandle handle, int32_t* status) {
+ auto gyro = analogGyroHandles->Get(handle);
+ if (gyro == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+ return gyro->center;
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/athena/AnalogInput.cpp b/hal/src/main/native/athena/AnalogInput.cpp
new file mode 100644
index 0000000..1d502b9
--- /dev/null
+++ b/hal/src/main/native/athena/AnalogInput.cpp
@@ -0,0 +1,261 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 "hal/AnalogInput.h"
+
+#include <FRC_NetworkCommunication/AICalibration.h>
+#include <wpi/mutex.h>
+
+#include "AnalogInternal.h"
+#include "HALInitializer.h"
+#include "PortsInternal.h"
+#include "hal/AnalogAccumulator.h"
+#include "hal/handles/HandlesInternal.h"
+
+namespace hal {
+namespace init {
+void InitializeAnalogInput() {}
+} // namespace init
+} // namespace hal
+
+using namespace hal;
+
+extern "C" {
+
+HAL_AnalogInputHandle HAL_InitializeAnalogInputPort(HAL_PortHandle portHandle,
+ int32_t* status) {
+ hal::init::CheckInit();
+ initializeAnalog(status);
+
+ if (*status != 0) return HAL_kInvalidHandle;
+
+ int16_t channel = getPortHandleChannel(portHandle);
+ if (channel == InvalidHandleIndex) {
+ *status = PARAMETER_OUT_OF_RANGE;
+ return HAL_kInvalidHandle;
+ }
+
+ HAL_AnalogInputHandle handle = analogInputHandles->Allocate(channel, status);
+
+ if (*status != 0)
+ return HAL_kInvalidHandle; // failed to allocate. Pass error back.
+
+ // Initialize port structure
+ auto analog_port = analogInputHandles->Get(handle);
+ if (analog_port == nullptr) { // would only error on thread issue
+ *status = HAL_HANDLE_ERROR;
+ return HAL_kInvalidHandle;
+ }
+ analog_port->channel = static_cast<uint8_t>(channel);
+ if (HAL_IsAccumulatorChannel(handle, status)) {
+ analog_port->accumulator.reset(tAccumulator::create(channel, status));
+ } else {
+ analog_port->accumulator = nullptr;
+ }
+
+ // Set default configuration
+ analogInputSystem->writeScanList(channel, channel, status);
+ HAL_SetAnalogAverageBits(handle, kDefaultAverageBits, status);
+ HAL_SetAnalogOversampleBits(handle, kDefaultOversampleBits, status);
+ return handle;
+}
+
+void HAL_FreeAnalogInputPort(HAL_AnalogInputHandle analogPortHandle) {
+ // no status, so no need to check for a proper free.
+ analogInputHandles->Free(analogPortHandle);
+}
+
+HAL_Bool HAL_CheckAnalogModule(int32_t module) { return module == 1; }
+
+HAL_Bool HAL_CheckAnalogInputChannel(int32_t channel) {
+ return channel < kNumAnalogInputs && channel >= 0;
+}
+
+void HAL_SetAnalogInputSimDevice(HAL_AnalogInputHandle handle,
+ HAL_SimDeviceHandle device) {}
+
+void HAL_SetAnalogSampleRate(double samplesPerSecond, int32_t* status) {
+ // TODO: This will change when variable size scan lists are implemented.
+ // TODO: Need double comparison with epsilon.
+ // wpi_assert(!sampleRateSet || GetSampleRate() == samplesPerSecond);
+ initializeAnalog(status);
+ if (*status != 0) return;
+ setAnalogSampleRate(samplesPerSecond, status);
+}
+
+double HAL_GetAnalogSampleRate(int32_t* status) {
+ initializeAnalog(status);
+ if (*status != 0) return 0;
+ uint32_t ticksPerConversion = analogInputSystem->readLoopTiming(status);
+ uint32_t ticksPerSample =
+ ticksPerConversion * getAnalogNumActiveChannels(status);
+ return static_cast<double>(kTimebase) / static_cast<double>(ticksPerSample);
+}
+
+void HAL_SetAnalogAverageBits(HAL_AnalogInputHandle analogPortHandle,
+ int32_t bits, int32_t* status) {
+ auto port = analogInputHandles->Get(analogPortHandle);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ analogInputSystem->writeAverageBits(port->channel, static_cast<uint8_t>(bits),
+ status);
+}
+
+int32_t HAL_GetAnalogAverageBits(HAL_AnalogInputHandle analogPortHandle,
+ int32_t* status) {
+ auto port = analogInputHandles->Get(analogPortHandle);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return kDefaultAverageBits;
+ }
+ uint8_t result = analogInputSystem->readAverageBits(port->channel, status);
+ return result;
+}
+
+void HAL_SetAnalogOversampleBits(HAL_AnalogInputHandle analogPortHandle,
+ int32_t bits, int32_t* status) {
+ auto port = analogInputHandles->Get(analogPortHandle);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ analogInputSystem->writeOversampleBits(port->channel,
+ static_cast<uint8_t>(bits), status);
+}
+
+int32_t HAL_GetAnalogOversampleBits(HAL_AnalogInputHandle analogPortHandle,
+ int32_t* status) {
+ auto port = analogInputHandles->Get(analogPortHandle);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return kDefaultOversampleBits;
+ }
+ uint8_t result = analogInputSystem->readOversampleBits(port->channel, status);
+ return result;
+}
+
+int32_t HAL_GetAnalogValue(HAL_AnalogInputHandle analogPortHandle,
+ int32_t* status) {
+ auto port = analogInputHandles->Get(analogPortHandle);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+
+ tAI::tReadSelect readSelect;
+ readSelect.Channel = port->channel;
+ readSelect.Averaged = false;
+
+ std::scoped_lock lock(analogRegisterWindowMutex);
+ analogInputSystem->writeReadSelect(readSelect, status);
+ analogInputSystem->strobeLatchOutput(status);
+ return static_cast<int16_t>(analogInputSystem->readOutput(status));
+}
+
+int32_t HAL_GetAnalogAverageValue(HAL_AnalogInputHandle analogPortHandle,
+ int32_t* status) {
+ auto port = analogInputHandles->Get(analogPortHandle);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+ tAI::tReadSelect readSelect;
+ readSelect.Channel = port->channel;
+ readSelect.Averaged = true;
+
+ std::scoped_lock lock(analogRegisterWindowMutex);
+ analogInputSystem->writeReadSelect(readSelect, status);
+ analogInputSystem->strobeLatchOutput(status);
+ return static_cast<int32_t>(analogInputSystem->readOutput(status));
+}
+
+int32_t HAL_GetAnalogVoltsToValue(HAL_AnalogInputHandle analogPortHandle,
+ double voltage, int32_t* status) {
+ if (voltage > 5.0) {
+ voltage = 5.0;
+ *status = VOLTAGE_OUT_OF_RANGE;
+ }
+ if (voltage < 0.0) {
+ voltage = 0.0;
+ *status = VOLTAGE_OUT_OF_RANGE;
+ }
+ int32_t LSBWeight = HAL_GetAnalogLSBWeight(analogPortHandle, status);
+ int32_t offset = HAL_GetAnalogOffset(analogPortHandle, status);
+ int32_t value =
+ static_cast<int32_t>((voltage + offset * 1.0e-9) / (LSBWeight * 1.0e-9));
+ return value;
+}
+
+double HAL_GetAnalogVoltage(HAL_AnalogInputHandle analogPortHandle,
+ int32_t* status) {
+ int32_t value = HAL_GetAnalogValue(analogPortHandle, status);
+ int32_t LSBWeight = HAL_GetAnalogLSBWeight(analogPortHandle, status);
+ int32_t offset = HAL_GetAnalogOffset(analogPortHandle, status);
+ double voltage = LSBWeight * 1.0e-9 * value - offset * 1.0e-9;
+ return voltage;
+}
+
+double HAL_GetAnalogValueToVolts(HAL_AnalogInputHandle analogPortHandle,
+ int32_t rawValue, int32_t* status) {
+ int32_t LSBWeight = HAL_GetAnalogLSBWeight(analogPortHandle, status);
+ int32_t offset = HAL_GetAnalogOffset(analogPortHandle, status);
+ double voltage = LSBWeight * 1.0e-9 * rawValue - offset * 1.0e-9;
+ return voltage;
+}
+
+double HAL_GetAnalogAverageVoltage(HAL_AnalogInputHandle analogPortHandle,
+ int32_t* status) {
+ int32_t value = HAL_GetAnalogAverageValue(analogPortHandle, status);
+ int32_t LSBWeight = HAL_GetAnalogLSBWeight(analogPortHandle, status);
+ int32_t offset = HAL_GetAnalogOffset(analogPortHandle, status);
+ int32_t oversampleBits =
+ HAL_GetAnalogOversampleBits(analogPortHandle, status);
+ double voltage =
+ LSBWeight * 1.0e-9 * value / static_cast<double>(1 << oversampleBits) -
+ offset * 1.0e-9;
+ return voltage;
+}
+
+int32_t HAL_GetAnalogLSBWeight(HAL_AnalogInputHandle analogPortHandle,
+ int32_t* status) {
+ // On the roboRIO, LSB is the same for all channels. So the channel lookup can
+ // be avoided
+ return FRC_NetworkCommunication_nAICalibration_getLSBWeight(0, 0, status);
+
+ // Keep the old code for future hardware
+
+ // auto port = analogInputHandles->Get(analogPortHandle);
+ // if (port == nullptr) {
+ // *status = HAL_HANDLE_ERROR;
+ // return 0;
+ // }
+ // int32_t lsbWeight = FRC_NetworkCommunication_nAICalibration_getLSBWeight(
+ // 0, port->channel, status); // XXX: aiSystemIndex == 0?
+ // return lsbWeight;
+}
+
+int32_t HAL_GetAnalogOffset(HAL_AnalogInputHandle analogPortHandle,
+ int32_t* status) {
+ // On the roboRIO, offset is the same for all channels. So the channel lookup
+ // can be avoided
+ return FRC_NetworkCommunication_nAICalibration_getOffset(0, 0, status);
+
+ // Keep the old code for future hardware
+
+ // auto port = analogInputHandles->Get(analogPortHandle);
+ // if (port == nullptr) {
+ // *status = HAL_HANDLE_ERROR;
+ // return 0;
+ // }
+ // int32_t offset = FRC_NetworkCommunication_nAICalibration_getOffset(
+ // 0, port->channel, status); // XXX: aiSystemIndex == 0?
+ // return offset;
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/athena/AnalogInternal.cpp b/hal/src/main/native/athena/AnalogInternal.cpp
new file mode 100644
index 0000000..03a246d
--- /dev/null
+++ b/hal/src/main/native/athena/AnalogInternal.cpp
@@ -0,0 +1,100 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 "AnalogInternal.h"
+
+#include <atomic>
+
+#include <wpi/mutex.h>
+
+#include "HALInitializer.h"
+#include "PortsInternal.h"
+#include "hal/AnalogInput.h"
+#include "hal/ChipObject.h"
+
+namespace hal {
+
+wpi::mutex analogRegisterWindowMutex;
+std::unique_ptr<tAI> analogInputSystem;
+std::unique_ptr<tAO> analogOutputSystem;
+
+IndexedHandleResource<HAL_AnalogInputHandle, ::hal::AnalogPort,
+ kNumAnalogInputs, HAL_HandleEnum::AnalogInput>*
+ analogInputHandles;
+
+static int32_t analogNumChannelsToActivate = 0;
+
+static std::atomic<bool> analogSystemInitialized{false};
+
+bool analogSampleRateSet = false;
+
+namespace init {
+void InitializeAnalogInternal() {
+ static IndexedHandleResource<HAL_AnalogInputHandle, ::hal::AnalogPort,
+ kNumAnalogInputs, HAL_HandleEnum::AnalogInput>
+ alH;
+ analogInputHandles = &alH;
+}
+} // namespace init
+
+void initializeAnalog(int32_t* status) {
+ hal::init::CheckInit();
+ if (analogSystemInitialized) return;
+ std::scoped_lock lock(analogRegisterWindowMutex);
+ if (analogSystemInitialized) return;
+ analogInputSystem.reset(tAI::create(status));
+ analogOutputSystem.reset(tAO::create(status));
+ setAnalogNumChannelsToActivate(kNumAnalogInputs);
+ setAnalogSampleRate(kDefaultSampleRate, status);
+ analogSystemInitialized = true;
+}
+
+int32_t getAnalogNumActiveChannels(int32_t* status) {
+ int32_t scanSize = analogInputSystem->readConfig_ScanSize(status);
+ if (scanSize == 0) return 8;
+ return scanSize;
+}
+
+void setAnalogNumChannelsToActivate(int32_t channels) {
+ analogNumChannelsToActivate = channels;
+}
+
+int32_t getAnalogNumChannelsToActivate(int32_t* status) {
+ if (analogNumChannelsToActivate == 0)
+ return getAnalogNumActiveChannels(status);
+ return analogNumChannelsToActivate;
+}
+
+void setAnalogSampleRate(double samplesPerSecond, int32_t* status) {
+ // TODO: This will change when variable size scan lists are implemented.
+ // TODO: Need double comparison with epsilon.
+ // wpi_assert(!sampleRateSet || GetSampleRate() == samplesPerSecond);
+ analogSampleRateSet = true;
+
+ // Compute the convert rate
+ uint32_t ticksPerSample =
+ static_cast<uint32_t>(static_cast<double>(kTimebase) / samplesPerSecond);
+ uint32_t ticksPerConversion =
+ ticksPerSample / getAnalogNumChannelsToActivate(status);
+ // ticksPerConversion must be at least 80
+ if (ticksPerConversion < 80) {
+ if ((*status) >= 0) *status = SAMPLE_RATE_TOO_HIGH;
+ ticksPerConversion = 80;
+ }
+
+ // Atomically set the scan size and the convert rate so that the sample rate
+ // is constant
+ tAI::tConfig config;
+ config.ScanSize = getAnalogNumChannelsToActivate(status);
+ config.ConvertRate = ticksPerConversion;
+ analogInputSystem->writeConfig(config, status);
+
+ // Indicate that the scan size has been commited to hardware.
+ setAnalogNumChannelsToActivate(0);
+}
+
+} // namespace hal
diff --git a/hal/src/main/native/athena/AnalogInternal.h b/hal/src/main/native/athena/AnalogInternal.h
new file mode 100644
index 0000000..a74562f
--- /dev/null
+++ b/hal/src/main/native/athena/AnalogInternal.h
@@ -0,0 +1,88 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 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 <stdint.h>
+
+#include <memory>
+
+#include <wpi/mutex.h>
+
+#include "PortsInternal.h"
+#include "hal/ChipObject.h"
+#include "hal/Ports.h"
+#include "hal/handles/IndexedHandleResource.h"
+
+namespace hal {
+
+constexpr int32_t kTimebase = 40000000; ///< 40 MHz clock
+constexpr int32_t kDefaultOversampleBits = 0;
+constexpr int32_t kDefaultAverageBits = 7;
+constexpr double kDefaultSampleRate = 50000.0;
+static constexpr uint32_t kAccumulatorChannels[] = {0, 1};
+
+extern std::unique_ptr<tAI> analogInputSystem;
+extern std::unique_ptr<tAO> analogOutputSystem;
+extern wpi::mutex analogRegisterWindowMutex;
+extern bool analogSampleRateSet;
+
+struct AnalogPort {
+ uint8_t channel;
+ std::unique_ptr<tAccumulator> accumulator;
+};
+
+extern IndexedHandleResource<HAL_AnalogInputHandle, hal::AnalogPort,
+ kNumAnalogInputs, HAL_HandleEnum::AnalogInput>*
+ analogInputHandles;
+
+/**
+ * Initialize the analog System.
+ */
+void initializeAnalog(int32_t* status);
+
+/**
+ * Return the number of channels on the module in use.
+ *
+ * @return Active channels.
+ */
+int32_t getAnalogNumActiveChannels(int32_t* status);
+
+/**
+ * Set the number of active channels.
+ *
+ * Store the number of active channels to set. Don't actually commit to
+ * hardware
+ * until SetSampleRate().
+ *
+ * @param channels Number of active channels.
+ */
+void setAnalogNumChannelsToActivate(int32_t channels);
+
+/**
+ * Get the number of active channels.
+ *
+ * This is an internal function to allow the atomic update of both the
+ * number of active channels and the sample rate.
+ *
+ * When the number of channels changes, use the new value. Otherwise,
+ * return the curent value.
+ *
+ * @return Value to write to the active channels field.
+ */
+int32_t getAnalogNumChannelsToActivate(int32_t* status);
+
+/**
+ * Set the sample rate.
+ *
+ * This is a global setting for the Athena and effects all channels.
+ *
+ * @param samplesPerSecond The number of samples per channel per second.
+ */
+void setAnalogSampleRate(double samplesPerSecond, int32_t* status);
+
+} // namespace hal
diff --git a/hal/src/main/native/athena/AnalogOutput.cpp b/hal/src/main/native/athena/AnalogOutput.cpp
new file mode 100644
index 0000000..77f841b
--- /dev/null
+++ b/hal/src/main/native/athena/AnalogOutput.cpp
@@ -0,0 +1,113 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 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 "hal/AnalogOutput.h"
+
+#include "AnalogInternal.h"
+#include "HALInitializer.h"
+#include "PortsInternal.h"
+#include "hal/Errors.h"
+#include "hal/handles/HandlesInternal.h"
+#include "hal/handles/IndexedHandleResource.h"
+
+using namespace hal;
+
+namespace {
+
+struct AnalogOutput {
+ uint8_t channel;
+};
+
+} // namespace
+
+static IndexedHandleResource<HAL_AnalogOutputHandle, AnalogOutput,
+ kNumAnalogOutputs, HAL_HandleEnum::AnalogOutput>*
+ analogOutputHandles;
+
+namespace hal {
+namespace init {
+void InitializeAnalogOutput() {
+ static IndexedHandleResource<HAL_AnalogOutputHandle, AnalogOutput,
+ kNumAnalogOutputs, HAL_HandleEnum::AnalogOutput>
+ aoH;
+ analogOutputHandles = &aoH;
+}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+
+HAL_AnalogOutputHandle HAL_InitializeAnalogOutputPort(HAL_PortHandle portHandle,
+ int32_t* status) {
+ hal::init::CheckInit();
+ initializeAnalog(status);
+
+ if (*status != 0) return HAL_kInvalidHandle;
+
+ int16_t channel = getPortHandleChannel(portHandle);
+ if (channel == InvalidHandleIndex) {
+ *status = PARAMETER_OUT_OF_RANGE;
+ return HAL_kInvalidHandle;
+ }
+
+ HAL_AnalogOutputHandle handle =
+ analogOutputHandles->Allocate(channel, status);
+
+ if (*status != 0)
+ return HAL_kInvalidHandle; // failed to allocate. Pass error back.
+
+ auto port = analogOutputHandles->Get(handle);
+ if (port == nullptr) { // would only error on thread issue
+ *status = HAL_HANDLE_ERROR;
+ return HAL_kInvalidHandle;
+ }
+
+ port->channel = static_cast<uint8_t>(channel);
+ return handle;
+}
+
+void HAL_FreeAnalogOutputPort(HAL_AnalogOutputHandle analogOutputHandle) {
+ // no status, so no need to check for a proper free.
+ analogOutputHandles->Free(analogOutputHandle);
+}
+
+void HAL_SetAnalogOutput(HAL_AnalogOutputHandle analogOutputHandle,
+ double voltage, int32_t* status) {
+ auto port = analogOutputHandles->Get(analogOutputHandle);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ uint16_t rawValue = static_cast<uint16_t>(voltage / 5.0 * 0x1000);
+
+ if (voltage < 0.0)
+ rawValue = 0;
+ else if (voltage > 5.0)
+ rawValue = 0x1000;
+
+ analogOutputSystem->writeMXP(port->channel, rawValue, status);
+}
+
+double HAL_GetAnalogOutput(HAL_AnalogOutputHandle analogOutputHandle,
+ int32_t* status) {
+ auto port = analogOutputHandles->Get(analogOutputHandle);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0.0;
+ }
+
+ uint16_t rawValue = analogOutputSystem->readMXP(port->channel, status);
+
+ return rawValue * 5.0 / 0x1000;
+}
+
+HAL_Bool HAL_CheckAnalogOutputChannel(int32_t channel) {
+ return channel < kNumAnalogOutputs && channel >= 0;
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/athena/AnalogTrigger.cpp b/hal/src/main/native/athena/AnalogTrigger.cpp
new file mode 100644
index 0000000..9ec3f29
--- /dev/null
+++ b/hal/src/main/native/athena/AnalogTrigger.cpp
@@ -0,0 +1,275 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 "hal/AnalogTrigger.h"
+
+#include "AnalogInternal.h"
+#include "DutyCycleInternal.h"
+#include "HALInitializer.h"
+#include "PortsInternal.h"
+#include "hal/AnalogInput.h"
+#include "hal/DutyCycle.h"
+#include "hal/Errors.h"
+#include "hal/handles/HandlesInternal.h"
+#include "hal/handles/LimitedHandleResource.h"
+
+using namespace hal;
+
+namespace {
+
+struct AnalogTrigger {
+ std::unique_ptr<tAnalogTrigger> trigger;
+ HAL_Handle handle;
+ uint8_t index;
+};
+
+} // namespace
+
+static LimitedHandleResource<HAL_AnalogTriggerHandle, AnalogTrigger,
+ kNumAnalogTriggers, HAL_HandleEnum::AnalogTrigger>*
+ analogTriggerHandles;
+
+namespace hal {
+namespace init {
+void InitializeAnalogTrigger() {
+ static LimitedHandleResource<HAL_AnalogTriggerHandle, AnalogTrigger,
+ kNumAnalogTriggers,
+ HAL_HandleEnum::AnalogTrigger>
+ atH;
+ analogTriggerHandles = &atH;
+}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+
+HAL_AnalogTriggerHandle HAL_InitializeAnalogTrigger(
+ HAL_AnalogInputHandle portHandle, int32_t* status) {
+ hal::init::CheckInit();
+ // ensure we are given a valid and active AnalogInput handle
+ auto analog_port = analogInputHandles->Get(portHandle);
+ if (analog_port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return HAL_kInvalidHandle;
+ }
+ HAL_AnalogTriggerHandle handle = analogTriggerHandles->Allocate();
+ if (handle == HAL_kInvalidHandle) {
+ *status = NO_AVAILABLE_RESOURCES;
+ return HAL_kInvalidHandle;
+ }
+ auto trigger = analogTriggerHandles->Get(handle);
+ if (trigger == nullptr) { // would only occur on thread issue
+ *status = HAL_HANDLE_ERROR;
+ return HAL_kInvalidHandle;
+ }
+ trigger->handle = portHandle;
+ trigger->index = static_cast<uint8_t>(getHandleIndex(handle));
+
+ trigger->trigger.reset(tAnalogTrigger::create(trigger->index, status));
+ trigger->trigger->writeSourceSelect_Channel(analog_port->channel, status);
+ return handle;
+}
+
+HAL_AnalogTriggerHandle HAL_InitializeAnalogTriggerDutyCycle(
+ HAL_DutyCycleHandle dutyCycleHandle, int32_t* status) {
+ hal::init::CheckInit();
+ // ensure we are given a valid and active DutyCycle handle
+ auto dutyCycle = dutyCycleHandles->Get(dutyCycleHandle);
+ if (dutyCycle == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return HAL_kInvalidHandle;
+ }
+ HAL_AnalogTriggerHandle handle = analogTriggerHandles->Allocate();
+ if (handle == HAL_kInvalidHandle) {
+ *status = NO_AVAILABLE_RESOURCES;
+ return HAL_kInvalidHandle;
+ }
+ auto trigger = analogTriggerHandles->Get(handle);
+ if (trigger == nullptr) { // would only occur on thread issue
+ *status = HAL_HANDLE_ERROR;
+ return HAL_kInvalidHandle;
+ }
+ trigger->handle = dutyCycleHandle;
+ trigger->index = static_cast<uint8_t>(getHandleIndex(handle));
+
+ trigger->trigger.reset(tAnalogTrigger::create(trigger->index, status));
+ trigger->trigger->writeSourceSelect_Channel(dutyCycle->index, status);
+ trigger->trigger->writeSourceSelect_DutyCycle(true, status);
+ return handle;
+}
+
+void HAL_CleanAnalogTrigger(HAL_AnalogTriggerHandle analogTriggerHandle,
+ int32_t* status) {
+ analogTriggerHandles->Free(analogTriggerHandle);
+ // caller owns the input handle.
+}
+
+void HAL_SetAnalogTriggerLimitsRaw(HAL_AnalogTriggerHandle analogTriggerHandle,
+ int32_t lower, int32_t upper,
+ int32_t* status) {
+ auto trigger = analogTriggerHandles->Get(analogTriggerHandle);
+ if (trigger == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ if (lower > upper) {
+ *status = ANALOG_TRIGGER_LIMIT_ORDER_ERROR;
+ return;
+ }
+ trigger->trigger->writeLowerLimit(lower, status);
+ trigger->trigger->writeUpperLimit(upper, status);
+}
+
+void HAL_SetAnalogTriggerLimitsDutyCycle(
+ HAL_AnalogTriggerHandle analogTriggerHandle, double lower, double upper,
+ int32_t* status) {
+ auto trigger = analogTriggerHandles->Get(analogTriggerHandle);
+ if (trigger == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ if (getHandleType(trigger->handle) != HAL_HandleEnum::DutyCycle) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ if (lower > upper) {
+ *status = ANALOG_TRIGGER_LIMIT_ORDER_ERROR;
+ return;
+ }
+
+ if (lower < 0.0 || upper > 1.0) {
+ *status = PARAMETER_OUT_OF_RANGE;
+ return;
+ }
+
+ int32_t scaleFactor =
+ HAL_GetDutyCycleOutputScaleFactor(trigger->handle, status);
+ if (*status != 0) {
+ return;
+ }
+
+ trigger->trigger->writeLowerLimit(static_cast<int32_t>(scaleFactor * lower),
+ status);
+ trigger->trigger->writeUpperLimit(static_cast<int32_t>(scaleFactor * upper),
+ status);
+}
+
+void HAL_SetAnalogTriggerLimitsVoltage(
+ HAL_AnalogTriggerHandle analogTriggerHandle, double lower, double upper,
+ int32_t* status) {
+ auto trigger = analogTriggerHandles->Get(analogTriggerHandle);
+ if (trigger == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (getHandleType(trigger->handle) != HAL_HandleEnum::AnalogInput) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ if (lower > upper) {
+ *status = ANALOG_TRIGGER_LIMIT_ORDER_ERROR;
+ return;
+ }
+
+ // TODO: This depends on the averaged setting. Only raw values will work as
+ // is.
+ trigger->trigger->writeLowerLimit(
+ HAL_GetAnalogVoltsToValue(trigger->handle, lower, status), status);
+ trigger->trigger->writeUpperLimit(
+ HAL_GetAnalogVoltsToValue(trigger->handle, upper, status), status);
+}
+
+void HAL_SetAnalogTriggerAveraged(HAL_AnalogTriggerHandle analogTriggerHandle,
+ HAL_Bool useAveragedValue, int32_t* status) {
+ auto trigger = analogTriggerHandles->Get(analogTriggerHandle);
+ if (trigger == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ if (trigger->trigger->readSourceSelect_Filter(status) != 0 ||
+ trigger->trigger->readSourceSelect_DutyCycle(status) != 0) {
+ *status = INCOMPATIBLE_STATE;
+ // TODO: wpi_setWPIErrorWithContext(IncompatibleMode, "Hardware does not
+ // support average and filtering at the same time.");
+ }
+ trigger->trigger->writeSourceSelect_Averaged(useAveragedValue, status);
+}
+
+void HAL_SetAnalogTriggerFiltered(HAL_AnalogTriggerHandle analogTriggerHandle,
+ HAL_Bool useFilteredValue, int32_t* status) {
+ auto trigger = analogTriggerHandles->Get(analogTriggerHandle);
+ if (trigger == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ if (trigger->trigger->readSourceSelect_Averaged(status) != 0 ||
+ trigger->trigger->readSourceSelect_DutyCycle(status) != 0) {
+ *status = INCOMPATIBLE_STATE;
+ // TODO: wpi_setWPIErrorWithContext(IncompatibleMode, "Hardware does not "
+ // "support average and filtering at the same time.");
+ }
+ trigger->trigger->writeSourceSelect_Filter(useFilteredValue, status);
+}
+
+HAL_Bool HAL_GetAnalogTriggerInWindow(
+ HAL_AnalogTriggerHandle analogTriggerHandle, int32_t* status) {
+ auto trigger = analogTriggerHandles->Get(analogTriggerHandle);
+ if (trigger == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return false;
+ }
+ return trigger->trigger->readOutput_InHysteresis(trigger->index, status) != 0;
+}
+
+HAL_Bool HAL_GetAnalogTriggerTriggerState(
+ HAL_AnalogTriggerHandle analogTriggerHandle, int32_t* status) {
+ auto trigger = analogTriggerHandles->Get(analogTriggerHandle);
+ if (trigger == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return false;
+ }
+ return trigger->trigger->readOutput_OverLimit(trigger->index, status) != 0;
+}
+
+HAL_Bool HAL_GetAnalogTriggerOutput(HAL_AnalogTriggerHandle analogTriggerHandle,
+ HAL_AnalogTriggerType type,
+ int32_t* status) {
+ auto trigger = analogTriggerHandles->Get(analogTriggerHandle);
+ if (trigger == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return false;
+ }
+ bool result = false;
+ switch (type) {
+ case HAL_Trigger_kInWindow:
+ result =
+ trigger->trigger->readOutput_InHysteresis(trigger->index, status);
+ break;
+ case HAL_Trigger_kState:
+ result = trigger->trigger->readOutput_OverLimit(trigger->index, status);
+ break;
+ case HAL_Trigger_kRisingPulse:
+ case HAL_Trigger_kFallingPulse:
+ *status = ANALOG_TRIGGER_PULSE_OUTPUT_ERROR;
+ return false;
+ break;
+ }
+ return result;
+}
+
+int32_t HAL_GetAnalogTriggerFPGAIndex(
+ HAL_AnalogTriggerHandle analogTriggerHandle, int32_t* status) {
+ auto trigger = analogTriggerHandles->Get(analogTriggerHandle);
+ if (trigger == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return -1;
+ }
+ return trigger->index;
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/athena/CAN.cpp b/hal/src/main/native/athena/CAN.cpp
new file mode 100644
index 0000000..8105358
--- /dev/null
+++ b/hal/src/main/native/athena/CAN.cpp
@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 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 "hal/CAN.h"
+
+#include <FRC_NetworkCommunication/CANSessionMux.h>
+
+namespace hal {
+namespace init {
+void InitializeCAN() {}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+
+void HAL_CAN_SendMessage(uint32_t messageID, const uint8_t* data,
+ uint8_t dataSize, int32_t periodMs, int32_t* status) {
+ FRC_NetworkCommunication_CANSessionMux_sendMessage(messageID, data, dataSize,
+ periodMs, status);
+}
+void HAL_CAN_ReceiveMessage(uint32_t* messageID, uint32_t messageIDMask,
+ uint8_t* data, uint8_t* dataSize,
+ uint32_t* timeStamp, int32_t* status) {
+ FRC_NetworkCommunication_CANSessionMux_receiveMessage(
+ messageID, messageIDMask, data, dataSize, timeStamp, status);
+}
+void HAL_CAN_OpenStreamSession(uint32_t* sessionHandle, uint32_t messageID,
+ uint32_t messageIDMask, uint32_t maxMessages,
+ int32_t* status) {
+ FRC_NetworkCommunication_CANSessionMux_openStreamSession(
+ sessionHandle, messageID, messageIDMask, maxMessages, status);
+}
+void HAL_CAN_CloseStreamSession(uint32_t sessionHandle) {
+ FRC_NetworkCommunication_CANSessionMux_closeStreamSession(sessionHandle);
+}
+void HAL_CAN_ReadStreamSession(uint32_t sessionHandle,
+ struct HAL_CANStreamMessage* messages,
+ uint32_t messagesToRead, uint32_t* messagesRead,
+ int32_t* status) {
+ FRC_NetworkCommunication_CANSessionMux_readStreamSession(
+ sessionHandle, reinterpret_cast<tCANStreamMessage*>(messages),
+ messagesToRead, messagesRead, status);
+}
+void HAL_CAN_GetCANStatus(float* percentBusUtilization, uint32_t* busOffCount,
+ uint32_t* txFullCount, uint32_t* receiveErrorCount,
+ uint32_t* transmitErrorCount, int32_t* status) {
+ FRC_NetworkCommunication_CANSessionMux_getCANStatus(
+ percentBusUtilization, busOffCount, txFullCount, receiveErrorCount,
+ transmitErrorCount, status);
+}
+} // extern "C"
diff --git a/hal/src/main/native/athena/CANAPI.cpp b/hal/src/main/native/athena/CANAPI.cpp
new file mode 100644
index 0000000..44cdb58
--- /dev/null
+++ b/hal/src/main/native/athena/CANAPI.cpp
@@ -0,0 +1,288 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 "hal/CANAPI.h"
+
+#include <atomic>
+#include <ctime>
+
+#include <wpi/DenseMap.h>
+
+#include "HALInitializer.h"
+#include "hal/CAN.h"
+#include "hal/Errors.h"
+#include "hal/HAL.h"
+#include "hal/handles/UnlimitedHandleResource.h"
+
+using namespace hal;
+
+namespace {
+struct Receives {
+ uint32_t lastTimeStamp;
+ uint8_t data[8];
+ uint8_t length;
+};
+
+struct CANStorage {
+ HAL_CANManufacturer manufacturer;
+ HAL_CANDeviceType deviceType;
+ uint8_t deviceId;
+ wpi::mutex mapMutex;
+ wpi::SmallDenseMap<int32_t, int32_t> periodicSends;
+ wpi::SmallDenseMap<int32_t, Receives> receives;
+};
+} // namespace
+
+static UnlimitedHandleResource<HAL_CANHandle, CANStorage, HAL_HandleEnum::CAN>*
+ canHandles;
+
+static uint32_t GetPacketBaseTime() {
+ timespec t;
+ clock_gettime(CLOCK_MONOTONIC, &t);
+
+ // Convert t to milliseconds
+ uint64_t ms = t.tv_sec * 1000ull + t.tv_nsec / 1000000ull;
+ return ms & 0xFFFFFFFF;
+}
+
+namespace hal {
+namespace init {
+void InitializeCANAPI() {
+ static UnlimitedHandleResource<HAL_CANHandle, CANStorage, HAL_HandleEnum::CAN>
+ cH;
+ canHandles = &cH;
+}
+} // namespace init
+} // namespace hal
+
+static int32_t CreateCANId(CANStorage* storage, int32_t apiId) {
+ int32_t createdId = 0;
+ createdId |= (static_cast<int32_t>(storage->deviceType) & 0x1F) << 24;
+ createdId |= (static_cast<int32_t>(storage->manufacturer) & 0xFF) << 16;
+ createdId |= (apiId & 0x3FF) << 6;
+ createdId |= (storage->deviceId & 0x3F);
+ return createdId;
+}
+
+extern "C" {
+
+HAL_CANHandle HAL_InitializeCAN(HAL_CANManufacturer manufacturer,
+ int32_t deviceId, HAL_CANDeviceType deviceType,
+ int32_t* status) {
+ hal::init::CheckInit();
+ auto can = std::make_shared<CANStorage>();
+
+ auto handle = canHandles->Allocate(can);
+
+ if (handle == HAL_kInvalidHandle) {
+ *status = NO_AVAILABLE_RESOURCES;
+ return HAL_kInvalidHandle;
+ }
+
+ can->deviceId = deviceId;
+ can->deviceType = deviceType;
+ can->manufacturer = manufacturer;
+
+ return handle;
+}
+
+void HAL_CleanCAN(HAL_CANHandle handle) {
+ auto data = canHandles->Free(handle);
+
+ std::scoped_lock lock(data->mapMutex);
+
+ for (auto&& i : data->periodicSends) {
+ int32_t s = 0;
+ auto id = CreateCANId(data.get(), i.first);
+ HAL_CAN_SendMessage(id, nullptr, 0, HAL_CAN_SEND_PERIOD_STOP_REPEATING, &s);
+ i.second = -1;
+ }
+}
+
+void HAL_WriteCANPacket(HAL_CANHandle handle, const uint8_t* data,
+ int32_t length, int32_t apiId, int32_t* status) {
+ auto can = canHandles->Get(handle);
+ if (!can) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ auto id = CreateCANId(can.get(), apiId);
+
+ HAL_CAN_SendMessage(id, data, length, HAL_CAN_SEND_PERIOD_NO_REPEAT, status);
+
+ if (*status != 0) {
+ return;
+ }
+ std::scoped_lock lock(can->mapMutex);
+ can->periodicSends[apiId] = -1;
+}
+
+void HAL_WriteCANPacketRepeating(HAL_CANHandle handle, const uint8_t* data,
+ int32_t length, int32_t apiId,
+ int32_t repeatMs, int32_t* status) {
+ auto can = canHandles->Get(handle);
+ if (!can) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ auto id = CreateCANId(can.get(), apiId);
+
+ HAL_CAN_SendMessage(id, data, length, repeatMs, status);
+
+ if (*status != 0) {
+ return;
+ }
+ std::scoped_lock lock(can->mapMutex);
+ can->periodicSends[apiId] = repeatMs;
+}
+
+void HAL_WriteCANRTRFrame(HAL_CANHandle handle, int32_t length, int32_t apiId,
+ int32_t* status) {
+ auto can = canHandles->Get(handle);
+ if (!can) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ auto id = CreateCANId(can.get(), apiId);
+ id |= HAL_CAN_IS_FRAME_REMOTE;
+ uint8_t data[8];
+ std::memset(data, 0, sizeof(data));
+
+ HAL_CAN_SendMessage(id, data, length, HAL_CAN_SEND_PERIOD_NO_REPEAT, status);
+
+ if (*status != 0) {
+ return;
+ }
+ std::scoped_lock lock(can->mapMutex);
+ can->periodicSends[apiId] = -1;
+}
+
+void HAL_StopCANPacketRepeating(HAL_CANHandle handle, int32_t apiId,
+ int32_t* status) {
+ auto can = canHandles->Get(handle);
+ if (!can) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ auto id = CreateCANId(can.get(), apiId);
+
+ HAL_CAN_SendMessage(id, nullptr, 0, HAL_CAN_SEND_PERIOD_STOP_REPEATING,
+ status);
+
+ if (*status != 0) {
+ return;
+ }
+ std::scoped_lock lock(can->mapMutex);
+ can->periodicSends[apiId] = -1;
+}
+
+void HAL_ReadCANPacketNew(HAL_CANHandle handle, int32_t apiId, uint8_t* data,
+ int32_t* length, uint64_t* receivedTimestamp,
+ int32_t* status) {
+ auto can = canHandles->Get(handle);
+ if (!can) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ uint32_t messageId = CreateCANId(can.get(), apiId);
+ uint8_t dataSize = 0;
+ uint32_t ts = 0;
+ HAL_CAN_ReceiveMessage(&messageId, 0x1FFFFFFF, data, &dataSize, &ts, status);
+
+ if (*status == 0) {
+ std::scoped_lock lock(can->mapMutex);
+ auto& msg = can->receives[messageId];
+ msg.length = dataSize;
+ msg.lastTimeStamp = ts;
+ // The NetComm call placed in data, copy into the msg
+ std::memcpy(msg.data, data, dataSize);
+ }
+ *length = dataSize;
+ *receivedTimestamp = ts;
+}
+
+void HAL_ReadCANPacketLatest(HAL_CANHandle handle, int32_t apiId, uint8_t* data,
+ int32_t* length, uint64_t* receivedTimestamp,
+ int32_t* status) {
+ auto can = canHandles->Get(handle);
+ if (!can) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ uint32_t messageId = CreateCANId(can.get(), apiId);
+ uint8_t dataSize = 0;
+ uint32_t ts = 0;
+ HAL_CAN_ReceiveMessage(&messageId, 0x1FFFFFFF, data, &dataSize, &ts, status);
+
+ std::scoped_lock lock(can->mapMutex);
+ if (*status == 0) {
+ // fresh update
+ auto& msg = can->receives[messageId];
+ msg.length = dataSize;
+ *length = dataSize;
+ msg.lastTimeStamp = ts;
+ *receivedTimestamp = ts;
+ // The NetComm call placed in data, copy into the msg
+ std::memcpy(msg.data, data, dataSize);
+ } else {
+ auto i = can->receives.find(messageId);
+ if (i != can->receives.end()) {
+ // Read the data from the stored message into the output
+ std::memcpy(data, i->second.data, i->second.length);
+ *length = i->second.length;
+ *receivedTimestamp = i->second.lastTimeStamp;
+ *status = 0;
+ }
+ }
+}
+
+void HAL_ReadCANPacketTimeout(HAL_CANHandle handle, int32_t apiId,
+ uint8_t* data, int32_t* length,
+ uint64_t* receivedTimestamp, int32_t timeoutMs,
+ int32_t* status) {
+ auto can = canHandles->Get(handle);
+ if (!can) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ uint32_t messageId = CreateCANId(can.get(), apiId);
+ uint8_t dataSize = 0;
+ uint32_t ts = 0;
+ HAL_CAN_ReceiveMessage(&messageId, 0x1FFFFFFF, data, &dataSize, &ts, status);
+
+ std::scoped_lock lock(can->mapMutex);
+ if (*status == 0) {
+ // fresh update
+ auto& msg = can->receives[messageId];
+ msg.length = dataSize;
+ *length = dataSize;
+ msg.lastTimeStamp = ts;
+ *receivedTimestamp = ts;
+ // The NetComm call placed in data, copy into the msg
+ std::memcpy(msg.data, data, dataSize);
+ } else {
+ auto i = can->receives.find(messageId);
+ if (i != can->receives.end()) {
+ // Found, check if new enough
+ uint32_t now = GetPacketBaseTime();
+ if (now - i->second.lastTimeStamp > static_cast<uint32_t>(timeoutMs)) {
+ // Timeout, return bad status
+ *status = HAL_CAN_TIMEOUT;
+ return;
+ }
+ // Read the data from the stored message into the output
+ std::memcpy(data, i->second.data, i->second.length);
+ *length = i->second.length;
+ *receivedTimestamp = i->second.lastTimeStamp;
+ *status = 0;
+ }
+ }
+}
+} // extern "C"
diff --git a/hal/src/main/native/athena/Compressor.cpp b/hal/src/main/native/athena/Compressor.cpp
new file mode 100644
index 0000000..f381305
--- /dev/null
+++ b/hal/src/main/native/athena/Compressor.cpp
@@ -0,0 +1,202 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 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 "hal/Compressor.h"
+
+#include "HALInitializer.h"
+#include "PCMInternal.h"
+#include "PortsInternal.h"
+#include "ctre/PCM.h"
+#include "hal/Errors.h"
+#include "hal/handles/HandlesInternal.h"
+
+using namespace hal;
+
+namespace hal {
+namespace init {
+void InitializeCompressor() {}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+
+HAL_CompressorHandle HAL_InitializeCompressor(int32_t module, int32_t* status) {
+ hal::init::CheckInit();
+ // Use status to check for invalid index
+ initializePCM(module, status);
+ if (*status != 0) {
+ return HAL_kInvalidHandle;
+ }
+
+ // As compressors can have unlimited objects, just create a
+ // handle with the module number as the index.
+
+ return (HAL_CompressorHandle)createHandle(static_cast<int16_t>(module),
+ HAL_HandleEnum::Compressor, 0);
+}
+
+HAL_Bool HAL_CheckCompressorModule(int32_t module) {
+ return module < kNumPCMModules && module >= 0;
+}
+
+HAL_Bool HAL_GetCompressor(HAL_CompressorHandle compressorHandle,
+ int32_t* status) {
+ int16_t index =
+ getHandleTypedIndex(compressorHandle, HAL_HandleEnum::Compressor, 0);
+ if (index == InvalidHandleIndex) {
+ *status = HAL_HANDLE_ERROR;
+ return false;
+ }
+ bool value;
+
+ *status = PCM_modules[index]->GetCompressor(value);
+
+ return value;
+}
+
+void HAL_SetCompressorClosedLoopControl(HAL_CompressorHandle compressorHandle,
+ HAL_Bool value, int32_t* status) {
+ int16_t index =
+ getHandleTypedIndex(compressorHandle, HAL_HandleEnum::Compressor, 0);
+ if (index == InvalidHandleIndex) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ *status = PCM_modules[index]->SetClosedLoopControl(value);
+}
+
+HAL_Bool HAL_GetCompressorClosedLoopControl(
+ HAL_CompressorHandle compressorHandle, int32_t* status) {
+ int16_t index =
+ getHandleTypedIndex(compressorHandle, HAL_HandleEnum::Compressor, 0);
+ if (index == InvalidHandleIndex) {
+ *status = HAL_HANDLE_ERROR;
+ return false;
+ }
+ bool value;
+
+ *status = PCM_modules[index]->GetClosedLoopControl(value);
+
+ return value;
+}
+
+HAL_Bool HAL_GetCompressorPressureSwitch(HAL_CompressorHandle compressorHandle,
+ int32_t* status) {
+ int16_t index =
+ getHandleTypedIndex(compressorHandle, HAL_HandleEnum::Compressor, 0);
+ if (index == InvalidHandleIndex) {
+ *status = HAL_HANDLE_ERROR;
+ return false;
+ }
+ bool value;
+
+ *status = PCM_modules[index]->GetPressure(value);
+
+ return value;
+}
+
+double HAL_GetCompressorCurrent(HAL_CompressorHandle compressorHandle,
+ int32_t* status) {
+ int16_t index =
+ getHandleTypedIndex(compressorHandle, HAL_HandleEnum::Compressor, 0);
+ if (index == InvalidHandleIndex) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+ float value;
+
+ *status = PCM_modules[index]->GetCompressorCurrent(value);
+
+ return value;
+}
+HAL_Bool HAL_GetCompressorCurrentTooHighFault(
+ HAL_CompressorHandle compressorHandle, int32_t* status) {
+ int16_t index =
+ getHandleTypedIndex(compressorHandle, HAL_HandleEnum::Compressor, 0);
+ if (index == InvalidHandleIndex) {
+ *status = HAL_HANDLE_ERROR;
+ return false;
+ }
+ bool value;
+
+ *status = PCM_modules[index]->GetCompressorCurrentTooHighFault(value);
+
+ return value;
+}
+HAL_Bool HAL_GetCompressorCurrentTooHighStickyFault(
+ HAL_CompressorHandle compressorHandle, int32_t* status) {
+ int16_t index =
+ getHandleTypedIndex(compressorHandle, HAL_HandleEnum::Compressor, 0);
+ if (index == InvalidHandleIndex) {
+ *status = HAL_HANDLE_ERROR;
+ return false;
+ }
+ bool value;
+
+ *status = PCM_modules[index]->GetCompressorCurrentTooHighStickyFault(value);
+
+ return value;
+}
+HAL_Bool HAL_GetCompressorShortedStickyFault(
+ HAL_CompressorHandle compressorHandle, int32_t* status) {
+ int16_t index =
+ getHandleTypedIndex(compressorHandle, HAL_HandleEnum::Compressor, 0);
+ if (index == InvalidHandleIndex) {
+ *status = HAL_HANDLE_ERROR;
+ return false;
+ }
+ bool value;
+
+ *status = PCM_modules[index]->GetCompressorShortedStickyFault(value);
+
+ return value;
+}
+HAL_Bool HAL_GetCompressorShortedFault(HAL_CompressorHandle compressorHandle,
+ int32_t* status) {
+ int16_t index =
+ getHandleTypedIndex(compressorHandle, HAL_HandleEnum::Compressor, 0);
+ if (index == InvalidHandleIndex) {
+ *status = HAL_HANDLE_ERROR;
+ return false;
+ }
+ bool value;
+
+ *status = PCM_modules[index]->GetCompressorShortedFault(value);
+
+ return value;
+}
+HAL_Bool HAL_GetCompressorNotConnectedStickyFault(
+ HAL_CompressorHandle compressorHandle, int32_t* status) {
+ int16_t index =
+ getHandleTypedIndex(compressorHandle, HAL_HandleEnum::Compressor, 0);
+ if (index == InvalidHandleIndex) {
+ *status = HAL_HANDLE_ERROR;
+ return false;
+ }
+ bool value;
+
+ *status = PCM_modules[index]->GetCompressorNotConnectedStickyFault(value);
+
+ return value;
+}
+HAL_Bool HAL_GetCompressorNotConnectedFault(
+ HAL_CompressorHandle compressorHandle, int32_t* status) {
+ int16_t index =
+ getHandleTypedIndex(compressorHandle, HAL_HandleEnum::Compressor, 0);
+ if (index == InvalidHandleIndex) {
+ *status = HAL_HANDLE_ERROR;
+ return false;
+ }
+ bool value;
+
+ *status = PCM_modules[index]->GetCompressorNotConnectedFault(value);
+
+ return value;
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/athena/Constants.cpp b/hal/src/main/native/athena/Constants.cpp
new file mode 100644
index 0000000..6af443d
--- /dev/null
+++ b/hal/src/main/native/athena/Constants.cpp
@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 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 "hal/Constants.h"
+
+#include "ConstantsInternal.h"
+
+using namespace hal;
+
+namespace hal {
+namespace init {
+void InitializeConstants() {}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+
+int32_t HAL_GetSystemClockTicksPerMicrosecond(void) {
+ return kSystemClockTicksPerMicrosecond;
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/athena/ConstantsInternal.h b/hal/src/main/native/athena/ConstantsInternal.h
new file mode 100644
index 0000000..55bbdee
--- /dev/null
+++ b/hal/src/main/native/athena/ConstantsInternal.h
@@ -0,0 +1,16 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 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 <stdint.h>
+
+namespace hal {
+
+constexpr int32_t kSystemClockTicksPerMicrosecond = 40;
+
+} // namespace hal
diff --git a/hal/src/main/native/athena/Counter.cpp b/hal/src/main/native/athena/Counter.cpp
new file mode 100644
index 0000000..6d7e254
--- /dev/null
+++ b/hal/src/main/native/athena/Counter.cpp
@@ -0,0 +1,372 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 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 "hal/Counter.h"
+
+#include "ConstantsInternal.h"
+#include "DigitalInternal.h"
+#include "HALInitializer.h"
+#include "PortsInternal.h"
+#include "hal/HAL.h"
+#include "hal/handles/LimitedHandleResource.h"
+
+using namespace hal;
+
+namespace {
+
+struct Counter {
+ std::unique_ptr<tCounter> counter;
+ uint8_t index;
+};
+
+} // namespace
+
+static LimitedHandleResource<HAL_CounterHandle, Counter, kNumCounters,
+ HAL_HandleEnum::Counter>* counterHandles;
+
+namespace hal {
+namespace init {
+void InitializeCounter() {
+ static LimitedHandleResource<HAL_CounterHandle, Counter, kNumCounters,
+ HAL_HandleEnum::Counter>
+ ch;
+ counterHandles = &ch;
+}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+
+HAL_CounterHandle HAL_InitializeCounter(HAL_Counter_Mode mode, int32_t* index,
+ int32_t* status) {
+ hal::init::CheckInit();
+ auto handle = counterHandles->Allocate();
+ if (handle == HAL_kInvalidHandle) { // out of resources
+ *status = NO_AVAILABLE_RESOURCES;
+ return HAL_kInvalidHandle;
+ }
+ auto counter = counterHandles->Get(handle);
+ if (counter == nullptr) { // would only occur on thread issues
+ *status = HAL_HANDLE_ERROR;
+ return HAL_kInvalidHandle;
+ }
+ counter->index = static_cast<uint8_t>(getHandleIndex(handle));
+ *index = counter->index;
+
+ counter->counter.reset(tCounter::create(counter->index, status));
+ counter->counter->writeConfig_Mode(mode, status);
+ counter->counter->writeTimerConfig_AverageSize(1, status);
+ return handle;
+}
+
+void HAL_FreeCounter(HAL_CounterHandle counterHandle, int32_t* status) {
+ counterHandles->Free(counterHandle);
+}
+
+void HAL_SetCounterAverageSize(HAL_CounterHandle counterHandle, int32_t size,
+ int32_t* status) {
+ auto counter = counterHandles->Get(counterHandle);
+ if (counter == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ counter->counter->writeTimerConfig_AverageSize(size, status);
+}
+
+void HAL_SetCounterUpSource(HAL_CounterHandle counterHandle,
+ HAL_Handle digitalSourceHandle,
+ HAL_AnalogTriggerType analogTriggerType,
+ int32_t* status) {
+ auto counter = counterHandles->Get(counterHandle);
+ if (counter == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ bool routingAnalogTrigger = false;
+ uint8_t routingChannel = 0;
+ uint8_t routingModule = 0;
+ bool success =
+ remapDigitalSource(digitalSourceHandle, analogTriggerType, routingChannel,
+ routingModule, routingAnalogTrigger);
+ if (!success) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ counter->counter->writeConfig_UpSource_Module(routingModule, status);
+ counter->counter->writeConfig_UpSource_Channel(routingChannel, status);
+ counter->counter->writeConfig_UpSource_AnalogTrigger(routingAnalogTrigger,
+ status);
+
+ if (counter->counter->readConfig_Mode(status) == HAL_Counter_kTwoPulse ||
+ counter->counter->readConfig_Mode(status) ==
+ HAL_Counter_kExternalDirection) {
+ HAL_SetCounterUpSourceEdge(counterHandle, true, false, status);
+ }
+ counter->counter->strobeReset(status);
+}
+
+void HAL_SetCounterUpSourceEdge(HAL_CounterHandle counterHandle,
+ HAL_Bool risingEdge, HAL_Bool fallingEdge,
+ int32_t* status) {
+ auto counter = counterHandles->Get(counterHandle);
+ if (counter == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ counter->counter->writeConfig_UpRisingEdge(risingEdge, status);
+ counter->counter->writeConfig_UpFallingEdge(fallingEdge, status);
+}
+
+void HAL_ClearCounterUpSource(HAL_CounterHandle counterHandle,
+ int32_t* status) {
+ auto counter = counterHandles->Get(counterHandle);
+ if (counter == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ counter->counter->writeConfig_UpFallingEdge(false, status);
+ counter->counter->writeConfig_UpRisingEdge(false, status);
+ // Index 0 of digital is always 0.
+ counter->counter->writeConfig_UpSource_Channel(0, status);
+ counter->counter->writeConfig_UpSource_AnalogTrigger(false, status);
+}
+
+void HAL_SetCounterDownSource(HAL_CounterHandle counterHandle,
+ HAL_Handle digitalSourceHandle,
+ HAL_AnalogTriggerType analogTriggerType,
+ int32_t* status) {
+ auto counter = counterHandles->Get(counterHandle);
+ if (counter == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ uint8_t mode = counter->counter->readConfig_Mode(status);
+ if (mode != HAL_Counter_kTwoPulse && mode != HAL_Counter_kExternalDirection) {
+ // TODO: wpi_setWPIErrorWithContext(ParameterOutOfRange, "Counter only
+ // supports DownSource in TwoPulse and ExternalDirection modes.");
+ *status = PARAMETER_OUT_OF_RANGE;
+ return;
+ }
+
+ bool routingAnalogTrigger = false;
+ uint8_t routingChannel = 0;
+ uint8_t routingModule = 0;
+ bool success =
+ remapDigitalSource(digitalSourceHandle, analogTriggerType, routingChannel,
+ routingModule, routingAnalogTrigger);
+ if (!success) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ counter->counter->writeConfig_DownSource_Module(routingModule, status);
+ counter->counter->writeConfig_DownSource_Channel(routingChannel, status);
+ counter->counter->writeConfig_DownSource_AnalogTrigger(routingAnalogTrigger,
+ status);
+
+ HAL_SetCounterDownSourceEdge(counterHandle, true, false, status);
+ counter->counter->strobeReset(status);
+}
+
+void HAL_SetCounterDownSourceEdge(HAL_CounterHandle counterHandle,
+ HAL_Bool risingEdge, HAL_Bool fallingEdge,
+ int32_t* status) {
+ auto counter = counterHandles->Get(counterHandle);
+ if (counter == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ counter->counter->writeConfig_DownRisingEdge(risingEdge, status);
+ counter->counter->writeConfig_DownFallingEdge(fallingEdge, status);
+}
+
+void HAL_ClearCounterDownSource(HAL_CounterHandle counterHandle,
+ int32_t* status) {
+ auto counter = counterHandles->Get(counterHandle);
+ if (counter == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ counter->counter->writeConfig_DownFallingEdge(false, status);
+ counter->counter->writeConfig_DownRisingEdge(false, status);
+ // Index 0 of digital is always 0.
+ counter->counter->writeConfig_DownSource_Channel(0, status);
+ counter->counter->writeConfig_DownSource_AnalogTrigger(false, status);
+}
+
+void HAL_SetCounterUpDownMode(HAL_CounterHandle counterHandle,
+ int32_t* status) {
+ auto counter = counterHandles->Get(counterHandle);
+ if (counter == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ counter->counter->writeConfig_Mode(HAL_Counter_kTwoPulse, status);
+}
+
+void HAL_SetCounterExternalDirectionMode(HAL_CounterHandle counterHandle,
+ int32_t* status) {
+ auto counter = counterHandles->Get(counterHandle);
+ if (counter == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ counter->counter->writeConfig_Mode(HAL_Counter_kExternalDirection, status);
+}
+
+void HAL_SetCounterSemiPeriodMode(HAL_CounterHandle counterHandle,
+ HAL_Bool highSemiPeriod, int32_t* status) {
+ auto counter = counterHandles->Get(counterHandle);
+ if (counter == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ counter->counter->writeConfig_Mode(HAL_Counter_kSemiperiod, status);
+ counter->counter->writeConfig_UpRisingEdge(highSemiPeriod, status);
+ HAL_SetCounterUpdateWhenEmpty(counterHandle, false, status);
+}
+
+void HAL_SetCounterPulseLengthMode(HAL_CounterHandle counterHandle,
+ double threshold, int32_t* status) {
+ auto counter = counterHandles->Get(counterHandle);
+ if (counter == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ counter->counter->writeConfig_Mode(HAL_Counter_kPulseLength, status);
+ counter->counter->writeConfig_PulseLengthThreshold(
+ static_cast<uint32_t>(threshold * 1.0e6) *
+ kSystemClockTicksPerMicrosecond,
+ status);
+}
+
+int32_t HAL_GetCounterSamplesToAverage(HAL_CounterHandle counterHandle,
+ int32_t* status) {
+ auto counter = counterHandles->Get(counterHandle);
+ if (counter == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+ return counter->counter->readTimerConfig_AverageSize(status);
+}
+
+void HAL_SetCounterSamplesToAverage(HAL_CounterHandle counterHandle,
+ int32_t samplesToAverage, int32_t* status) {
+ auto counter = counterHandles->Get(counterHandle);
+ if (counter == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ if (samplesToAverage < 1 || samplesToAverage > 127) {
+ *status = PARAMETER_OUT_OF_RANGE;
+ }
+ counter->counter->writeTimerConfig_AverageSize(samplesToAverage, status);
+}
+
+void HAL_ResetCounter(HAL_CounterHandle counterHandle, int32_t* status) {
+ auto counter = counterHandles->Get(counterHandle);
+ if (counter == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ counter->counter->strobeReset(status);
+}
+
+int32_t HAL_GetCounter(HAL_CounterHandle counterHandle, int32_t* status) {
+ auto counter = counterHandles->Get(counterHandle);
+ if (counter == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+ int32_t value = counter->counter->readOutput_Value(status);
+ return value;
+}
+
+double HAL_GetCounterPeriod(HAL_CounterHandle counterHandle, int32_t* status) {
+ auto counter = counterHandles->Get(counterHandle);
+ if (counter == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0.0;
+ }
+ tCounter::tTimerOutput output = counter->counter->readTimerOutput(status);
+ double period;
+ if (output.Stalled) {
+ // Return infinity
+ double zero = 0.0;
+ period = 1.0 / zero;
+ } else {
+ // output.Period is a fixed point number that counts by 2 (24 bits, 25
+ // integer bits)
+ period = static_cast<double>(output.Period << 1) /
+ static_cast<double>(output.Count);
+ }
+ return static_cast<double>(period *
+ 2.5e-8); // result * timebase (currently 25ns)
+}
+
+void HAL_SetCounterMaxPeriod(HAL_CounterHandle counterHandle, double maxPeriod,
+ int32_t* status) {
+ auto counter = counterHandles->Get(counterHandle);
+ if (counter == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ counter->counter->writeTimerConfig_StallPeriod(
+ static_cast<uint32_t>(maxPeriod * 4.0e8), status);
+}
+
+void HAL_SetCounterUpdateWhenEmpty(HAL_CounterHandle counterHandle,
+ HAL_Bool enabled, int32_t* status) {
+ auto counter = counterHandles->Get(counterHandle);
+ if (counter == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ counter->counter->writeTimerConfig_UpdateWhenEmpty(enabled, status);
+}
+
+HAL_Bool HAL_GetCounterStopped(HAL_CounterHandle counterHandle,
+ int32_t* status) {
+ auto counter = counterHandles->Get(counterHandle);
+ if (counter == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return false;
+ }
+ return counter->counter->readTimerOutput_Stalled(status);
+}
+
+HAL_Bool HAL_GetCounterDirection(HAL_CounterHandle counterHandle,
+ int32_t* status) {
+ auto counter = counterHandles->Get(counterHandle);
+ if (counter == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return false;
+ }
+ bool value = counter->counter->readOutput_Direction(status);
+ return value;
+}
+
+void HAL_SetCounterReverseDirection(HAL_CounterHandle counterHandle,
+ HAL_Bool reverseDirection,
+ int32_t* status) {
+ auto counter = counterHandles->Get(counterHandle);
+ if (counter == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ if (counter->counter->readConfig_Mode(status) ==
+ HAL_Counter_kExternalDirection) {
+ if (reverseDirection)
+ HAL_SetCounterDownSourceEdge(counterHandle, true, true, status);
+ else
+ HAL_SetCounterDownSourceEdge(counterHandle, false, true, status);
+ }
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/athena/DIO.cpp b/hal/src/main/native/athena/DIO.cpp
new file mode 100644
index 0000000..dc4631e
--- /dev/null
+++ b/hal/src/main/native/athena/DIO.cpp
@@ -0,0 +1,489 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 "hal/DIO.h"
+
+#include <cmath>
+#include <thread>
+
+#include <wpi/raw_ostream.h>
+
+#include "DigitalInternal.h"
+#include "HALInitializer.h"
+#include "PortsInternal.h"
+#include "hal/cpp/fpga_clock.h"
+#include "hal/handles/HandlesInternal.h"
+#include "hal/handles/LimitedHandleResource.h"
+
+using namespace hal;
+
+// Create a mutex to protect changes to the DO PWM config
+static wpi::mutex digitalPwmMutex;
+
+static LimitedHandleResource<HAL_DigitalPWMHandle, uint8_t,
+ kNumDigitalPWMOutputs, HAL_HandleEnum::DigitalPWM>*
+ digitalPWMHandles;
+
+namespace hal {
+namespace init {
+void InitializeDIO() {
+ static LimitedHandleResource<HAL_DigitalPWMHandle, uint8_t,
+ kNumDigitalPWMOutputs,
+ HAL_HandleEnum::DigitalPWM>
+ dpH;
+ digitalPWMHandles = &dpH;
+}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+
+HAL_DigitalHandle HAL_InitializeDIOPort(HAL_PortHandle portHandle,
+ HAL_Bool input, int32_t* status) {
+ hal::init::CheckInit();
+ initializeDigital(status);
+
+ if (*status != 0) return HAL_kInvalidHandle;
+
+ int16_t channel = getPortHandleChannel(portHandle);
+ if (channel == InvalidHandleIndex || channel >= kNumDigitalChannels) {
+ *status = PARAMETER_OUT_OF_RANGE;
+ return HAL_kInvalidHandle;
+ }
+
+ auto handle =
+ digitalChannelHandles->Allocate(channel, HAL_HandleEnum::DIO, status);
+
+ if (*status != 0)
+ return HAL_kInvalidHandle; // failed to allocate. Pass error back.
+
+ auto port = digitalChannelHandles->Get(handle, HAL_HandleEnum::DIO);
+ if (port == nullptr) { // would only occur on thread issue.
+ *status = HAL_HANDLE_ERROR;
+ return HAL_kInvalidHandle;
+ }
+
+ port->channel = static_cast<uint8_t>(channel);
+
+ std::scoped_lock lock(digitalDIOMutex);
+
+ tDIO::tOutputEnable outputEnable = digitalSystem->readOutputEnable(status);
+
+ if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) {
+ if (!getPortHandleSPIEnable(portHandle)) {
+ // if this flag is not set, we actually want DIO.
+ uint32_t bitToSet = 1u << remapSPIChannel(port->channel);
+
+ uint16_t specialFunctions = spiSystem->readEnableDIO(status);
+ // Set the field to enable SPI DIO
+ spiSystem->writeEnableDIO(specialFunctions | bitToSet, status);
+
+ if (input) {
+ outputEnable.SPIPort =
+ outputEnable.SPIPort & (~bitToSet); // clear the field for read
+ } else {
+ outputEnable.SPIPort =
+ outputEnable.SPIPort | bitToSet; // set the bits for write
+ }
+ }
+ } else if (port->channel < kNumDigitalHeaders) {
+ uint32_t bitToSet = 1u << port->channel;
+ if (input) {
+ outputEnable.Headers =
+ outputEnable.Headers & (~bitToSet); // clear the bit for read
+ } else {
+ outputEnable.Headers =
+ outputEnable.Headers | bitToSet; // set the bit for write
+ }
+ } else {
+ uint32_t bitToSet = 1u << remapMXPChannel(port->channel);
+
+ uint16_t specialFunctions =
+ digitalSystem->readEnableMXPSpecialFunction(status);
+ digitalSystem->writeEnableMXPSpecialFunction(specialFunctions & ~bitToSet,
+ status);
+
+ if (input) {
+ outputEnable.MXP =
+ outputEnable.MXP & (~bitToSet); // clear the bit for read
+ } else {
+ outputEnable.MXP = outputEnable.MXP | bitToSet; // set the bit for write
+ }
+ }
+
+ digitalSystem->writeOutputEnable(outputEnable, status);
+
+ return handle;
+}
+
+HAL_Bool HAL_CheckDIOChannel(int32_t channel) {
+ return channel < kNumDigitalChannels && channel >= 0;
+}
+
+void HAL_FreeDIOPort(HAL_DigitalHandle dioPortHandle) {
+ auto port = digitalChannelHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
+ // no status, so no need to check for a proper free.
+ if (port == nullptr) return;
+ digitalChannelHandles->Free(dioPortHandle, HAL_HandleEnum::DIO);
+
+ // Wait for no other object to hold this handle.
+ auto start = hal::fpga_clock::now();
+ while (port.use_count() != 1) {
+ auto current = hal::fpga_clock::now();
+ if (start + std::chrono::seconds(1) < current) {
+ wpi::outs() << "DIO handle free timeout\n";
+ wpi::outs().flush();
+ break;
+ }
+ std::this_thread::yield();
+ }
+
+ int32_t status = 0;
+ std::scoped_lock lock(digitalDIOMutex);
+ if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) {
+ // Unset the SPI flag
+ int32_t bitToUnset = 1 << remapSPIChannel(port->channel);
+ uint16_t specialFunctions = spiSystem->readEnableDIO(&status);
+ spiSystem->writeEnableDIO(specialFunctions & ~bitToUnset, &status);
+ } else if (port->channel >= kNumDigitalHeaders) {
+ // Unset the MXP flag
+ uint32_t bitToUnset = 1u << remapMXPChannel(port->channel);
+
+ uint16_t specialFunctions =
+ digitalSystem->readEnableMXPSpecialFunction(&status);
+ digitalSystem->writeEnableMXPSpecialFunction(specialFunctions | bitToUnset,
+ &status);
+ }
+}
+
+void HAL_SetDIOSimDevice(HAL_DigitalHandle handle, HAL_SimDeviceHandle device) {
+}
+
+HAL_DigitalPWMHandle HAL_AllocateDigitalPWM(int32_t* status) {
+ auto handle = digitalPWMHandles->Allocate();
+ if (handle == HAL_kInvalidHandle) {
+ *status = NO_AVAILABLE_RESOURCES;
+ return HAL_kInvalidHandle;
+ }
+
+ auto id = digitalPWMHandles->Get(handle);
+ if (id == nullptr) { // would only occur on thread issue.
+ *status = HAL_HANDLE_ERROR;
+ return HAL_kInvalidHandle;
+ }
+ *id = static_cast<uint8_t>(getHandleIndex(handle));
+
+ return handle;
+}
+
+void HAL_FreeDigitalPWM(HAL_DigitalPWMHandle pwmGenerator, int32_t* status) {
+ digitalPWMHandles->Free(pwmGenerator);
+}
+
+void HAL_SetDigitalPWMRate(double rate, int32_t* status) {
+ // Currently rounding in the log rate domain... heavy weight toward picking a
+ // higher freq.
+ // TODO: Round in the linear rate domain.
+ initializeDigital(status);
+ if (*status != 0) return;
+ uint16_t pwmPeriodPower = static_cast<uint16_t>(
+ std::log(1.0 / (16 * 1.0E-6 * rate)) / std::log(2.0) + 0.5);
+ digitalSystem->writePWMPeriodPower(pwmPeriodPower, status);
+}
+
+void HAL_SetDigitalPWMDutyCycle(HAL_DigitalPWMHandle pwmGenerator,
+ double dutyCycle, int32_t* status) {
+ auto port = digitalPWMHandles->Get(pwmGenerator);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ int32_t id = *port;
+ if (dutyCycle > 1.0) dutyCycle = 1.0;
+ if (dutyCycle < 0.0) dutyCycle = 0.0;
+ double rawDutyCycle = 256.0 * dutyCycle;
+ if (rawDutyCycle > 255.5) rawDutyCycle = 255.5;
+ {
+ std::scoped_lock lock(digitalPwmMutex);
+ uint16_t pwmPeriodPower = digitalSystem->readPWMPeriodPower(status);
+ if (pwmPeriodPower < 4) {
+ // The resolution of the duty cycle drops close to the highest
+ // frequencies.
+ rawDutyCycle = rawDutyCycle / std::pow(2.0, 4 - pwmPeriodPower);
+ }
+ if (id < 4)
+ digitalSystem->writePWMDutyCycleA(id, static_cast<uint8_t>(rawDutyCycle),
+ status);
+ else
+ digitalSystem->writePWMDutyCycleB(
+ id - 4, static_cast<uint8_t>(rawDutyCycle), status);
+ }
+}
+
+void HAL_SetDigitalPWMOutputChannel(HAL_DigitalPWMHandle pwmGenerator,
+ int32_t channel, int32_t* status) {
+ auto port = digitalPWMHandles->Get(pwmGenerator);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ int32_t id = *port;
+ if (channel >= kNumDigitalHeaders &&
+ channel <
+ kNumDigitalHeaders + kNumDigitalMXPChannels) { // If it is on the MXP
+ /* Then to write as a digital PWM channel an offset is needed to write on
+ * the correct channel
+ */
+ channel += kMXPDigitalPWMOffset;
+ }
+ digitalSystem->writePWMOutputSelect(id, channel, status);
+}
+
+void HAL_SetDIO(HAL_DigitalHandle dioPortHandle, HAL_Bool value,
+ int32_t* status) {
+ auto port = digitalChannelHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ if (value != 0 && value != 1) {
+ if (value != 0) value = 1;
+ }
+ {
+ std::scoped_lock lock(digitalDIOMutex);
+ tDIO::tDO currentDIO = digitalSystem->readDO(status);
+
+ if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) {
+ if (value == 0) {
+ currentDIO.SPIPort =
+ currentDIO.SPIPort & ~(1u << remapSPIChannel(port->channel));
+ } else if (value == 1) {
+ currentDIO.SPIPort =
+ currentDIO.SPIPort | (1u << remapSPIChannel(port->channel));
+ }
+ } else if (port->channel < kNumDigitalHeaders) {
+ if (value == 0) {
+ currentDIO.Headers = currentDIO.Headers & ~(1u << port->channel);
+ } else if (value == 1) {
+ currentDIO.Headers = currentDIO.Headers | (1u << port->channel);
+ }
+ } else {
+ if (value == 0) {
+ currentDIO.MXP =
+ currentDIO.MXP & ~(1u << remapMXPChannel(port->channel));
+ } else if (value == 1) {
+ currentDIO.MXP =
+ currentDIO.MXP | (1u << remapMXPChannel(port->channel));
+ }
+ }
+ digitalSystem->writeDO(currentDIO, status);
+ }
+}
+
+void HAL_SetDIODirection(HAL_DigitalHandle dioPortHandle, HAL_Bool input,
+ int32_t* status) {
+ auto port = digitalChannelHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ {
+ std::scoped_lock lock(digitalDIOMutex);
+ tDIO::tOutputEnable currentDIO = digitalSystem->readOutputEnable(status);
+
+ if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) {
+ if (input) {
+ currentDIO.SPIPort =
+ currentDIO.SPIPort & ~(1u << remapSPIChannel(port->channel));
+ } else {
+ currentDIO.SPIPort =
+ currentDIO.SPIPort | (1u << remapSPIChannel(port->channel));
+ }
+ } else if (port->channel < kNumDigitalHeaders) {
+ if (input) {
+ currentDIO.Headers = currentDIO.Headers & ~(1u << port->channel);
+ } else {
+ currentDIO.Headers = currentDIO.Headers | (1u << port->channel);
+ }
+ } else {
+ if (input) {
+ currentDIO.MXP =
+ currentDIO.MXP & ~(1u << remapMXPChannel(port->channel));
+ } else {
+ currentDIO.MXP =
+ currentDIO.MXP | (1u << remapMXPChannel(port->channel));
+ }
+ }
+ digitalSystem->writeOutputEnable(currentDIO, status);
+ }
+}
+
+HAL_Bool HAL_GetDIO(HAL_DigitalHandle dioPortHandle, int32_t* status) {
+ auto port = digitalChannelHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return false;
+ }
+ tDIO::tDI currentDIO = digitalSystem->readDI(status);
+ // Shift 00000001 over channel-1 places.
+ // AND it against the currentDIO
+ // if it == 0, then return false
+ // else return true
+
+ if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) {
+ return ((currentDIO.SPIPort >> remapSPIChannel(port->channel)) & 1) != 0;
+ } else if (port->channel < kNumDigitalHeaders) {
+ return ((currentDIO.Headers >> port->channel) & 1) != 0;
+ } else {
+ return ((currentDIO.MXP >> remapMXPChannel(port->channel)) & 1) != 0;
+ }
+}
+
+HAL_Bool HAL_GetDIODirection(HAL_DigitalHandle dioPortHandle, int32_t* status) {
+ auto port = digitalChannelHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return false;
+ }
+ tDIO::tOutputEnable currentOutputEnable =
+ digitalSystem->readOutputEnable(status);
+ // Shift 00000001 over port->channel-1 places.
+ // AND it against the currentOutputEnable
+ // if it == 0, then return false
+ // else return true
+
+ if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) {
+ return ((currentOutputEnable.SPIPort >> remapSPIChannel(port->channel)) &
+ 1) != 0;
+ } else if (port->channel < kNumDigitalHeaders) {
+ return ((currentOutputEnable.Headers >> port->channel) & 1) != 0;
+ } else {
+ return ((currentOutputEnable.MXP >> remapMXPChannel(port->channel)) & 1) !=
+ 0;
+ }
+}
+
+void HAL_Pulse(HAL_DigitalHandle dioPortHandle, double pulseLength,
+ int32_t* status) {
+ auto port = digitalChannelHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ tDIO::tPulse pulse;
+
+ if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) {
+ pulse.SPIPort = 1u << remapSPIChannel(port->channel);
+ } else if (port->channel < kNumDigitalHeaders) {
+ pulse.Headers = 1u << port->channel;
+ } else {
+ pulse.MXP = 1u << remapMXPChannel(port->channel);
+ }
+
+ digitalSystem->writePulseLength(
+ static_cast<uint16_t>(1.0e9 * pulseLength /
+ (pwmSystem->readLoopTiming(status) * 25)),
+ status);
+ digitalSystem->writePulse(pulse, status);
+}
+
+HAL_Bool HAL_IsPulsing(HAL_DigitalHandle dioPortHandle, int32_t* status) {
+ auto port = digitalChannelHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return false;
+ }
+ tDIO::tPulse pulseRegister = digitalSystem->readPulse(status);
+
+ if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) {
+ return (pulseRegister.SPIPort & (1 << remapSPIChannel(port->channel))) != 0;
+ } else if (port->channel < kNumDigitalHeaders) {
+ return (pulseRegister.Headers & (1 << port->channel)) != 0;
+ } else {
+ return (pulseRegister.MXP & (1 << remapMXPChannel(port->channel))) != 0;
+ }
+}
+
+HAL_Bool HAL_IsAnyPulsing(int32_t* status) {
+ initializeDigital(status);
+ if (*status != 0) return false;
+ tDIO::tPulse pulseRegister = digitalSystem->readPulse(status);
+ return pulseRegister.Headers != 0 && pulseRegister.MXP != 0 &&
+ pulseRegister.SPIPort != 0;
+}
+
+void HAL_SetFilterSelect(HAL_DigitalHandle dioPortHandle, int32_t filterIndex,
+ int32_t* status) {
+ auto port = digitalChannelHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ std::scoped_lock lock(digitalDIOMutex);
+ if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) {
+ // Channels 10-15 are SPI channels, so subtract our MXP channels
+ digitalSystem->writeFilterSelectHdr(port->channel - kNumDigitalMXPChannels,
+ filterIndex, status);
+ } else if (port->channel < kNumDigitalHeaders) {
+ digitalSystem->writeFilterSelectHdr(port->channel, filterIndex, status);
+ } else {
+ digitalSystem->writeFilterSelectMXP(remapMXPChannel(port->channel),
+ filterIndex, status);
+ }
+}
+
+int32_t HAL_GetFilterSelect(HAL_DigitalHandle dioPortHandle, int32_t* status) {
+ auto port = digitalChannelHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+
+ std::scoped_lock lock(digitalDIOMutex);
+ if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) {
+ // Channels 10-15 are SPI channels, so subtract our MXP channels
+ return digitalSystem->readFilterSelectHdr(
+ port->channel - kNumDigitalMXPChannels, status);
+ } else if (port->channel < kNumDigitalHeaders) {
+ return digitalSystem->readFilterSelectHdr(port->channel, status);
+ } else {
+ return digitalSystem->readFilterSelectMXP(remapMXPChannel(port->channel),
+ status);
+ }
+}
+
+void HAL_SetFilterPeriod(int32_t filterIndex, int64_t value, int32_t* status) {
+ initializeDigital(status);
+ if (*status != 0) return;
+ std::scoped_lock lock(digitalDIOMutex);
+ digitalSystem->writeFilterPeriodHdr(filterIndex, value, status);
+ if (*status == 0) {
+ digitalSystem->writeFilterPeriodMXP(filterIndex, value, status);
+ }
+}
+
+int64_t HAL_GetFilterPeriod(int32_t filterIndex, int32_t* status) {
+ initializeDigital(status);
+ if (*status != 0) return 0;
+ uint32_t hdrPeriod = 0;
+ uint32_t mxpPeriod = 0;
+ {
+ std::scoped_lock lock(digitalDIOMutex);
+ hdrPeriod = digitalSystem->readFilterPeriodHdr(filterIndex, status);
+ if (*status == 0) {
+ mxpPeriod = digitalSystem->readFilterPeriodMXP(filterIndex, status);
+ }
+ }
+ if (hdrPeriod != mxpPeriod) {
+ *status = NiFpga_Status_SoftwareFault;
+ return -1;
+ }
+ return hdrPeriod;
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/athena/DMA.cpp b/hal/src/main/native/athena/DMA.cpp
new file mode 100644
index 0000000..19c3068
--- /dev/null
+++ b/hal/src/main/native/athena/DMA.cpp
@@ -0,0 +1,1006 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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 "hal/DMA.h"
+
+#include <array>
+#include <cstddef>
+#include <cstring>
+#include <memory>
+#include <type_traits>
+
+#include "AnalogInternal.h"
+#include "DigitalInternal.h"
+#include "EncoderInternal.h"
+#include "PortsInternal.h"
+#include "hal/AnalogAccumulator.h"
+//#include "hal/AnalogGyro.h"
+#include "hal/AnalogInput.h"
+#include "hal/ChipObject.h"
+#include "hal/Errors.h"
+#include "hal/HALBase.h"
+#include "hal/handles/HandlesInternal.h"
+#include "hal/handles/LimitedHandleResource.h"
+#include "hal/handles/UnlimitedHandleResource.h"
+
+using namespace hal;
+
+static_assert(std::is_standard_layout_v<HAL_DMASample>,
+ "HAL_DMASample must have standard layout");
+
+namespace {
+
+struct DMA {
+ std::unique_ptr<tDMAManager> manager;
+ std::unique_ptr<tDMA> aDMA;
+
+ HAL_DMASample captureStore;
+};
+} // namespace
+
+static constexpr size_t kChannelSize[22] = {2, 2, 4, 4, 2, 2, 4, 4, 3, 3, 2,
+ 1, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4};
+
+enum DMAOffsetConstants {
+ kEnable_AI0_Low = 0,
+ kEnable_AI0_High = 1,
+ kEnable_AIAveraged0_Low = 2,
+ kEnable_AIAveraged0_High = 3,
+ kEnable_AI1_Low = 4,
+ kEnable_AI1_High = 5,
+ kEnable_AIAveraged1_Low = 6,
+ kEnable_AIAveraged1_High = 7,
+ kEnable_Accumulator0 = 8,
+ kEnable_Accumulator1 = 9,
+ kEnable_DI = 10,
+ kEnable_AnalogTriggers = 11,
+ kEnable_Counters_Low = 12,
+ kEnable_Counters_High = 13,
+ kEnable_CounterTimers_Low = 14,
+ kEnable_CounterTimers_High = 15,
+ kEnable_Encoders_Low = 16,
+ kEnable_Encoders_High = 17,
+ kEnable_EncoderTimers_Low = 18,
+ kEnable_EncoderTimers_High = 19,
+ kEnable_DutyCycle_Low = 20,
+ kEnable_DutyCycle_High = 21,
+};
+
+static hal::LimitedHandleResource<HAL_DMAHandle, DMA, 1, HAL_HandleEnum::DMA>*
+ dmaHandles;
+
+namespace hal {
+namespace init {
+void InitializeDMA() {
+ static hal::LimitedHandleResource<HAL_DMAHandle, DMA, 1, HAL_HandleEnum::DMA>
+ dH;
+ dmaHandles = &dH;
+}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+
+HAL_DMAHandle HAL_InitializeDMA(int32_t* status) {
+ HAL_Handle handle = dmaHandles->Allocate();
+ if (handle == HAL_kInvalidHandle) {
+ *status = NO_AVAILABLE_RESOURCES;
+ return HAL_kInvalidHandle;
+ }
+
+ auto dma = dmaHandles->Get(handle);
+
+ if (!dma) {
+ // Can only happen on thread error
+ *status = HAL_HANDLE_ERROR;
+ return HAL_kInvalidHandle;
+ }
+
+ // Manager does not get created until DMA is started
+ dma->aDMA.reset(tDMA::create(status));
+ if (*status != 0) {
+ dmaHandles->Free(handle);
+ return HAL_kInvalidHandle;
+ }
+
+ dma->aDMA->writeConfig_ExternalClock(false, status);
+ if (*status != 0) {
+ dmaHandles->Free(handle);
+ return HAL_kInvalidHandle;
+ }
+
+ HAL_SetDMARate(handle, 1, status);
+ if (*status != 0) {
+ dmaHandles->Free(handle);
+ return HAL_kInvalidHandle;
+ }
+
+ HAL_SetDMAPause(handle, false, status);
+ return handle;
+}
+
+void HAL_FreeDMA(HAL_DMAHandle handle) {
+ auto dma = dmaHandles->Get(handle);
+ dmaHandles->Free(handle);
+
+ if (!dma) return;
+
+ int32_t status = 0;
+ if (dma->manager) {
+ dma->manager->stop(&status);
+ }
+}
+
+void HAL_SetDMAPause(HAL_DMAHandle handle, HAL_Bool pause, int32_t* status) {
+ auto dma = dmaHandles->Get(handle);
+ if (!dma) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ dma->aDMA->writeConfig_Pause(pause, status);
+}
+void HAL_SetDMARate(HAL_DMAHandle handle, int32_t cycles, int32_t* status) {
+ auto dma = dmaHandles->Get(handle);
+ if (!dma) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (cycles < 1) {
+ cycles = 1;
+ }
+
+ dma->aDMA->writeRate(static_cast<uint32_t>(cycles), status);
+}
+
+void HAL_AddDMAEncoder(HAL_DMAHandle handle, HAL_EncoderHandle encoderHandle,
+ int32_t* status) {
+ // Detect a counter encoder vs an actual encoder, and use the right DMA calls
+ HAL_FPGAEncoderHandle fpgaEncoderHandle = HAL_kInvalidHandle;
+ HAL_CounterHandle counterHandle = HAL_kInvalidHandle;
+
+ bool validEncoderHandle = hal::GetEncoderBaseHandle(
+ encoderHandle, &fpgaEncoderHandle, &counterHandle);
+
+ if (!validEncoderHandle) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (counterHandle != HAL_kInvalidHandle) {
+ HAL_AddDMACounter(handle, counterHandle, status);
+ return;
+ }
+
+ auto dma = dmaHandles->Get(handle);
+ if (!dma) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (dma->manager) {
+ *status = HAL_INVALID_DMA_ADDITION;
+ return;
+ }
+
+ if (getHandleType(fpgaEncoderHandle) != HAL_HandleEnum::FPGAEncoder) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ int32_t index = getHandleIndex(fpgaEncoderHandle);
+ if (index < 0) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (index < 4) {
+ dma->aDMA->writeConfig_Enable_Encoders_Low(true, status);
+ } else if (index < 8) {
+ dma->aDMA->writeConfig_Enable_Encoders_High(true, status);
+ } else {
+ *status = NiFpga_Status_InvalidParameter;
+ }
+}
+
+void HAL_AddDMAEncoderPeriod(HAL_DMAHandle handle,
+ HAL_EncoderHandle encoderHandle, int32_t* status) {
+ // Detect a counter encoder vs an actual encoder, and use the right DMA calls
+ HAL_FPGAEncoderHandle fpgaEncoderHandle = HAL_kInvalidHandle;
+ HAL_CounterHandle counterHandle = HAL_kInvalidHandle;
+
+ bool validEncoderHandle = hal::GetEncoderBaseHandle(
+ encoderHandle, &fpgaEncoderHandle, &counterHandle);
+
+ if (!validEncoderHandle) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (counterHandle != HAL_kInvalidHandle) {
+ HAL_AddDMACounterPeriod(handle, counterHandle, status);
+ return;
+ }
+
+ auto dma = dmaHandles->Get(handle);
+ if (!dma) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (dma->manager) {
+ *status = HAL_INVALID_DMA_ADDITION;
+ return;
+ }
+
+ if (getHandleType(fpgaEncoderHandle) != HAL_HandleEnum::FPGAEncoder) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ int32_t index = getHandleIndex(fpgaEncoderHandle);
+ if (index < 0) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (index < 4) {
+ dma->aDMA->writeConfig_Enable_EncoderTimers_Low(true, status);
+ } else if (index < 8) {
+ dma->aDMA->writeConfig_Enable_EncoderTimers_High(true, status);
+ } else {
+ *status = NiFpga_Status_InvalidParameter;
+ }
+}
+
+void HAL_AddDMACounter(HAL_DMAHandle handle, HAL_CounterHandle counterHandle,
+ int32_t* status) {
+ auto dma = dmaHandles->Get(handle);
+ if (!dma) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (dma->manager) {
+ *status = HAL_INVALID_DMA_ADDITION;
+ return;
+ }
+
+ if (getHandleType(counterHandle) != HAL_HandleEnum::Counter) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ int32_t index = getHandleIndex(counterHandle);
+ if (index < 0) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (index < 4) {
+ dma->aDMA->writeConfig_Enable_Counters_Low(true, status);
+ } else if (index < 8) {
+ dma->aDMA->writeConfig_Enable_Counters_High(true, status);
+ } else {
+ *status = NiFpga_Status_InvalidParameter;
+ }
+}
+
+void HAL_AddDMACounterPeriod(HAL_DMAHandle handle,
+ HAL_CounterHandle counterHandle, int32_t* status) {
+ auto dma = dmaHandles->Get(handle);
+ if (!dma) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (dma->manager) {
+ *status = HAL_INVALID_DMA_ADDITION;
+ return;
+ }
+
+ if (getHandleType(counterHandle) != HAL_HandleEnum::Counter) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ int32_t index = getHandleIndex(counterHandle);
+ if (index < 0) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (index < 4) {
+ dma->aDMA->writeConfig_Enable_CounterTimers_Low(true, status);
+ } else if (index < 8) {
+ dma->aDMA->writeConfig_Enable_CounterTimers_High(true, status);
+ } else {
+ *status = NiFpga_Status_InvalidParameter;
+ }
+}
+
+void HAL_AddDMADigitalSource(HAL_DMAHandle handle,
+ HAL_Handle digitalSourceHandle, int32_t* status) {
+ auto dma = dmaHandles->Get(handle);
+ if (!dma) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (dma->manager) {
+ *status = HAL_INVALID_DMA_ADDITION;
+ return;
+ }
+
+ if (isHandleType(digitalSourceHandle, HAL_HandleEnum::AnalogTrigger)) {
+ dma->aDMA->writeConfig_Enable_AnalogTriggers(true, status);
+ } else if (isHandleType(digitalSourceHandle, HAL_HandleEnum::DIO)) {
+ dma->aDMA->writeConfig_Enable_DI(true, status);
+ } else {
+ *status = NiFpga_Status_InvalidParameter;
+ }
+}
+
+void HAL_AddDMAAnalogInput(HAL_DMAHandle handle,
+ HAL_AnalogInputHandle aInHandle, int32_t* status) {
+ auto dma = dmaHandles->Get(handle);
+ if (!dma) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (dma->manager) {
+ *status = HAL_INVALID_DMA_ADDITION;
+ return;
+ }
+
+ if (getHandleType(aInHandle) != HAL_HandleEnum::AnalogInput) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ int32_t index = getHandleIndex(aInHandle);
+ if (index < 0) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (index < 4) {
+ dma->aDMA->writeConfig_Enable_AI0_Low(true, status);
+ } else if (index < 8) {
+ dma->aDMA->writeConfig_Enable_AI0_High(true, status);
+ } else {
+ *status = NiFpga_Status_InvalidParameter;
+ }
+}
+
+void HAL_AddDMADutyCycle(HAL_DMAHandle handle,
+ HAL_DutyCycleHandle dutyCycleHandle, int32_t* status) {
+ auto dma = dmaHandles->Get(handle);
+ if (!dma) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (dma->manager) {
+ *status = HAL_INVALID_DMA_ADDITION;
+ return;
+ }
+
+ if (getHandleType(dutyCycleHandle) != HAL_HandleEnum::DutyCycle) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ int32_t index = getHandleIndex(dutyCycleHandle);
+ if (index < 0) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (index < 4) {
+ dma->aDMA->writeConfig_Enable_DutyCycle_Low(true, status);
+ } else if (index < 8) {
+ dma->aDMA->writeConfig_Enable_DutyCycle_High(true, status);
+ } else {
+ *status = NiFpga_Status_InvalidParameter;
+ }
+}
+
+void HAL_AddDMAAveragedAnalogInput(HAL_DMAHandle handle,
+ HAL_AnalogInputHandle aInHandle,
+ int32_t* status) {
+ auto dma = dmaHandles->Get(handle);
+ if (!dma) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (dma->manager) {
+ *status = HAL_INVALID_DMA_ADDITION;
+ return;
+ }
+
+ if (getHandleType(aInHandle) != HAL_HandleEnum::AnalogInput) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ int32_t index = getHandleIndex(aInHandle);
+ if (index < 0) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (index < 4) {
+ dma->aDMA->writeConfig_Enable_AIAveraged0_Low(true, status);
+ } else if (index < 8) {
+ dma->aDMA->writeConfig_Enable_AIAveraged0_High(true, status);
+ } else {
+ *status = NiFpga_Status_InvalidParameter;
+ }
+}
+
+void HAL_AddDMAAnalogAccumulator(HAL_DMAHandle handle,
+ HAL_AnalogInputHandle aInHandle,
+ int32_t* status) {
+ auto dma = dmaHandles->Get(handle);
+ if (!dma) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (dma->manager) {
+ *status = HAL_INVALID_DMA_ADDITION;
+ return;
+ }
+
+ if (!HAL_IsAccumulatorChannel(aInHandle, status)) {
+ *status = HAL_INVALID_ACCUMULATOR_CHANNEL;
+ return;
+ }
+
+ int32_t index = getHandleIndex(aInHandle);
+ if (index < 0) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (index == 0) {
+ dma->aDMA->writeConfig_Enable_Accumulator0(true, status);
+ } else if (index == 1) {
+ dma->aDMA->writeConfig_Enable_Accumulator1(true, status);
+ } else {
+ *status = NiFpga_Status_InvalidParameter;
+ }
+}
+
+void HAL_SetDMAExternalTrigger(HAL_DMAHandle handle,
+ HAL_Handle digitalSourceHandle,
+ HAL_AnalogTriggerType analogTriggerType,
+ HAL_Bool rising, HAL_Bool falling,
+ int32_t* status) {
+ auto dma = dmaHandles->Get(handle);
+ if (!dma) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (dma->manager) {
+ *status = HAL_INVALID_DMA_ADDITION;
+ return;
+ }
+
+ int index = 0;
+ auto triggerChannels = dma->captureStore.triggerChannels;
+ do {
+ if (((triggerChannels >> index) & 0x1) == 0) {
+ break;
+ }
+ index++;
+ } while (index < 8);
+
+ if (index == 8) {
+ *status = NO_AVAILABLE_RESOURCES;
+ return;
+ }
+
+ dma->captureStore.triggerChannels |= (1 << index);
+
+ auto channelIndex = index;
+
+ auto isExternalClock = dma->aDMA->readConfig_ExternalClock(status);
+ if (*status == 0 && !isExternalClock) {
+ dma->aDMA->writeConfig_ExternalClock(true, status);
+ if (*status != 0) return;
+ } else if (*status != 0) {
+ return;
+ }
+
+ uint8_t pin = 0;
+ uint8_t module = 0;
+ bool analogTrigger = false;
+ bool success = remapDigitalSource(digitalSourceHandle, analogTriggerType, pin,
+ module, analogTrigger);
+
+ if (!success) {
+ *status = PARAMETER_OUT_OF_RANGE;
+ return;
+ }
+
+ tDMA::tExternalTriggers newTrigger;
+ newTrigger.FallingEdge = falling;
+ newTrigger.RisingEdge = rising;
+ newTrigger.ExternalClockSource_AnalogTrigger = analogTrigger;
+ newTrigger.ExternalClockSource_Channel = pin;
+ newTrigger.ExternalClockSource_Module = module;
+
+ dma->aDMA->writeExternalTriggers(channelIndex / 4, channelIndex % 4,
+ newTrigger, status);
+}
+
+void HAL_StartDMA(HAL_DMAHandle handle, int32_t queueDepth, int32_t* status) {
+ auto dma = dmaHandles->Get(handle);
+ if (!dma) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (dma->manager) {
+ *status = INCOMPATIBLE_STATE;
+ return;
+ }
+
+ tDMA::tConfig config = dma->aDMA->readConfig(status);
+ if (*status != 0) return;
+
+ {
+ size_t accum_size = 0;
+#define SET_SIZE(bit) \
+ if (config.bit) { \
+ dma->captureStore.channelOffsets[k##bit] = accum_size; \
+ accum_size += kChannelSize[k##bit]; \
+ } else { \
+ dma->captureStore.channelOffsets[k##bit] = -1; \
+ }
+ SET_SIZE(Enable_AI0_Low);
+ SET_SIZE(Enable_AI0_High);
+ SET_SIZE(Enable_AIAveraged0_Low);
+ SET_SIZE(Enable_AIAveraged0_High);
+ SET_SIZE(Enable_AI1_Low);
+ SET_SIZE(Enable_AI1_High);
+ SET_SIZE(Enable_AIAveraged1_Low);
+ SET_SIZE(Enable_AIAveraged1_High);
+ SET_SIZE(Enable_Accumulator0);
+ SET_SIZE(Enable_Accumulator1);
+ SET_SIZE(Enable_DI);
+ SET_SIZE(Enable_AnalogTriggers);
+ SET_SIZE(Enable_Counters_Low);
+ SET_SIZE(Enable_Counters_High);
+ SET_SIZE(Enable_CounterTimers_Low);
+ SET_SIZE(Enable_CounterTimers_High);
+ SET_SIZE(Enable_Encoders_Low);
+ SET_SIZE(Enable_Encoders_High);
+ SET_SIZE(Enable_EncoderTimers_Low);
+ SET_SIZE(Enable_EncoderTimers_High);
+ SET_SIZE(Enable_DutyCycle_Low);
+ SET_SIZE(Enable_DutyCycle_High);
+#undef SET_SIZE
+ dma->captureStore.captureSize = accum_size + 1;
+ }
+
+ dma->manager = std::make_unique<tDMAManager>(
+ g_DMA_index, queueDepth * dma->captureStore.captureSize, status);
+ if (*status != 0) {
+ return;
+ }
+
+ dma->manager->start(status);
+ dma->manager->stop(status);
+ dma->manager->start(status);
+}
+
+void HAL_StopDMA(HAL_DMAHandle handle, int32_t* status) {
+ auto dma = dmaHandles->Get(handle);
+ if (!dma) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (dma->manager) {
+ dma->manager->stop(status);
+ dma->manager = nullptr;
+ }
+}
+
+void* HAL_GetDMADirectPointer(HAL_DMAHandle handle) {
+ auto dma = dmaHandles->Get(handle);
+ return dma.get();
+}
+
+enum HAL_DMAReadStatus HAL_ReadDMADirect(void* dmaPointer,
+ HAL_DMASample* dmaSample,
+ int32_t timeoutMs,
+ int32_t* remainingOut,
+ int32_t* status) {
+ DMA* dma = static_cast<DMA*>(dmaPointer);
+ *remainingOut = 0;
+ size_t remainingBytes = 0;
+
+ if (!dma->manager) {
+ *status = INCOMPATIBLE_STATE;
+ return HAL_DMA_ERROR;
+ }
+
+ dma->manager->read(dmaSample->readBuffer, dma->captureStore.captureSize,
+ timeoutMs, &remainingBytes, status);
+
+ *remainingOut = remainingBytes / dma->captureStore.captureSize;
+
+ if (*status == 0) {
+ uint32_t lower_sample =
+ dmaSample->readBuffer[dma->captureStore.captureSize - 1];
+ dmaSample->timeStamp = HAL_ExpandFPGATime(lower_sample, status);
+ if (*status != 0) {
+ return HAL_DMA_ERROR;
+ }
+ dmaSample->triggerChannels = dma->captureStore.triggerChannels;
+ dmaSample->captureSize = dma->captureStore.captureSize;
+ std::memcpy(dmaSample->channelOffsets, dma->captureStore.channelOffsets,
+ sizeof(dmaSample->channelOffsets));
+ return HAL_DMA_OK;
+ } else if (*status == NiFpga_Status_FifoTimeout) {
+ *status = 0;
+ return HAL_DMA_TIMEOUT;
+ } else {
+ return HAL_DMA_ERROR;
+ }
+}
+
+enum HAL_DMAReadStatus HAL_ReadDMA(HAL_DMAHandle handle,
+ HAL_DMASample* dmaSample, int32_t timeoutMs,
+ int32_t* remainingOut, int32_t* status) {
+ auto dma = dmaHandles->Get(handle);
+ if (!dma) {
+ *status = HAL_HANDLE_ERROR;
+ return HAL_DMA_ERROR;
+ }
+
+ return HAL_ReadDMADirect(dma.get(), dmaSample, timeoutMs, remainingOut,
+ status);
+}
+
+static uint32_t ReadDMAValue(const HAL_DMASample& dma, int valueType, int index,
+ int32_t* status) {
+ auto offset = dma.channelOffsets[valueType];
+ if (offset == -1) {
+ *status = NiFpga_Status_ResourceNotFound;
+ return 0;
+ }
+ return dma.readBuffer[offset + index];
+}
+
+uint64_t HAL_GetDMASampleTime(const HAL_DMASample* dmaSample, int32_t* status) {
+ return dmaSample->timeStamp;
+}
+
+int32_t HAL_GetDMASampleEncoderRaw(const HAL_DMASample* dmaSample,
+ HAL_EncoderHandle encoderHandle,
+ int32_t* status) {
+ HAL_FPGAEncoderHandle fpgaEncoderHandle = 0;
+ HAL_CounterHandle counterHandle = 0;
+ bool validEncoderHandle = hal::GetEncoderBaseHandle(
+ encoderHandle, &fpgaEncoderHandle, &counterHandle);
+
+ if (!validEncoderHandle) {
+ *status = HAL_HANDLE_ERROR;
+ return -1;
+ }
+
+ if (counterHandle != HAL_kInvalidHandle) {
+ return HAL_GetDMASampleCounter(dmaSample, counterHandle, status);
+ }
+
+ if (getHandleType(fpgaEncoderHandle) != HAL_HandleEnum::FPGAEncoder) {
+ *status = HAL_HANDLE_ERROR;
+ return -1;
+ }
+
+ int32_t index = getHandleIndex(fpgaEncoderHandle);
+ if (index < 0) {
+ *status = HAL_HANDLE_ERROR;
+ return -1;
+ }
+
+ uint32_t dmaWord = 0;
+ *status = 0;
+ if (index < 4) {
+ dmaWord = ReadDMAValue(*dmaSample, kEnable_Encoders_Low, index, status);
+ } else if (index < 8) {
+ dmaWord =
+ ReadDMAValue(*dmaSample, kEnable_Encoders_High, index - 4, status);
+ } else {
+ *status = NiFpga_Status_ResourceNotFound;
+ }
+ if (*status != 0) {
+ return -1;
+ }
+
+ return static_cast<int32_t>(dmaWord) >> 1;
+}
+
+int32_t HAL_GetDMASampleEncoderPeriodRaw(const HAL_DMASample* dmaSample,
+ HAL_EncoderHandle encoderHandle,
+ int32_t* status) {
+ HAL_FPGAEncoderHandle fpgaEncoderHandle = 0;
+ HAL_CounterHandle counterHandle = 0;
+ bool validEncoderHandle = hal::GetEncoderBaseHandle(
+ encoderHandle, &fpgaEncoderHandle, &counterHandle);
+
+ if (!validEncoderHandle) {
+ *status = HAL_HANDLE_ERROR;
+ return -1;
+ }
+
+ if (counterHandle != HAL_kInvalidHandle) {
+ return HAL_GetDMASampleCounterPeriod(dmaSample, counterHandle, status);
+ }
+
+ if (getHandleType(fpgaEncoderHandle) != HAL_HandleEnum::FPGAEncoder) {
+ *status = HAL_HANDLE_ERROR;
+ return -1;
+ }
+
+ int32_t index = getHandleIndex(fpgaEncoderHandle);
+ if (index < 0) {
+ *status = HAL_HANDLE_ERROR;
+ return -1;
+ }
+
+ uint32_t dmaWord = 0;
+ *status = 0;
+ if (index < 4) {
+ dmaWord =
+ ReadDMAValue(*dmaSample, kEnable_EncoderTimers_Low, index, status);
+ } else if (index < 8) {
+ dmaWord =
+ ReadDMAValue(*dmaSample, kEnable_EncoderTimers_High, index - 4, status);
+ } else {
+ *status = NiFpga_Status_ResourceNotFound;
+ }
+ if (*status != 0) {
+ return -1;
+ }
+
+ return static_cast<int32_t>(dmaWord) & 0x7FFFFF;
+}
+
+int32_t HAL_GetDMASampleCounter(const HAL_DMASample* dmaSample,
+ HAL_CounterHandle counterHandle,
+ int32_t* status) {
+ if (getHandleType(counterHandle) != HAL_HandleEnum::Counter) {
+ *status = HAL_HANDLE_ERROR;
+ return -1;
+ }
+
+ int32_t index = getHandleIndex(counterHandle);
+ if (index < 0) {
+ *status = HAL_HANDLE_ERROR;
+ return -1;
+ }
+
+ uint32_t dmaWord = 0;
+ *status = 0;
+ if (index < 4) {
+ dmaWord = ReadDMAValue(*dmaSample, kEnable_Counters_Low, index, status);
+ } else if (index < 8) {
+ dmaWord =
+ ReadDMAValue(*dmaSample, kEnable_Counters_High, index - 4, status);
+ } else {
+ *status = NiFpga_Status_ResourceNotFound;
+ }
+ if (*status != 0) {
+ return -1;
+ }
+
+ return static_cast<int32_t>(dmaWord) >> 1;
+}
+
+int32_t HAL_GetDMASampleCounterPeriod(const HAL_DMASample* dmaSample,
+ HAL_CounterHandle counterHandle,
+ int32_t* status) {
+ if (getHandleType(counterHandle) != HAL_HandleEnum::Counter) {
+ *status = HAL_HANDLE_ERROR;
+ return -1;
+ }
+
+ int32_t index = getHandleIndex(counterHandle);
+ if (index < 0) {
+ *status = HAL_HANDLE_ERROR;
+ return -1;
+ }
+
+ uint32_t dmaWord = 0;
+ *status = 0;
+ if (index < 4) {
+ dmaWord =
+ ReadDMAValue(*dmaSample, kEnable_CounterTimers_Low, index, status);
+ } else if (index < 8) {
+ dmaWord =
+ ReadDMAValue(*dmaSample, kEnable_CounterTimers_High, index - 4, status);
+ } else {
+ *status = NiFpga_Status_ResourceNotFound;
+ }
+ if (*status != 0) {
+ return -1;
+ }
+
+ return static_cast<int32_t>(dmaWord) & 0x7FFFFF;
+}
+
+HAL_Bool HAL_GetDMASampleDigitalSource(const HAL_DMASample* dmaSample,
+ HAL_Handle dSourceHandle,
+ int32_t* status) {
+ HAL_HandleEnum handleType = getHandleType(dSourceHandle);
+ int32_t index = getHandleIndex(dSourceHandle);
+
+ *status = 0;
+ if (handleType == HAL_HandleEnum::DIO) {
+ auto readVal = ReadDMAValue(*dmaSample, kEnable_DI, 0, status);
+ if (*status == 0) {
+ if (index < kNumDigitalHeaders) {
+ return (readVal >> index) & 0x1;
+ } else {
+ return (readVal >> (index + 6)) & 0x1;
+ }
+ }
+ } else if (handleType == HAL_HandleEnum::AnalogTrigger) {
+ auto readVal = ReadDMAValue(*dmaSample, kEnable_AnalogTriggers, 0, status);
+ if (*status == 0) {
+ return (readVal >> index) & 0x1;
+ }
+ } else {
+ *status = NiFpga_Status_InvalidParameter;
+ }
+ return false;
+}
+int32_t HAL_GetDMASampleAnalogInputRaw(const HAL_DMASample* dmaSample,
+ HAL_AnalogInputHandle aInHandle,
+ int32_t* status) {
+ if (getHandleType(aInHandle) != HAL_HandleEnum::AnalogInput) {
+ *status = HAL_HANDLE_ERROR;
+ return 0xFFFFFFFF;
+ }
+
+ int32_t index = getHandleIndex(aInHandle);
+ if (index < 0) {
+ *status = HAL_HANDLE_ERROR;
+ return 0xFFFFFFFF;
+ }
+
+ uint32_t dmaWord = 0;
+ if (index < 4) {
+ dmaWord = ReadDMAValue(*dmaSample, kEnable_AI0_Low, index / 2, status);
+ } else if (index < 8) {
+ dmaWord =
+ ReadDMAValue(*dmaSample, kEnable_AI0_High, (index - 4) / 2, status);
+ } else {
+ *status = NiFpga_Status_ResourceNotFound;
+ }
+ if (*status != 0) {
+ return 0xFFFFFFFF;
+ }
+
+ if (index % 2) {
+ return (dmaWord >> 16) & 0xffff;
+ } else {
+ return dmaWord & 0xffff;
+ }
+}
+
+int32_t HAL_GetDMASampleAveragedAnalogInputRaw(const HAL_DMASample* dmaSample,
+ HAL_AnalogInputHandle aInHandle,
+ int32_t* status) {
+ if (getHandleType(aInHandle) != HAL_HandleEnum::AnalogInput) {
+ *status = HAL_HANDLE_ERROR;
+ return 0xFFFFFFFF;
+ }
+
+ int32_t index = getHandleIndex(aInHandle);
+ if (index < 0) {
+ *status = HAL_HANDLE_ERROR;
+ return 0xFFFFFFFF;
+ }
+
+ uint32_t dmaWord = 0;
+ if (index < 4) {
+ dmaWord = ReadDMAValue(*dmaSample, kEnable_AIAveraged0_Low, index, status);
+ } else if (index < 8) {
+ dmaWord =
+ ReadDMAValue(*dmaSample, kEnable_AIAveraged0_High, index - 4, status);
+ } else {
+ *status = NiFpga_Status_ResourceNotFound;
+ }
+ if (*status != 0) {
+ return 0xFFFFFFFF;
+ }
+
+ return dmaWord;
+}
+
+void HAL_GetDMASampleAnalogAccumulator(const HAL_DMASample* dmaSample,
+ HAL_AnalogInputHandle aInHandle,
+ int64_t* count, int64_t* value,
+ int32_t* status) {
+ if (!HAL_IsAccumulatorChannel(aInHandle, status)) {
+ *status = HAL_INVALID_ACCUMULATOR_CHANNEL;
+ return;
+ }
+
+ int32_t index = getHandleIndex(aInHandle);
+ if (index < 0) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ uint32_t dmaWord = 0;
+ uint32_t dmaValue1 = 0;
+ uint32_t dmaValue2 = 0;
+ if (index == 0) {
+ dmaWord = ReadDMAValue(*dmaSample, kEnable_Accumulator0, index, status);
+ dmaValue1 =
+ ReadDMAValue(*dmaSample, kEnable_Accumulator0, index + 1, status);
+ dmaValue2 =
+ ReadDMAValue(*dmaSample, kEnable_Accumulator0, index + 2, status);
+ } else if (index == 1) {
+ dmaWord = ReadDMAValue(*dmaSample, kEnable_Accumulator1, index - 1, status);
+ dmaValue1 = ReadDMAValue(*dmaSample, kEnable_Accumulator0, index, status);
+ dmaValue2 =
+ ReadDMAValue(*dmaSample, kEnable_Accumulator0, index + 1, status);
+ } else {
+ *status = NiFpga_Status_ResourceNotFound;
+ }
+ if (*status != 0) {
+ return;
+ }
+
+ *count = dmaWord;
+
+ *value = static_cast<int64_t>(dmaValue1) << 32 | dmaValue2;
+}
+
+int32_t HAL_GetDMASampleDutyCycleOutputRaw(const HAL_DMASample* dmaSample,
+ HAL_DutyCycleHandle dutyCycleHandle,
+ int32_t* status) {
+ if (getHandleType(dutyCycleHandle) != HAL_HandleEnum::DutyCycle) {
+ *status = HAL_HANDLE_ERROR;
+ return -1;
+ }
+
+ int32_t index = getHandleIndex(dutyCycleHandle);
+ if (index < 0) {
+ *status = HAL_HANDLE_ERROR;
+ return -1;
+ }
+
+ uint32_t dmaWord = 0;
+ *status = 0;
+ if (index < 4) {
+ dmaWord = ReadDMAValue(*dmaSample, kEnable_DutyCycle_Low, index, status);
+ } else if (index < 8) {
+ dmaWord =
+ ReadDMAValue(*dmaSample, kEnable_DutyCycle_High, index - 4, status);
+ } else {
+ *status = NiFpga_Status_ResourceNotFound;
+ }
+ if (*status != 0) {
+ return -1;
+ }
+ return dmaWord;
+}
+} // extern "C"
diff --git a/hal/src/main/native/athena/DigitalInternal.cpp b/hal/src/main/native/athena/DigitalInternal.cpp
new file mode 100644
index 0000000..205ce3e
--- /dev/null
+++ b/hal/src/main/native/athena/DigitalInternal.cpp
@@ -0,0 +1,183 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 "DigitalInternal.h"
+
+#include <atomic>
+#include <thread>
+
+#include <FRC_NetworkCommunication/LoadOut.h>
+#include <wpi/mutex.h>
+
+#include "ConstantsInternal.h"
+#include "HALInitializer.h"
+#include "PortsInternal.h"
+#include "hal/AnalogTrigger.h"
+#include "hal/ChipObject.h"
+#include "hal/HAL.h"
+#include "hal/Ports.h"
+#include "hal/cpp/UnsafeDIO.h"
+
+namespace hal {
+
+std::unique_ptr<tDIO> digitalSystem;
+std::unique_ptr<tRelay> relaySystem;
+std::unique_ptr<tPWM> pwmSystem;
+std::unique_ptr<tSPI> spiSystem;
+
+// Create a mutex to protect changes to the digital output values
+wpi::mutex digitalDIOMutex;
+
+DigitalHandleResource<HAL_DigitalHandle, DigitalPort,
+ kNumDigitalChannels + kNumPWMHeaders>*
+ digitalChannelHandles;
+
+namespace init {
+void InitializeDigitalInternal() {
+ static DigitalHandleResource<HAL_DigitalHandle, DigitalPort,
+ kNumDigitalChannels + kNumPWMHeaders>
+ dcH;
+ digitalChannelHandles = &dcH;
+}
+} // namespace init
+
+namespace detail {
+wpi::mutex& UnsafeGetDIOMutex() { return digitalDIOMutex; }
+tDIO* UnsafeGetDigialSystem() { return digitalSystem.get(); }
+int32_t ComputeDigitalMask(HAL_DigitalHandle handle, int32_t* status) {
+ auto port = digitalChannelHandles->Get(handle, HAL_HandleEnum::DIO);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+ tDIO::tDO output;
+ output.value = 0;
+ if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) {
+ output.SPIPort = (1u << remapSPIChannel(port->channel));
+ } else if (port->channel < kNumDigitalHeaders) {
+ output.Headers = (1u << port->channel);
+ } else {
+ output.MXP = (1u << remapMXPChannel(port->channel));
+ }
+ return output.value;
+}
+} // namespace detail
+
+void initializeDigital(int32_t* status) {
+ hal::init::CheckInit();
+ static std::atomic_bool initialized{false};
+ static wpi::mutex initializeMutex;
+ // Initial check, as if it's true initialization has finished
+ if (initialized) return;
+
+ std::scoped_lock lock(initializeMutex);
+ // Second check in case another thread was waiting
+ if (initialized) return;
+
+ digitalSystem.reset(tDIO::create(status));
+
+ // Relay Setup
+ relaySystem.reset(tRelay::create(status));
+
+ // Turn off all relay outputs.
+ relaySystem->writeValue_Forward(0, status);
+ relaySystem->writeValue_Reverse(0, status);
+
+ // PWM Setup
+ pwmSystem.reset(tPWM::create(status));
+
+ // Make sure that the 9403 IONode has had a chance to initialize before
+ // continuing.
+ while (pwmSystem->readLoopTiming(status) == 0) std::this_thread::yield();
+
+ if (pwmSystem->readLoopTiming(status) != kExpectedLoopTiming) {
+ *status = LOOP_TIMING_ERROR; // NOTE: Doesn't display the error
+ }
+
+ // Calculate the length, in ms, of one DIO loop
+ double loopTime = pwmSystem->readLoopTiming(status) /
+ (kSystemClockTicksPerMicrosecond * 1e3);
+
+ pwmSystem->writeConfig_Period(
+ static_cast<uint16_t>(kDefaultPwmPeriod / loopTime + 0.5), status);
+ uint16_t minHigh = static_cast<uint16_t>(
+ (kDefaultPwmCenter - kDefaultPwmStepsDown * loopTime) / loopTime + 0.5);
+ pwmSystem->writeConfig_MinHigh(minHigh, status);
+ // Ensure that PWM output values are set to OFF
+ for (uint8_t pwmIndex = 0; pwmIndex < kNumPWMChannels; pwmIndex++) {
+ // Copy of SetPWM
+ if (pwmIndex < tPWM::kNumHdrRegisters) {
+ pwmSystem->writeHdr(pwmIndex, kPwmDisabled, status);
+ } else {
+ pwmSystem->writeMXP(pwmIndex - tPWM::kNumHdrRegisters, kPwmDisabled,
+ status);
+ }
+
+ // Copy of SetPWMPeriodScale, set to 4x by default.
+ if (pwmIndex < tPWM::kNumPeriodScaleHdrElements) {
+ pwmSystem->writePeriodScaleHdr(pwmIndex, 3, status);
+ } else {
+ pwmSystem->writePeriodScaleMXP(
+ pwmIndex - tPWM::kNumPeriodScaleHdrElements, 3, status);
+ }
+ }
+
+ // SPI setup
+ spiSystem.reset(tSPI::create(status));
+
+ initialized = true;
+}
+
+bool remapDigitalSource(HAL_Handle digitalSourceHandle,
+ HAL_AnalogTriggerType analogTriggerType,
+ uint8_t& channel, uint8_t& module,
+ bool& analogTrigger) {
+ if (isHandleType(digitalSourceHandle, HAL_HandleEnum::AnalogTrigger)) {
+ // If handle passed, index is not negative
+ int32_t index = getHandleIndex(digitalSourceHandle);
+ channel = (index << 2) + analogTriggerType;
+ module = channel >> 4;
+ analogTrigger = true;
+ return true;
+ } else if (isHandleType(digitalSourceHandle, HAL_HandleEnum::DIO)) {
+ int32_t index = getHandleIndex(digitalSourceHandle);
+ if (index >= kNumDigitalHeaders + kNumDigitalMXPChannels) {
+ // channels 10-15, so need to add headers to remap index
+ channel = remapSPIChannel(index) + kNumDigitalHeaders;
+ module = 0;
+ } else if (index >= kNumDigitalHeaders) {
+ channel = remapMXPChannel(index);
+ module = 1;
+ } else {
+ channel = index;
+ module = 0;
+ }
+ analogTrigger = false;
+ return true;
+ } else {
+ return false;
+ }
+}
+
+int32_t remapMXPChannel(int32_t channel) { return channel - 10; }
+
+int32_t remapMXPPWMChannel(int32_t channel) {
+ if (channel < 14) {
+ return channel - 10; // first block of 4 pwms (MXP 0-3)
+ } else {
+ return channel - 6; // block of PWMs after SPI
+ }
+}
+
+int32_t remapSPIChannel(int32_t channel) { return channel - 26; }
+
+} // namespace hal
+
+// Unused function here to test template compile.
+__attribute__((unused)) static void CompileFunctorTest() {
+ hal::UnsafeManipulateDIO(0, nullptr, [](hal::DIOSetProxy& proxy) {});
+}
diff --git a/hal/src/main/native/athena/DigitalInternal.h b/hal/src/main/native/athena/DigitalInternal.h
new file mode 100644
index 0000000..2cb9b3c
--- /dev/null
+++ b/hal/src/main/native/athena/DigitalInternal.h
@@ -0,0 +1,127 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 <stdint.h>
+
+#include <memory>
+
+#include <wpi/mutex.h>
+
+#include "PortsInternal.h"
+#include "hal/AnalogTrigger.h"
+#include "hal/ChipObject.h"
+#include "hal/Ports.h"
+#include "hal/Types.h"
+#include "hal/handles/DigitalHandleResource.h"
+#include "hal/handles/HandlesInternal.h"
+
+namespace hal {
+
+/**
+ * MXP channels when used as digital output PWM are offset from actual value
+ */
+constexpr int32_t kMXPDigitalPWMOffset = 6;
+
+constexpr int32_t kExpectedLoopTiming = 40;
+
+/**
+ * kDefaultPwmPeriod is in ms
+ *
+ * - 20ms periods (50 Hz) are the "safest" setting in that this works for all
+ * devices
+ * - 20ms periods seem to be desirable for Vex Motors
+ * - 20ms periods are the specified period for HS-322HD servos, but work
+ * reliably down to 10.0 ms; starting at about 8.5ms, the servo sometimes hums
+ * and get hot; by 5.0ms the hum is nearly continuous
+ * - 10ms periods work well for Victor 884
+ * - 5ms periods allows higher update rates for Luminary Micro Jaguar speed
+ * controllers. Due to the shipping firmware on the Jaguar, we can't run the
+ * update period less than 5.05 ms.
+ *
+ * kDefaultPwmPeriod is the 1x period (5.05 ms). In hardware, the period
+ * scaling is implemented as an output squelch to get longer periods for old
+ * devices.
+ */
+constexpr double kDefaultPwmPeriod = 5.05;
+/**
+ * kDefaultPwmCenter is the PWM range center in ms
+ */
+constexpr double kDefaultPwmCenter = 1.5;
+/**
+ * kDefaultPWMStepsDown is the number of PWM steps below the centerpoint
+ */
+constexpr int32_t kDefaultPwmStepsDown = 1000;
+constexpr int32_t kPwmDisabled = 0;
+
+extern std::unique_ptr<tDIO> digitalSystem;
+extern std::unique_ptr<tRelay> relaySystem;
+extern std::unique_ptr<tPWM> pwmSystem;
+extern std::unique_ptr<tSPI> spiSystem;
+
+struct DigitalPort {
+ uint8_t channel;
+ bool configSet = false;
+ bool eliminateDeadband = false;
+ int32_t maxPwm = 0;
+ int32_t deadbandMaxPwm = 0;
+ int32_t centerPwm = 0;
+ int32_t deadbandMinPwm = 0;
+ int32_t minPwm = 0;
+};
+
+extern DigitalHandleResource<HAL_DigitalHandle, DigitalPort,
+ kNumDigitalChannels + kNumPWMHeaders>*
+ digitalChannelHandles;
+
+extern wpi::mutex digitalDIOMutex;
+
+/**
+ * Initialize the digital system.
+ */
+void initializeDigital(int32_t* status);
+
+/**
+ * remap the digital source channel and set the module.
+ * If it's an analog trigger, determine the module from the high order routing
+ * channel else do normal digital input remapping based on channel number
+ * (MXP)
+ */
+bool remapDigitalSource(HAL_Handle digitalSourceHandle,
+ HAL_AnalogTriggerType analogTriggerType,
+ uint8_t& channel, uint8_t& module, bool& analogTrigger);
+
+/**
+ * Remap the Digital Channel to map to the bitfield channel of the FPGA
+ */
+constexpr int32_t remapDigitalChannelToBitfieldChannel(int32_t channel) {
+ // First 10 are headers
+ if (channel < kNumDigitalHeaders) return channel;
+ // 2nd group of 16 are mxp. So if mxp port, add 6, since they start at 10
+ else if (channel < kNumDigitalMXPChannels)
+ return channel + 6;
+ // Assume SPI, so remove MXP channels
+ else
+ return channel - kNumDigitalMXPChannels;
+}
+
+/**
+ * Map DIO channel numbers from their physical number (10 to 26) to their
+ * position in the bit field.
+ */
+int32_t remapMXPChannel(int32_t channel);
+
+int32_t remapMXPPWMChannel(int32_t channel);
+
+/**
+ * Map SPI channel numbers from their physical number (27 to 31) to their
+ * position in the bit field.
+ */
+int32_t remapSPIChannel(int32_t channel);
+
+} // namespace hal
diff --git a/hal/src/main/native/athena/DutyCycle.cpp b/hal/src/main/native/athena/DutyCycle.cpp
new file mode 100644
index 0000000..1c1a678
--- /dev/null
+++ b/hal/src/main/native/athena/DutyCycle.cpp
@@ -0,0 +1,124 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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 "hal/DutyCycle.h"
+
+#include <memory>
+
+#include "DigitalInternal.h"
+#include "DutyCycleInternal.h"
+#include "HALInitializer.h"
+#include "PortsInternal.h"
+#include "hal/ChipObject.h"
+#include "hal/Errors.h"
+#include "hal/handles/HandlesInternal.h"
+#include "hal/handles/LimitedHandleResource.h"
+
+using namespace hal;
+
+namespace hal {
+LimitedHandleResource<HAL_DutyCycleHandle, DutyCycle, kNumDutyCycles,
+ HAL_HandleEnum::DutyCycle>* dutyCycleHandles;
+namespace init {
+void InitializeDutyCycle() {
+ static LimitedHandleResource<HAL_DutyCycleHandle, DutyCycle, kNumDutyCycles,
+ HAL_HandleEnum::DutyCycle>
+ dcH;
+ dutyCycleHandles = &dcH;
+}
+} // namespace init
+} // namespace hal
+
+static constexpr int32_t kScaleFactor = 4e7 - 1;
+
+extern "C" {
+HAL_DutyCycleHandle HAL_InitializeDutyCycle(HAL_Handle digitalSourceHandle,
+ HAL_AnalogTriggerType triggerType,
+ int32_t* status) {
+ hal::init::CheckInit();
+
+ bool routingAnalogTrigger = false;
+ uint8_t routingChannel = 0;
+ uint8_t routingModule = 0;
+ bool success =
+ remapDigitalSource(digitalSourceHandle, triggerType, routingChannel,
+ routingModule, routingAnalogTrigger);
+
+ if (!success) {
+ *status = HAL_HANDLE_ERROR;
+ return HAL_kInvalidHandle;
+ }
+
+ HAL_DutyCycleHandle handle = dutyCycleHandles->Allocate();
+ if (handle == HAL_kInvalidHandle) {
+ *status = NO_AVAILABLE_RESOURCES;
+ return HAL_kInvalidHandle;
+ }
+
+ auto dutyCycle = dutyCycleHandles->Get(handle);
+ uint32_t index = static_cast<uint32_t>(getHandleIndex(handle));
+ dutyCycle->dutyCycle.reset(tDutyCycle::create(index, status));
+
+ dutyCycle->dutyCycle->writeSource_AnalogTrigger(routingAnalogTrigger, status);
+ dutyCycle->dutyCycle->writeSource_Channel(routingChannel, status);
+ dutyCycle->dutyCycle->writeSource_Module(routingModule, status);
+ dutyCycle->index = index;
+
+ return handle;
+}
+void HAL_FreeDutyCycle(HAL_DutyCycleHandle dutyCycleHandle) {
+ // Just free it, the unique ptr will take care of everything else
+ dutyCycleHandles->Free(dutyCycleHandle);
+}
+
+int32_t HAL_GetDutyCycleFrequency(HAL_DutyCycleHandle dutyCycleHandle,
+ int32_t* status) {
+ auto dutyCycle = dutyCycleHandles->Get(dutyCycleHandle);
+ if (!dutyCycle) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+
+ // TODO Handle Overflow
+ unsigned char overflow = 0;
+ return dutyCycle->dutyCycle->readFrequency(&overflow, status);
+}
+
+double HAL_GetDutyCycleOutput(HAL_DutyCycleHandle dutyCycleHandle,
+ int32_t* status) {
+ return HAL_GetDutyCycleOutputRaw(dutyCycleHandle, status) /
+ static_cast<double>(kScaleFactor);
+}
+
+int32_t HAL_GetDutyCycleOutputRaw(HAL_DutyCycleHandle dutyCycleHandle,
+ int32_t* status) {
+ auto dutyCycle = dutyCycleHandles->Get(dutyCycleHandle);
+ if (!dutyCycle) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+
+ // TODO Handle Overflow
+ unsigned char overflow = 0;
+ return dutyCycle->dutyCycle->readOutput(&overflow, status);
+}
+
+int32_t HAL_GetDutyCycleOutputScaleFactor(HAL_DutyCycleHandle dutyCycleHandle,
+ int32_t* status) {
+ return kScaleFactor;
+}
+
+int32_t HAL_GetDutyCycleFPGAIndex(HAL_DutyCycleHandle dutyCycleHandle,
+ int32_t* status) {
+ auto dutyCycle = dutyCycleHandles->Get(dutyCycleHandle);
+ if (!dutyCycle) {
+ *status = HAL_HANDLE_ERROR;
+ return -1;
+ }
+ return dutyCycle->index;
+}
+} // extern "C"
diff --git a/hal/src/main/native/athena/DutyCycleInternal.h b/hal/src/main/native/athena/DutyCycleInternal.h
new file mode 100644
index 0000000..33a8ff2
--- /dev/null
+++ b/hal/src/main/native/athena/DutyCycleInternal.h
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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 <memory>
+
+#include "hal/ChipObject.h"
+#include "hal/handles/HandlesInternal.h"
+#include "hal/handles/LimitedHandleResource.h"
+
+namespace hal {
+struct DutyCycle {
+ std::unique_ptr<tDutyCycle> dutyCycle;
+ int index;
+};
+
+extern LimitedHandleResource<HAL_DutyCycleHandle, DutyCycle, kNumDutyCycles,
+ HAL_HandleEnum::DutyCycle>* dutyCycleHandles;
+} // namespace hal
diff --git a/hal/src/main/native/athena/Encoder.cpp b/hal/src/main/native/athena/Encoder.cpp
new file mode 100644
index 0000000..be8c203
--- /dev/null
+++ b/hal/src/main/native/athena/Encoder.cpp
@@ -0,0 +1,478 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 "hal/Encoder.h"
+
+#include "EncoderInternal.h"
+#include "FPGAEncoder.h"
+#include "HALInitializer.h"
+#include "PortsInternal.h"
+#include "hal/ChipObject.h"
+#include "hal/Counter.h"
+#include "hal/Errors.h"
+#include "hal/handles/LimitedClassedHandleResource.h"
+
+using namespace hal;
+
+Encoder::Encoder(HAL_Handle digitalSourceHandleA,
+ HAL_AnalogTriggerType analogTriggerTypeA,
+ HAL_Handle digitalSourceHandleB,
+ HAL_AnalogTriggerType analogTriggerTypeB,
+ bool reverseDirection, HAL_EncoderEncodingType encodingType,
+ int32_t* status) {
+ m_encodingType = encodingType;
+ switch (encodingType) {
+ case HAL_Encoder_k4X: {
+ m_encodingScale = 4;
+ m_encoder = HAL_InitializeFPGAEncoder(
+ digitalSourceHandleA, analogTriggerTypeA, digitalSourceHandleB,
+ analogTriggerTypeB, reverseDirection, &m_index, status);
+ if (*status != 0) {
+ return;
+ }
+ m_counter = HAL_kInvalidHandle;
+ SetMaxPeriod(0.5, status);
+ break;
+ }
+ case HAL_Encoder_k1X:
+ case HAL_Encoder_k2X: {
+ SetupCounter(digitalSourceHandleA, analogTriggerTypeA,
+ digitalSourceHandleB, analogTriggerTypeB, reverseDirection,
+ encodingType, status);
+
+ m_encodingScale = encodingType == HAL_Encoder_k1X ? 1 : 2;
+ break;
+ }
+ default:
+ *status = PARAMETER_OUT_OF_RANGE;
+ return;
+ }
+}
+
+void Encoder::SetupCounter(HAL_Handle digitalSourceHandleA,
+ HAL_AnalogTriggerType analogTriggerTypeA,
+ HAL_Handle digitalSourceHandleB,
+ HAL_AnalogTriggerType analogTriggerTypeB,
+ bool reverseDirection,
+ HAL_EncoderEncodingType encodingType,
+ int32_t* status) {
+ m_encodingScale = encodingType == HAL_Encoder_k1X ? 1 : 2;
+ m_counter =
+ HAL_InitializeCounter(HAL_Counter_kExternalDirection, &m_index, status);
+ if (*status != 0) return;
+ HAL_SetCounterMaxPeriod(m_counter, 0.5, status);
+ if (*status != 0) return;
+ HAL_SetCounterUpSource(m_counter, digitalSourceHandleA, analogTriggerTypeA,
+ status);
+ if (*status != 0) return;
+ HAL_SetCounterDownSource(m_counter, digitalSourceHandleB, analogTriggerTypeB,
+ status);
+ if (*status != 0) return;
+ if (encodingType == HAL_Encoder_k1X) {
+ HAL_SetCounterUpSourceEdge(m_counter, true, false, status);
+ HAL_SetCounterAverageSize(m_counter, 1, status);
+ } else {
+ HAL_SetCounterUpSourceEdge(m_counter, true, true, status);
+ HAL_SetCounterAverageSize(m_counter, 2, status);
+ }
+ HAL_SetCounterDownSourceEdge(m_counter, reverseDirection, true, status);
+}
+
+Encoder::~Encoder() {
+ if (m_counter != HAL_kInvalidHandle) {
+ int32_t status = 0;
+ HAL_FreeCounter(m_counter, &status);
+ } else {
+ int32_t status = 0;
+ HAL_FreeFPGAEncoder(m_encoder, &status);
+ }
+}
+
+// CounterBase interface
+int32_t Encoder::Get(int32_t* status) const {
+ return static_cast<int32_t>(GetRaw(status) * DecodingScaleFactor());
+}
+
+int32_t Encoder::GetRaw(int32_t* status) const {
+ if (m_counter) {
+ return HAL_GetCounter(m_counter, status);
+ } else {
+ return HAL_GetFPGAEncoder(m_encoder, status);
+ }
+}
+
+int32_t Encoder::GetEncodingScale(int32_t* status) const {
+ return m_encodingScale;
+}
+
+void Encoder::Reset(int32_t* status) {
+ if (m_counter) {
+ HAL_ResetCounter(m_counter, status);
+ } else {
+ HAL_ResetFPGAEncoder(m_encoder, status);
+ }
+}
+
+double Encoder::GetPeriod(int32_t* status) const {
+ if (m_counter) {
+ return HAL_GetCounterPeriod(m_counter, status) / DecodingScaleFactor();
+ } else {
+ return HAL_GetFPGAEncoderPeriod(m_encoder, status);
+ }
+}
+
+void Encoder::SetMaxPeriod(double maxPeriod, int32_t* status) {
+ if (m_counter) {
+ HAL_SetCounterMaxPeriod(m_counter, maxPeriod, status);
+ } else {
+ HAL_SetFPGAEncoderMaxPeriod(m_encoder, maxPeriod, status);
+ }
+}
+
+bool Encoder::GetStopped(int32_t* status) const {
+ if (m_counter) {
+ return HAL_GetCounterStopped(m_counter, status);
+ } else {
+ return HAL_GetFPGAEncoderStopped(m_encoder, status);
+ }
+}
+
+bool Encoder::GetDirection(int32_t* status) const {
+ if (m_counter) {
+ return HAL_GetCounterDirection(m_counter, status);
+ } else {
+ return HAL_GetFPGAEncoderDirection(m_encoder, status);
+ }
+}
+
+double Encoder::GetDistance(int32_t* status) const {
+ return GetRaw(status) * DecodingScaleFactor() * m_distancePerPulse;
+}
+
+double Encoder::GetRate(int32_t* status) const {
+ return m_distancePerPulse / GetPeriod(status);
+}
+
+void Encoder::SetMinRate(double minRate, int32_t* status) {
+ SetMaxPeriod(m_distancePerPulse / minRate, status);
+}
+
+void Encoder::SetDistancePerPulse(double distancePerPulse, int32_t* status) {
+ m_distancePerPulse = distancePerPulse;
+}
+
+void Encoder::SetReverseDirection(bool reverseDirection, int32_t* status) {
+ if (m_counter) {
+ HAL_SetCounterReverseDirection(m_counter, reverseDirection, status);
+ } else {
+ HAL_SetFPGAEncoderReverseDirection(m_encoder, reverseDirection, status);
+ }
+}
+
+void Encoder::SetSamplesToAverage(int32_t samplesToAverage, int32_t* status) {
+ if (samplesToAverage < 1 || samplesToAverage > 127) {
+ *status = PARAMETER_OUT_OF_RANGE;
+ return;
+ }
+ if (m_counter) {
+ HAL_SetCounterSamplesToAverage(m_counter, samplesToAverage, status);
+ } else {
+ HAL_SetFPGAEncoderSamplesToAverage(m_encoder, samplesToAverage, status);
+ }
+}
+
+int32_t Encoder::GetSamplesToAverage(int32_t* status) const {
+ if (m_counter) {
+ return HAL_GetCounterSamplesToAverage(m_counter, status);
+ } else {
+ return HAL_GetFPGAEncoderSamplesToAverage(m_encoder, status);
+ }
+}
+
+void Encoder::SetIndexSource(HAL_Handle digitalSourceHandle,
+ HAL_AnalogTriggerType analogTriggerType,
+ HAL_EncoderIndexingType type, int32_t* status) {
+ if (m_counter) {
+ *status = HAL_COUNTER_NOT_SUPPORTED;
+ return;
+ }
+ bool activeHigh =
+ (type == HAL_kResetWhileHigh) || (type == HAL_kResetOnRisingEdge);
+ bool edgeSensitive =
+ (type == HAL_kResetOnFallingEdge) || (type == HAL_kResetOnRisingEdge);
+ HAL_SetFPGAEncoderIndexSource(m_encoder, digitalSourceHandle,
+ analogTriggerType, activeHigh, edgeSensitive,
+ status);
+}
+
+double Encoder::DecodingScaleFactor() const {
+ switch (m_encodingType) {
+ case HAL_Encoder_k1X:
+ return 1.0;
+ case HAL_Encoder_k2X:
+ return 0.5;
+ case HAL_Encoder_k4X:
+ return 0.25;
+ default:
+ return 0.0;
+ }
+}
+
+static LimitedClassedHandleResource<HAL_EncoderHandle, Encoder,
+ kNumEncoders + kNumCounters,
+ HAL_HandleEnum::Encoder>* encoderHandles;
+
+namespace hal {
+namespace init {
+void InitializeEncoder() {
+ static LimitedClassedHandleResource<HAL_EncoderHandle, Encoder,
+ kNumEncoders + kNumCounters,
+ HAL_HandleEnum::Encoder>
+ eH;
+ encoderHandles = &eH;
+}
+} // namespace init
+} // namespace hal
+
+namespace hal {
+bool GetEncoderBaseHandle(HAL_EncoderHandle handle,
+ HAL_FPGAEncoderHandle* fpgaHandle,
+ HAL_CounterHandle* counterHandle) {
+ auto encoder = encoderHandles->Get(handle);
+ if (!handle) return false;
+
+ *fpgaHandle = encoder->m_encoder;
+ *counterHandle = encoder->m_counter;
+ return true;
+}
+} // namespace hal
+
+extern "C" {
+HAL_EncoderHandle HAL_InitializeEncoder(
+ HAL_Handle digitalSourceHandleA, HAL_AnalogTriggerType analogTriggerTypeA,
+ HAL_Handle digitalSourceHandleB, HAL_AnalogTriggerType analogTriggerTypeB,
+ HAL_Bool reverseDirection, HAL_EncoderEncodingType encodingType,
+ int32_t* status) {
+ hal::init::CheckInit();
+ auto encoder = std::make_shared<Encoder>(
+ digitalSourceHandleA, analogTriggerTypeA, digitalSourceHandleB,
+ analogTriggerTypeB, reverseDirection, encodingType, status);
+ if (*status != 0) return HAL_kInvalidHandle; // return in creation error
+ auto handle = encoderHandles->Allocate(encoder);
+ if (handle == HAL_kInvalidHandle) {
+ *status = NO_AVAILABLE_RESOURCES;
+ return HAL_kInvalidHandle;
+ }
+ return handle;
+}
+
+void HAL_FreeEncoder(HAL_EncoderHandle encoderHandle, int32_t* status) {
+ encoderHandles->Free(encoderHandle);
+}
+
+void HAL_SetEncoderSimDevice(HAL_EncoderHandle handle,
+ HAL_SimDeviceHandle device) {}
+
+int32_t HAL_GetEncoder(HAL_EncoderHandle encoderHandle, int32_t* status) {
+ auto encoder = encoderHandles->Get(encoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+ return encoder->Get(status);
+}
+
+int32_t HAL_GetEncoderRaw(HAL_EncoderHandle encoderHandle, int32_t* status) {
+ auto encoder = encoderHandles->Get(encoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+ return encoder->GetRaw(status);
+}
+
+int32_t HAL_GetEncoderEncodingScale(HAL_EncoderHandle encoderHandle,
+ int32_t* status) {
+ auto encoder = encoderHandles->Get(encoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+ return encoder->GetEncodingScale(status);
+}
+
+void HAL_ResetEncoder(HAL_EncoderHandle encoderHandle, int32_t* status) {
+ auto encoder = encoderHandles->Get(encoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ encoder->Reset(status);
+}
+
+double HAL_GetEncoderPeriod(HAL_EncoderHandle encoderHandle, int32_t* status) {
+ auto encoder = encoderHandles->Get(encoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+ return encoder->GetPeriod(status);
+}
+
+void HAL_SetEncoderMaxPeriod(HAL_EncoderHandle encoderHandle, double maxPeriod,
+ int32_t* status) {
+ auto encoder = encoderHandles->Get(encoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ encoder->SetMaxPeriod(maxPeriod, status);
+}
+
+HAL_Bool HAL_GetEncoderStopped(HAL_EncoderHandle encoderHandle,
+ int32_t* status) {
+ auto encoder = encoderHandles->Get(encoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+ return encoder->GetStopped(status);
+}
+
+HAL_Bool HAL_GetEncoderDirection(HAL_EncoderHandle encoderHandle,
+ int32_t* status) {
+ auto encoder = encoderHandles->Get(encoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+ return encoder->GetDirection(status);
+}
+
+double HAL_GetEncoderDistance(HAL_EncoderHandle encoderHandle,
+ int32_t* status) {
+ auto encoder = encoderHandles->Get(encoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+ return encoder->GetDistance(status);
+}
+
+double HAL_GetEncoderRate(HAL_EncoderHandle encoderHandle, int32_t* status) {
+ auto encoder = encoderHandles->Get(encoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+ return encoder->GetRate(status);
+}
+
+void HAL_SetEncoderMinRate(HAL_EncoderHandle encoderHandle, double minRate,
+ int32_t* status) {
+ auto encoder = encoderHandles->Get(encoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ encoder->SetMinRate(minRate, status);
+}
+
+void HAL_SetEncoderDistancePerPulse(HAL_EncoderHandle encoderHandle,
+ double distancePerPulse, int32_t* status) {
+ auto encoder = encoderHandles->Get(encoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ encoder->SetDistancePerPulse(distancePerPulse, status);
+}
+
+void HAL_SetEncoderReverseDirection(HAL_EncoderHandle encoderHandle,
+ HAL_Bool reverseDirection,
+ int32_t* status) {
+ auto encoder = encoderHandles->Get(encoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ encoder->SetReverseDirection(reverseDirection, status);
+}
+
+void HAL_SetEncoderSamplesToAverage(HAL_EncoderHandle encoderHandle,
+ int32_t samplesToAverage, int32_t* status) {
+ auto encoder = encoderHandles->Get(encoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ encoder->SetSamplesToAverage(samplesToAverage, status);
+}
+
+int32_t HAL_GetEncoderSamplesToAverage(HAL_EncoderHandle encoderHandle,
+ int32_t* status) {
+ auto encoder = encoderHandles->Get(encoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+ return encoder->GetSamplesToAverage(status);
+}
+
+double HAL_GetEncoderDecodingScaleFactor(HAL_EncoderHandle encoderHandle,
+ int32_t* status) {
+ auto encoder = encoderHandles->Get(encoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+ return encoder->DecodingScaleFactor();
+}
+
+double HAL_GetEncoderDistancePerPulse(HAL_EncoderHandle encoderHandle,
+ int32_t* status) {
+ auto encoder = encoderHandles->Get(encoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+ return encoder->GetDistancePerPulse();
+}
+
+HAL_EncoderEncodingType HAL_GetEncoderEncodingType(
+ HAL_EncoderHandle encoderHandle, int32_t* status) {
+ auto encoder = encoderHandles->Get(encoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return HAL_Encoder_k4X; // default to k4X
+ }
+ return encoder->GetEncodingType();
+}
+
+void HAL_SetEncoderIndexSource(HAL_EncoderHandle encoderHandle,
+ HAL_Handle digitalSourceHandle,
+ HAL_AnalogTriggerType analogTriggerType,
+ HAL_EncoderIndexingType type, int32_t* status) {
+ auto encoder = encoderHandles->Get(encoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ encoder->SetIndexSource(digitalSourceHandle, analogTriggerType, type, status);
+}
+
+int32_t HAL_GetEncoderFPGAIndex(HAL_EncoderHandle encoderHandle,
+ int32_t* status) {
+ auto encoder = encoderHandles->Get(encoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+ return encoder->GetFPGAIndex();
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/athena/EncoderInternal.h b/hal/src/main/native/athena/EncoderInternal.h
new file mode 100644
index 0000000..bed4ee3
--- /dev/null
+++ b/hal/src/main/native/athena/EncoderInternal.h
@@ -0,0 +1,86 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 <stdint.h>
+
+#include "hal/Encoder.h"
+
+namespace hal {
+
+bool GetEncoderBaseHandle(HAL_EncoderHandle handle,
+ HAL_FPGAEncoderHandle* fpgaEncoderHandle,
+ HAL_CounterHandle* counterHandle);
+
+class Encoder {
+ public:
+ friend bool GetEncoderBaseHandle(HAL_EncoderHandle handle,
+ HAL_FPGAEncoderHandle* fpgaEncoderHandle,
+ HAL_CounterHandle* counterHandle);
+
+ Encoder(HAL_Handle digitalSourceHandleA,
+ HAL_AnalogTriggerType analogTriggerTypeA,
+ HAL_Handle digitalSourceHandleB,
+ HAL_AnalogTriggerType analogTriggerTypeB, bool reverseDirection,
+ HAL_EncoderEncodingType encodingType, int32_t* status);
+ ~Encoder();
+
+ // CounterBase interface
+ int32_t Get(int32_t* status) const;
+ int32_t GetRaw(int32_t* status) const;
+ int32_t GetEncodingScale(int32_t* status) const;
+ void Reset(int32_t* status);
+ double GetPeriod(int32_t* status) const;
+ void SetMaxPeriod(double maxPeriod, int32_t* status);
+ bool GetStopped(int32_t* status) const;
+ bool GetDirection(int32_t* status) const;
+
+ double GetDistance(int32_t* status) const;
+ double GetRate(int32_t* status) const;
+ void SetMinRate(double minRate, int32_t* status);
+ void SetDistancePerPulse(double distancePerPulse, int32_t* status);
+ void SetReverseDirection(bool reverseDirection, int32_t* status);
+ void SetSamplesToAverage(int32_t samplesToAverage, int32_t* status);
+ int32_t GetSamplesToAverage(int32_t* status) const;
+
+ void SetIndexSource(HAL_Handle digitalSourceHandle,
+ HAL_AnalogTriggerType analogTriggerType,
+ HAL_EncoderIndexingType type, int32_t* status);
+
+ int32_t GetFPGAIndex() const { return m_index; }
+
+ int32_t GetEncodingScale() const { return m_encodingScale; }
+
+ double DecodingScaleFactor() const;
+
+ double GetDistancePerPulse() const { return m_distancePerPulse; }
+
+ HAL_EncoderEncodingType GetEncodingType() const { return m_encodingType; }
+
+ private:
+ void SetupCounter(HAL_Handle digitalSourceHandleA,
+ HAL_AnalogTriggerType analogTriggerTypeA,
+ HAL_Handle digitalSourceHandleB,
+ HAL_AnalogTriggerType analogTriggerTypeB,
+ bool reverseDirection, HAL_EncoderEncodingType encodingType,
+ int32_t* status);
+
+ HAL_FPGAEncoderHandle m_encoder = HAL_kInvalidHandle;
+
+ HAL_CounterHandle m_counter = HAL_kInvalidHandle;
+
+ int32_t m_index = 0;
+
+ double m_distancePerPulse = 1.0;
+
+ HAL_EncoderEncodingType m_encodingType;
+
+ int32_t m_encodingScale;
+};
+
+} // namespace hal
diff --git a/hal/src/main/native/athena/FPGAEncoder.cpp b/hal/src/main/native/athena/FPGAEncoder.cpp
new file mode 100644
index 0000000..7f2e107
--- /dev/null
+++ b/hal/src/main/native/athena/FPGAEncoder.cpp
@@ -0,0 +1,245 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 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 "FPGAEncoder.h"
+
+#include <memory>
+
+#include "DigitalInternal.h"
+#include "HALInitializer.h"
+#include "PortsInternal.h"
+#include "hal/handles/LimitedHandleResource.h"
+
+using namespace hal;
+
+namespace {
+
+struct Encoder {
+ std::unique_ptr<tEncoder> encoder;
+ uint8_t index;
+};
+
+} // namespace
+
+static constexpr double DECODING_SCALING_FACTOR = 0.25;
+
+static LimitedHandleResource<HAL_FPGAEncoderHandle, Encoder, kNumEncoders,
+ HAL_HandleEnum::FPGAEncoder>* fpgaEncoderHandles;
+
+namespace hal {
+namespace init {
+void InitializeFPGAEncoder() {
+ static LimitedHandleResource<HAL_FPGAEncoderHandle, Encoder, kNumEncoders,
+ HAL_HandleEnum::FPGAEncoder>
+ feH;
+ fpgaEncoderHandles = &feH;
+}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+
+HAL_FPGAEncoderHandle HAL_InitializeFPGAEncoder(
+ HAL_Handle digitalSourceHandleA, HAL_AnalogTriggerType analogTriggerTypeA,
+ HAL_Handle digitalSourceHandleB, HAL_AnalogTriggerType analogTriggerTypeB,
+ HAL_Bool reverseDirection, int32_t* index, int32_t* status) {
+ hal::init::CheckInit();
+ bool routingAnalogTriggerA = false;
+ uint8_t routingChannelA = 0;
+ uint8_t routingModuleA = 0;
+ bool successA = remapDigitalSource(digitalSourceHandleA, analogTriggerTypeA,
+ routingChannelA, routingModuleA,
+ routingAnalogTriggerA);
+ bool routingAnalogTriggerB = false;
+ uint8_t routingChannelB = 0;
+ uint8_t routingModuleB = 0;
+ bool successB = remapDigitalSource(digitalSourceHandleB, analogTriggerTypeB,
+ routingChannelB, routingModuleB,
+ routingAnalogTriggerB);
+
+ if (!successA || !successB) {
+ *status = HAL_HANDLE_ERROR;
+ return HAL_kInvalidHandle;
+ }
+
+ auto handle = fpgaEncoderHandles->Allocate();
+ if (handle == HAL_kInvalidHandle) { // out of resources
+ *status = NO_AVAILABLE_RESOURCES;
+ return HAL_kInvalidHandle;
+ }
+
+ auto encoder = fpgaEncoderHandles->Get(handle);
+ if (encoder == nullptr) { // will only error on thread issue
+ *status = HAL_HANDLE_ERROR;
+ return HAL_kInvalidHandle;
+ }
+
+ encoder->index = static_cast<uint8_t>(getHandleIndex(handle));
+ *index = encoder->index;
+ // TODO: if (index == ~0ul) { CloneError(quadEncoders); return; }
+ encoder->encoder.reset(tEncoder::create(encoder->index, status));
+ encoder->encoder->writeConfig_ASource_Module(routingModuleA, status);
+ encoder->encoder->writeConfig_ASource_Channel(routingChannelA, status);
+ encoder->encoder->writeConfig_ASource_AnalogTrigger(routingAnalogTriggerA,
+ status);
+ encoder->encoder->writeConfig_BSource_Module(routingModuleB, status);
+ encoder->encoder->writeConfig_BSource_Channel(routingChannelB, status);
+ encoder->encoder->writeConfig_BSource_AnalogTrigger(routingAnalogTriggerB,
+ status);
+ encoder->encoder->strobeReset(status);
+ encoder->encoder->writeConfig_Reverse(reverseDirection, status);
+ encoder->encoder->writeTimerConfig_AverageSize(4, status);
+
+ return handle;
+}
+
+void HAL_FreeFPGAEncoder(HAL_FPGAEncoderHandle fpgaEncoderHandle,
+ int32_t* status) {
+ fpgaEncoderHandles->Free(fpgaEncoderHandle);
+}
+
+void HAL_ResetFPGAEncoder(HAL_FPGAEncoderHandle fpgaEncoderHandle,
+ int32_t* status) {
+ auto encoder = fpgaEncoderHandles->Get(fpgaEncoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ encoder->encoder->strobeReset(status);
+}
+
+int32_t HAL_GetFPGAEncoder(HAL_FPGAEncoderHandle fpgaEncoderHandle,
+ int32_t* status) {
+ auto encoder = fpgaEncoderHandles->Get(fpgaEncoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+ return encoder->encoder->readOutput_Value(status);
+}
+
+double HAL_GetFPGAEncoderPeriod(HAL_FPGAEncoderHandle fpgaEncoderHandle,
+ int32_t* status) {
+ auto encoder = fpgaEncoderHandles->Get(fpgaEncoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0.0;
+ }
+ tEncoder::tTimerOutput output = encoder->encoder->readTimerOutput(status);
+ double value;
+ if (output.Stalled) {
+ // Return infinity
+ double zero = 0.0;
+ value = 1.0 / zero;
+ } else {
+ // output.Period is a fixed point number that counts by 2 (24 bits, 25
+ // integer bits)
+ value = static_cast<double>(output.Period << 1) /
+ static_cast<double>(output.Count);
+ }
+ double measuredPeriod = value * 2.5e-8;
+ return measuredPeriod / DECODING_SCALING_FACTOR;
+}
+
+void HAL_SetFPGAEncoderMaxPeriod(HAL_FPGAEncoderHandle fpgaEncoderHandle,
+ double maxPeriod, int32_t* status) {
+ auto encoder = fpgaEncoderHandles->Get(fpgaEncoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ encoder->encoder->writeTimerConfig_StallPeriod(
+ static_cast<uint32_t>(maxPeriod * 4.0e8 * DECODING_SCALING_FACTOR),
+ status);
+}
+
+HAL_Bool HAL_GetFPGAEncoderStopped(HAL_FPGAEncoderHandle fpgaEncoderHandle,
+ int32_t* status) {
+ auto encoder = fpgaEncoderHandles->Get(fpgaEncoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return false;
+ }
+ return encoder->encoder->readTimerOutput_Stalled(status) != 0;
+}
+
+HAL_Bool HAL_GetFPGAEncoderDirection(HAL_FPGAEncoderHandle fpgaEncoderHandle,
+ int32_t* status) {
+ auto encoder = fpgaEncoderHandles->Get(fpgaEncoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return false;
+ }
+ return encoder->encoder->readOutput_Direction(status);
+}
+
+void HAL_SetFPGAEncoderReverseDirection(HAL_FPGAEncoderHandle fpgaEncoderHandle,
+ HAL_Bool reverseDirection,
+ int32_t* status) {
+ auto encoder = fpgaEncoderHandles->Get(fpgaEncoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ encoder->encoder->writeConfig_Reverse(reverseDirection, status);
+}
+
+void HAL_SetFPGAEncoderSamplesToAverage(HAL_FPGAEncoderHandle fpgaEncoderHandle,
+ int32_t samplesToAverage,
+ int32_t* status) {
+ auto encoder = fpgaEncoderHandles->Get(fpgaEncoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ if (samplesToAverage < 1 || samplesToAverage > 127) {
+ *status = PARAMETER_OUT_OF_RANGE;
+ }
+ encoder->encoder->writeTimerConfig_AverageSize(samplesToAverage, status);
+}
+
+int32_t HAL_GetFPGAEncoderSamplesToAverage(
+ HAL_FPGAEncoderHandle fpgaEncoderHandle, int32_t* status) {
+ auto encoder = fpgaEncoderHandles->Get(fpgaEncoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+ return encoder->encoder->readTimerConfig_AverageSize(status);
+}
+
+void HAL_SetFPGAEncoderIndexSource(HAL_FPGAEncoderHandle fpgaEncoderHandle,
+ HAL_Handle digitalSourceHandle,
+ HAL_AnalogTriggerType analogTriggerType,
+ HAL_Bool activeHigh, HAL_Bool edgeSensitive,
+ int32_t* status) {
+ auto encoder = fpgaEncoderHandles->Get(fpgaEncoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ bool routingAnalogTrigger = false;
+ uint8_t routingChannel = 0;
+ uint8_t routingModule = 0;
+ bool success =
+ remapDigitalSource(digitalSourceHandle, analogTriggerType, routingChannel,
+ routingModule, routingAnalogTrigger);
+ if (!success) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ encoder->encoder->writeConfig_IndexSource_Channel(routingChannel, status);
+ encoder->encoder->writeConfig_IndexSource_Module(routingModule, status);
+ encoder->encoder->writeConfig_IndexSource_AnalogTrigger(routingAnalogTrigger,
+ status);
+ encoder->encoder->writeConfig_IndexActiveHigh(activeHigh, status);
+ encoder->encoder->writeConfig_IndexEdgeSensitive(edgeSensitive, status);
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/athena/FPGAEncoder.h b/hal/src/main/native/athena/FPGAEncoder.h
new file mode 100644
index 0000000..f29aeae
--- /dev/null
+++ b/hal/src/main/native/athena/FPGAEncoder.h
@@ -0,0 +1,126 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 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 <stdint.h>
+
+#include "hal/AnalogTrigger.h"
+#include "hal/Types.h"
+
+extern "C" {
+
+HAL_FPGAEncoderHandle HAL_InitializeFPGAEncoder(
+ HAL_Handle digitalSourceHandleA, HAL_AnalogTriggerType analogTriggerTypeA,
+ HAL_Handle digitalSourceHandleB, HAL_AnalogTriggerType analogTriggerTypeB,
+ HAL_Bool reverseDirection, int32_t* index, int32_t* status);
+void HAL_FreeFPGAEncoder(HAL_FPGAEncoderHandle fpgaEncoderHandle,
+ int32_t* status);
+
+/**
+ * Reset the Encoder distance to zero.
+ * Resets the current count to zero on the encoder.
+ */
+void HAL_ResetFPGAEncoder(HAL_FPGAEncoderHandle fpgaEncoderHandle,
+ int32_t* status);
+
+/**
+ * Gets the fpga value from the encoder.
+ * The fpga value is the actual count unscaled by the 1x, 2x, or 4x scale
+ * factor.
+ * @return Current fpga count from the encoder
+ */
+int32_t HAL_GetFPGAEncoder(HAL_FPGAEncoderHandle fpgaEncoderHandle,
+ int32_t* status); // Raw value
+
+/**
+ * Returns the period of the most recent pulse.
+ * Returns the period of the most recent Encoder pulse in seconds.
+ * This method compenstates for the decoding type.
+ *
+ * @deprecated Use GetRate() in favor of this method. This returns unscaled
+ * periods and GetRate() scales using value from SetDistancePerPulse().
+ *
+ * @return Period in seconds of the most recent pulse.
+ */
+double HAL_GetFPGAEncoderPeriod(HAL_FPGAEncoderHandle fpgaEncoderHandle,
+ int32_t* status);
+
+/**
+ * Sets the maximum period for stopped detection.
+ * Sets the value that represents the maximum period of the Encoder before it
+ * will assume that the attached device is stopped. This timeout allows users
+ * to determine if the wheels or other shaft has stopped rotating.
+ * This method compensates for the decoding type.
+ *
+ * @deprecated Use SetMinRate() in favor of this method. This takes unscaled
+ * periods and SetMinRate() scales using value from SetDistancePerPulse().
+ *
+ * @param maxPeriod The maximum time between rising and falling edges before the
+ * FPGA will
+ * report the device stopped. This is expressed in seconds.
+ */
+void HAL_SetFPGAEncoderMaxPeriod(HAL_FPGAEncoderHandle fpgaEncoderHandle,
+ double maxPeriod, int32_t* status);
+
+/**
+ * Determine if the encoder is stopped.
+ * Using the MaxPeriod value, a boolean is returned that is true if the encoder
+ * is considered stopped and false if it is still moving. A stopped encoder is
+ * one where the most recent pulse width exceeds the MaxPeriod.
+ * @return True if the encoder is considered stopped.
+ */
+HAL_Bool HAL_GetFPGAEncoderStopped(HAL_FPGAEncoderHandle fpgaEncoderHandle,
+ int32_t* status);
+
+/**
+ * The last direction the encoder value changed.
+ * @return The last direction the encoder value changed.
+ */
+HAL_Bool HAL_GetFPGAEncoderDirection(HAL_FPGAEncoderHandle fpgaEncoderHandle,
+ int32_t* status);
+
+/**
+ * Set the direction sensing for this encoder.
+ * This sets the direction sensing on the encoder so that it could count in the
+ * correct software direction regardless of the mounting.
+ * @param reverseDirection true if the encoder direction should be reversed
+ */
+void HAL_SetFPGAEncoderReverseDirection(HAL_FPGAEncoderHandle fpgaEncoderHandle,
+ HAL_Bool reverseDirection,
+ int32_t* status);
+
+/**
+ * Set the Samples to Average which specifies the number of samples of the timer
+ * to average when calculating the period. Perform averaging to account for
+ * mechanical imperfections or as oversampling to increase resolution.
+ * @param samplesToAverage The number of samples to average from 1 to 127.
+ */
+void HAL_SetFPGAEncoderSamplesToAverage(HAL_FPGAEncoderHandle fpgaEncoderHandle,
+ int32_t samplesToAverage,
+ int32_t* status);
+
+/**
+ * Get the Samples to Average which specifies the number of samples of the timer
+ * to average when calculating the period. Perform averaging to account for
+ * mechanical imperfections or as oversampling to increase resolution.
+ * @return SamplesToAverage The number of samples being averaged (from 1 to 127)
+ */
+int32_t HAL_GetFPGAEncoderSamplesToAverage(
+ HAL_FPGAEncoderHandle fpgaEncoderHandle, int32_t* status);
+
+/**
+ * Set an index source for an encoder, which is an input that resets the
+ * encoder's count.
+ */
+void HAL_SetFPGAEncoderIndexSource(HAL_FPGAEncoderHandle fpgaEncoderHandle,
+ HAL_Handle digitalSourceHandle,
+ HAL_AnalogTriggerType analogTriggerType,
+ HAL_Bool activeHigh, HAL_Bool edgeSensitive,
+ int32_t* status);
+
+} // extern "C"
diff --git a/hal/src/main/native/athena/FRCDriverStation.cpp b/hal/src/main/native/athena/FRCDriverStation.cpp
new file mode 100644
index 0000000..5b29815
--- /dev/null
+++ b/hal/src/main/native/athena/FRCDriverStation.cpp
@@ -0,0 +1,456 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 <atomic>
+#include <chrono>
+#include <cstdlib>
+#include <cstring>
+#include <limits>
+
+#include <FRC_NetworkCommunication/FRCComm.h>
+#include <FRC_NetworkCommunication/NetCommRPCProxy_Occur.h>
+#include <wpi/SafeThread.h>
+#include <wpi/condition_variable.h>
+#include <wpi/mutex.h>
+#include <wpi/raw_ostream.h>
+
+#include "hal/DriverStation.h"
+
+static_assert(sizeof(int32_t) >= sizeof(int),
+ "FRC_NetworkComm status variable is larger than 32 bits");
+
+struct HAL_JoystickAxesInt {
+ int16_t count;
+ int16_t axes[HAL_kMaxJoystickAxes];
+};
+
+static constexpr int kJoystickPorts = 6;
+
+// Message and Data variables
+static wpi::mutex msgMutex;
+
+static int32_t HAL_GetJoystickAxesInternal(int32_t joystickNum,
+ HAL_JoystickAxes* axes) {
+ HAL_JoystickAxesInt axesInt;
+
+ int retVal = FRC_NetworkCommunication_getJoystickAxes(
+ joystickNum, reinterpret_cast<JoystickAxes_t*>(&axesInt),
+ HAL_kMaxJoystickAxes);
+
+ // copy integer values to double values
+ axes->count = axesInt.count;
+ // current scaling is -128 to 127, can easily be patched in the future by
+ // changing this function.
+ for (int32_t i = 0; i < axesInt.count; i++) {
+ int8_t value = axesInt.axes[i];
+ if (value < 0) {
+ axes->axes[i] = value / 128.0;
+ } else {
+ axes->axes[i] = value / 127.0;
+ }
+ }
+
+ return retVal;
+}
+
+static int32_t HAL_GetJoystickPOVsInternal(int32_t joystickNum,
+ HAL_JoystickPOVs* povs) {
+ return FRC_NetworkCommunication_getJoystickPOVs(
+ joystickNum, reinterpret_cast<JoystickPOV_t*>(povs),
+ HAL_kMaxJoystickPOVs);
+}
+
+static int32_t HAL_GetJoystickButtonsInternal(int32_t joystickNum,
+ HAL_JoystickButtons* buttons) {
+ return FRC_NetworkCommunication_getJoystickButtons(
+ joystickNum, &buttons->buttons, &buttons->count);
+}
+/**
+ * Retrieve the Joystick Descriptor for particular slot
+ * @param desc [out] descriptor (data transfer object) to fill in. desc is
+ * filled in regardless of success. In other words, if descriptor is not
+ * available, desc is filled in with default values matching the init-values in
+ * Java and C++ Driverstation for when caller requests a too-large joystick
+ * index.
+ *
+ * @return error code reported from Network Comm back-end. Zero is good,
+ * nonzero is bad.
+ */
+static int32_t HAL_GetJoystickDescriptorInternal(int32_t joystickNum,
+ HAL_JoystickDescriptor* desc) {
+ desc->isXbox = 0;
+ desc->type = (std::numeric_limits<uint8_t>::max)();
+ desc->name[0] = '\0';
+ desc->axisCount =
+ HAL_kMaxJoystickAxes; /* set to the desc->axisTypes's capacity */
+ desc->buttonCount = 0;
+ desc->povCount = 0;
+ int retval = FRC_NetworkCommunication_getJoystickDesc(
+ joystickNum, &desc->isXbox, &desc->type,
+ reinterpret_cast<char*>(&desc->name), &desc->axisCount,
+ reinterpret_cast<uint8_t*>(&desc->axisTypes), &desc->buttonCount,
+ &desc->povCount);
+ /* check the return, if there is an error and the RIOimage predates FRC2017,
+ * then axisCount needs to be cleared */
+ if (retval != 0) {
+ /* set count to zero so downstream code doesn't decode invalid axisTypes. */
+ desc->axisCount = 0;
+ }
+ return retval;
+}
+
+static int32_t HAL_GetControlWordInternal(HAL_ControlWord* controlWord) {
+ std::memset(controlWord, 0, sizeof(HAL_ControlWord));
+ return FRC_NetworkCommunication_getControlWord(
+ reinterpret_cast<ControlWord_t*>(controlWord));
+}
+
+static int32_t HAL_GetMatchInfoInternal(HAL_MatchInfo* info) {
+ MatchType_t matchType = MatchType_t::kMatchType_none;
+ int status = FRC_NetworkCommunication_getMatchInfo(
+ info->eventName, &matchType, &info->matchNumber, &info->replayNumber,
+ info->gameSpecificMessage, &info->gameSpecificMessageSize);
+
+ info->matchType = static_cast<HAL_MatchType>(matchType);
+
+ *(std::end(info->eventName) - 1) = '\0';
+
+ return status;
+}
+
+static wpi::mutex* newDSDataAvailableMutex;
+static wpi::condition_variable* newDSDataAvailableCond;
+static std::atomic_int newDSDataAvailableCounter{0};
+
+namespace hal {
+namespace init {
+void InitializeFRCDriverStation() {
+ static wpi::mutex newMutex;
+ newDSDataAvailableMutex = &newMutex;
+ static wpi::condition_variable newCond;
+ newDSDataAvailableCond = &newCond;
+}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+
+int32_t HAL_SendError(HAL_Bool isError, int32_t errorCode, HAL_Bool isLVCode,
+ const char* details, const char* location,
+ const char* callStack, HAL_Bool printMsg) {
+ // Avoid flooding console by keeping track of previous 5 error
+ // messages and only printing again if they're longer than 1 second old.
+ static constexpr int KEEP_MSGS = 5;
+ std::scoped_lock lock(msgMutex);
+ static std::string prevMsg[KEEP_MSGS];
+ static std::chrono::time_point<std::chrono::steady_clock>
+ prevMsgTime[KEEP_MSGS];
+ static bool initialized = false;
+ if (!initialized) {
+ for (int i = 0; i < KEEP_MSGS; i++) {
+ prevMsgTime[i] =
+ std::chrono::steady_clock::now() - std::chrono::seconds(2);
+ }
+ initialized = true;
+ }
+
+ auto curTime = std::chrono::steady_clock::now();
+ int i;
+ for (i = 0; i < KEEP_MSGS; ++i) {
+ if (prevMsg[i] == details) break;
+ }
+ int retval = 0;
+ if (i == KEEP_MSGS || (curTime - prevMsgTime[i]) >= std::chrono::seconds(1)) {
+ wpi::StringRef detailsRef{details};
+ wpi::StringRef locationRef{location};
+ wpi::StringRef callStackRef{callStack};
+
+ // 1 tag, 4 timestamp, 2 seqnum
+ // 2 numOccur, 4 error code, 1 flags, 6 strlen
+ // 1 extra needed for padding on Netcomm end.
+ size_t baseLength = 21;
+
+ if (baseLength + detailsRef.size() + locationRef.size() +
+ callStackRef.size() <=
+ 65536) {
+ // Pass through
+ retval = FRC_NetworkCommunication_sendError(isError, errorCode, isLVCode,
+ details, location, callStack);
+ } else if (baseLength + detailsRef.size() > 65536) {
+ // Details too long, cut both location and stack
+ auto newLen = 65536 - baseLength;
+ std::string newDetails{details, newLen};
+ char empty = '\0';
+ retval = FRC_NetworkCommunication_sendError(
+ isError, errorCode, isLVCode, newDetails.c_str(), &empty, &empty);
+ } else if (baseLength + detailsRef.size() + locationRef.size() > 65536) {
+ // Location too long, cut stack
+ auto newLen = 65536 - baseLength - detailsRef.size();
+ std::string newLocation{location, newLen};
+ char empty = '\0';
+ retval = FRC_NetworkCommunication_sendError(
+ isError, errorCode, isLVCode, details, newLocation.c_str(), &empty);
+ } else {
+ // Stack too long
+ auto newLen = 65536 - baseLength - detailsRef.size() - locationRef.size();
+ std::string newCallStack{callStack, newLen};
+ retval = FRC_NetworkCommunication_sendError(isError, errorCode, isLVCode,
+ details, location,
+ newCallStack.c_str());
+ }
+ if (printMsg) {
+ if (location && location[0] != '\0') {
+ wpi::errs() << (isError ? "Error" : "Warning") << " at " << location
+ << ": ";
+ }
+ wpi::errs() << details << "\n";
+ if (callStack && callStack[0] != '\0') {
+ wpi::errs() << callStack << "\n";
+ }
+ }
+ if (i == KEEP_MSGS) {
+ // replace the oldest one
+ i = 0;
+ auto first = prevMsgTime[0];
+ for (int j = 1; j < KEEP_MSGS; ++j) {
+ if (prevMsgTime[j] < first) {
+ first = prevMsgTime[j];
+ i = j;
+ }
+ }
+ prevMsg[i] = details;
+ }
+ prevMsgTime[i] = curTime;
+ }
+ return retval;
+}
+
+int32_t HAL_GetControlWord(HAL_ControlWord* controlWord) {
+ return HAL_GetControlWordInternal(controlWord);
+}
+
+int32_t HAL_GetJoystickAxes(int32_t joystickNum, HAL_JoystickAxes* axes) {
+ return HAL_GetJoystickAxesInternal(joystickNum, axes);
+}
+
+int32_t HAL_GetJoystickPOVs(int32_t joystickNum, HAL_JoystickPOVs* povs) {
+ return HAL_GetJoystickPOVsInternal(joystickNum, povs);
+}
+
+int32_t HAL_GetJoystickButtons(int32_t joystickNum,
+ HAL_JoystickButtons* buttons) {
+ return HAL_GetJoystickButtonsInternal(joystickNum, buttons);
+}
+
+int32_t HAL_GetJoystickDescriptor(int32_t joystickNum,
+ HAL_JoystickDescriptor* desc) {
+ return HAL_GetJoystickDescriptorInternal(joystickNum, desc);
+}
+
+int32_t HAL_GetMatchInfo(HAL_MatchInfo* info) {
+ return HAL_GetMatchInfoInternal(info);
+}
+
+HAL_AllianceStationID HAL_GetAllianceStation(int32_t* status) {
+ HAL_AllianceStationID allianceStation;
+ *status = FRC_NetworkCommunication_getAllianceStation(
+ reinterpret_cast<AllianceStationID_t*>(&allianceStation));
+ return allianceStation;
+}
+
+HAL_Bool HAL_GetJoystickIsXbox(int32_t joystickNum) {
+ HAL_JoystickDescriptor joystickDesc;
+ if (HAL_GetJoystickDescriptor(joystickNum, &joystickDesc) < 0) {
+ return 0;
+ } else {
+ return joystickDesc.isXbox;
+ }
+}
+
+int32_t HAL_GetJoystickType(int32_t joystickNum) {
+ HAL_JoystickDescriptor joystickDesc;
+ if (HAL_GetJoystickDescriptor(joystickNum, &joystickDesc) < 0) {
+ return -1;
+ } else {
+ return joystickDesc.type;
+ }
+}
+
+char* HAL_GetJoystickName(int32_t joystickNum) {
+ HAL_JoystickDescriptor joystickDesc;
+ if (HAL_GetJoystickDescriptor(joystickNum, &joystickDesc) < 0) {
+ char* name = static_cast<char*>(std::malloc(1));
+ name[0] = '\0';
+ return name;
+ } else {
+ size_t len = std::strlen(joystickDesc.name);
+ char* name = static_cast<char*>(std::malloc(len + 1));
+ std::strncpy(name, joystickDesc.name, len);
+ name[len] = '\0';
+ return name;
+ }
+}
+
+void HAL_FreeJoystickName(char* name) { std::free(name); }
+
+int32_t HAL_GetJoystickAxisType(int32_t joystickNum, int32_t axis) {
+ HAL_JoystickDescriptor joystickDesc;
+ if (HAL_GetJoystickDescriptor(joystickNum, &joystickDesc) < 0) {
+ return -1;
+ } else {
+ return joystickDesc.axisTypes[axis];
+ }
+}
+
+int32_t HAL_SetJoystickOutputs(int32_t joystickNum, int64_t outputs,
+ int32_t leftRumble, int32_t rightRumble) {
+ return FRC_NetworkCommunication_setJoystickOutputs(joystickNum, outputs,
+ leftRumble, rightRumble);
+}
+
+double HAL_GetMatchTime(int32_t* status) {
+ float matchTime;
+ *status = FRC_NetworkCommunication_getMatchTime(&matchTime);
+ return matchTime;
+}
+
+void HAL_ObserveUserProgramStarting(void) {
+ FRC_NetworkCommunication_observeUserProgramStarting();
+}
+
+void HAL_ObserveUserProgramDisabled(void) {
+ FRC_NetworkCommunication_observeUserProgramDisabled();
+}
+
+void HAL_ObserveUserProgramAutonomous(void) {
+ FRC_NetworkCommunication_observeUserProgramAutonomous();
+}
+
+void HAL_ObserveUserProgramTeleop(void) {
+ FRC_NetworkCommunication_observeUserProgramTeleop();
+}
+
+void HAL_ObserveUserProgramTest(void) {
+ FRC_NetworkCommunication_observeUserProgramTest();
+}
+
+static int& GetThreadLocalLastCount() {
+ // There is a rollover error condition here. At Packet# = n * (uintmax), this
+ // will return false when instead it should return true. However, this at a
+ // 20ms rate occurs once every 2.7 years of DS connected runtime, so not
+ // worth the cycles to check.
+ thread_local int lastCount{-1};
+ return lastCount;
+}
+
+void HAL_WaitForCachedControlData(void) {
+ HAL_WaitForCachedControlDataTimeout(0);
+}
+
+HAL_Bool HAL_WaitForCachedControlDataTimeout(double timeout) {
+ int& lastCount = GetThreadLocalLastCount();
+ int currentCount = newDSDataAvailableCounter.load();
+ if (lastCount != currentCount) {
+ lastCount = currentCount;
+ return true;
+ }
+ auto timeoutTime =
+ std::chrono::steady_clock::now() + std::chrono::duration<double>(timeout);
+
+ std::unique_lock lock{*newDSDataAvailableMutex};
+ while (newDSDataAvailableCounter.load() == currentCount) {
+ if (timeout > 0) {
+ auto timedOut = newDSDataAvailableCond->wait_until(lock, timeoutTime);
+ if (timedOut == std::cv_status::timeout) {
+ return false;
+ }
+ } else {
+ newDSDataAvailableCond->wait(lock);
+ }
+ }
+ return true;
+}
+
+HAL_Bool HAL_IsNewControlData(void) {
+ int& lastCount = GetThreadLocalLastCount();
+ int currentCount = newDSDataAvailableCounter.load();
+ if (lastCount == currentCount) return false;
+ lastCount = currentCount;
+ return true;
+}
+
+/**
+ * Waits for the newest DS packet to arrive. Note that this is a blocking call.
+ */
+void HAL_WaitForDSData(void) { HAL_WaitForDSDataTimeout(0); }
+
+/**
+ * Waits for the newest DS packet to arrive. If timeout is <= 0, this will wait
+ * forever. Otherwise, it will wait until either a new packet, or the timeout
+ * time has passed. Returns true on new data, false on timeout.
+ */
+HAL_Bool HAL_WaitForDSDataTimeout(double timeout) {
+ auto timeoutTime =
+ std::chrono::steady_clock::now() + std::chrono::duration<double>(timeout);
+
+ int currentCount = newDSDataAvailableCounter.load();
+ std::unique_lock lock{*newDSDataAvailableMutex};
+ while (newDSDataAvailableCounter.load() == currentCount) {
+ if (timeout > 0) {
+ auto timedOut = newDSDataAvailableCond->wait_until(lock, timeoutTime);
+ if (timedOut == std::cv_status::timeout) {
+ return false;
+ }
+ } else {
+ newDSDataAvailableCond->wait(lock);
+ }
+ }
+ return true;
+}
+
+// Constant number to be used for our occur handle
+constexpr int32_t refNumber = 42;
+
+static void newDataOccur(uint32_t refNum) {
+ // Since we could get other values, require our specific handle
+ // to signal our threads
+ if (refNum != refNumber) return;
+ // Notify all threads
+ newDSDataAvailableCounter.fetch_add(1);
+ newDSDataAvailableCond->notify_all();
+}
+
+/*
+ * Call this to initialize the driver station communication. This will properly
+ * handle multiple calls. However note that this CANNOT be called from a library
+ * that interfaces with LabVIEW.
+ */
+void HAL_InitializeDriverStation(void) {
+ static std::atomic_bool initialized{false};
+ static wpi::mutex initializeMutex;
+ // Initial check, as if it's true initialization has finished
+ if (initialized) return;
+
+ std::scoped_lock lock(initializeMutex);
+ // Second check in case another thread was waiting
+ if (initialized) return;
+
+ // Set up the occur function internally with NetComm
+ NetCommRPCProxy_SetOccurFuncPointer(newDataOccur);
+ // Set up our occur reference number
+ setNewDataOccurRef(refNumber);
+
+ initialized = true;
+}
+
+/*
+ * Releases the DS Mutex to allow proper shutdown of any threads that are
+ * waiting on it.
+ */
+void HAL_ReleaseDSMutex(void) { newDataOccur(refNumber); }
+
+} // extern "C"
diff --git a/hal/src/main/native/athena/HAL.cpp b/hal/src/main/native/athena/HAL.cpp
new file mode 100644
index 0000000..f698fa0
--- /dev/null
+++ b/hal/src/main/native/athena/HAL.cpp
@@ -0,0 +1,402 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 "hal/HAL.h"
+
+#include <signal.h> // linux for kill
+#include <sys/prctl.h>
+#include <unistd.h>
+
+#include <atomic>
+#include <cstdlib>
+#include <fstream>
+#include <thread>
+
+#include <FRC_NetworkCommunication/FRCComm.h>
+#include <FRC_NetworkCommunication/LoadOut.h>
+#include <FRC_NetworkCommunication/UsageReporting.h>
+#include <wpi/mutex.h>
+#include <wpi/raw_ostream.h>
+#include <wpi/timestamp.h>
+
+#include "HALInitializer.h"
+#include "ctre/ctre.h"
+#include "hal/ChipObject.h"
+#include "hal/DriverStation.h"
+#include "hal/Errors.h"
+#include "hal/Notifier.h"
+#include "hal/handles/HandlesInternal.h"
+
+using namespace hal;
+
+static std::unique_ptr<tGlobal> global;
+static std::unique_ptr<tSysWatchdog> watchdog;
+
+using namespace hal;
+
+namespace hal {
+namespace init {
+void InitializeHAL() {
+ InitializeAddressableLED();
+ InitializeAccelerometer();
+ InitializeAnalogAccumulator();
+ InitializeAnalogInput();
+ InitializeAnalogInternal();
+ InitializeAnalogOutput();
+ InitializeAnalogTrigger();
+ InitializeCAN();
+ InitializeCANAPI();
+ InitializeCompressor();
+ InitializeConstants();
+ InitializeCounter();
+ InitializeDigitalInternal();
+ InitializeDIO();
+ InitializeDMA();
+ InitializeDutyCycle();
+ InitializeEncoder();
+ InitializeFPGAEncoder();
+ InitializeFRCDriverStation();
+ InitializeI2C();
+ InitialzeInterrupts();
+ InitializeMain();
+ InitializeNotifier();
+ InitializePCMInternal();
+ InitializePDP();
+ InitializePorts();
+ InitializePower();
+ InitializePWM();
+ InitializeRelay();
+ InitializeSolenoid();
+ InitializeSPI();
+ InitializeThreads();
+}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+
+HAL_PortHandle HAL_GetPort(int32_t channel) {
+ // Dont allow a number that wouldn't fit in a uint8_t
+ if (channel < 0 || channel >= 255) return HAL_kInvalidHandle;
+ return createPortHandle(channel, 1);
+}
+
+HAL_PortHandle HAL_GetPortWithModule(int32_t module, int32_t channel) {
+ // Dont allow a number that wouldn't fit in a uint8_t
+ if (channel < 0 || channel >= 255) return HAL_kInvalidHandle;
+ if (module < 0 || module >= 255) return HAL_kInvalidHandle;
+ return createPortHandle(channel, module);
+}
+
+const char* HAL_GetErrorMessage(int32_t code) {
+ switch (code) {
+ case 0:
+ return "";
+ case CTR_RxTimeout:
+ return CTR_RxTimeout_MESSAGE;
+ case CTR_TxTimeout:
+ return CTR_TxTimeout_MESSAGE;
+ case CTR_InvalidParamValue:
+ return CTR_InvalidParamValue_MESSAGE;
+ case CTR_UnexpectedArbId:
+ return CTR_UnexpectedArbId_MESSAGE;
+ case CTR_TxFailed:
+ return CTR_TxFailed_MESSAGE;
+ case CTR_SigNotUpdated:
+ return CTR_SigNotUpdated_MESSAGE;
+ case NiFpga_Status_FifoTimeout:
+ return NiFpga_Status_FifoTimeout_MESSAGE;
+ case NiFpga_Status_TransferAborted:
+ return NiFpga_Status_TransferAborted_MESSAGE;
+ case NiFpga_Status_MemoryFull:
+ return NiFpga_Status_MemoryFull_MESSAGE;
+ case NiFpga_Status_SoftwareFault:
+ return NiFpga_Status_SoftwareFault_MESSAGE;
+ case NiFpga_Status_InvalidParameter:
+ return NiFpga_Status_InvalidParameter_MESSAGE;
+ case NiFpga_Status_ResourceNotFound:
+ return NiFpga_Status_ResourceNotFound_MESSAGE;
+ case NiFpga_Status_ResourceNotInitialized:
+ return NiFpga_Status_ResourceNotInitialized_MESSAGE;
+ case NiFpga_Status_HardwareFault:
+ return NiFpga_Status_HardwareFault_MESSAGE;
+ case NiFpga_Status_IrqTimeout:
+ return NiFpga_Status_IrqTimeout_MESSAGE;
+ case SAMPLE_RATE_TOO_HIGH:
+ return SAMPLE_RATE_TOO_HIGH_MESSAGE;
+ case VOLTAGE_OUT_OF_RANGE:
+ return VOLTAGE_OUT_OF_RANGE_MESSAGE;
+ case LOOP_TIMING_ERROR:
+ return LOOP_TIMING_ERROR_MESSAGE;
+ case SPI_WRITE_NO_MOSI:
+ return SPI_WRITE_NO_MOSI_MESSAGE;
+ case SPI_READ_NO_MISO:
+ return SPI_READ_NO_MISO_MESSAGE;
+ case SPI_READ_NO_DATA:
+ return SPI_READ_NO_DATA_MESSAGE;
+ case INCOMPATIBLE_STATE:
+ return INCOMPATIBLE_STATE_MESSAGE;
+ case NO_AVAILABLE_RESOURCES:
+ return NO_AVAILABLE_RESOURCES_MESSAGE;
+ case RESOURCE_IS_ALLOCATED:
+ return RESOURCE_IS_ALLOCATED_MESSAGE;
+ case RESOURCE_OUT_OF_RANGE:
+ return RESOURCE_OUT_OF_RANGE_MESSAGE;
+ case HAL_INVALID_ACCUMULATOR_CHANNEL:
+ return HAL_INVALID_ACCUMULATOR_CHANNEL_MESSAGE;
+ case HAL_HANDLE_ERROR:
+ return HAL_HANDLE_ERROR_MESSAGE;
+ case NULL_PARAMETER:
+ return NULL_PARAMETER_MESSAGE;
+ case ANALOG_TRIGGER_LIMIT_ORDER_ERROR:
+ return ANALOG_TRIGGER_LIMIT_ORDER_ERROR_MESSAGE;
+ case ANALOG_TRIGGER_PULSE_OUTPUT_ERROR:
+ return ANALOG_TRIGGER_PULSE_OUTPUT_ERROR_MESSAGE;
+ case PARAMETER_OUT_OF_RANGE:
+ return PARAMETER_OUT_OF_RANGE_MESSAGE;
+ case HAL_COUNTER_NOT_SUPPORTED:
+ return HAL_COUNTER_NOT_SUPPORTED_MESSAGE;
+ case HAL_ERR_CANSessionMux_InvalidBuffer:
+ return ERR_CANSessionMux_InvalidBuffer_MESSAGE;
+ case HAL_ERR_CANSessionMux_MessageNotFound:
+ return ERR_CANSessionMux_MessageNotFound_MESSAGE;
+ case HAL_WARN_CANSessionMux_NoToken:
+ return WARN_CANSessionMux_NoToken_MESSAGE;
+ case HAL_ERR_CANSessionMux_NotAllowed:
+ return ERR_CANSessionMux_NotAllowed_MESSAGE;
+ case HAL_ERR_CANSessionMux_NotInitialized:
+ return ERR_CANSessionMux_NotInitialized_MESSAGE;
+ case HAL_PWM_SCALE_ERROR:
+ return HAL_PWM_SCALE_ERROR_MESSAGE;
+ case HAL_SERIAL_PORT_NOT_FOUND:
+ return HAL_SERIAL_PORT_NOT_FOUND_MESSAGE;
+ case HAL_THREAD_PRIORITY_ERROR:
+ return HAL_THREAD_PRIORITY_ERROR_MESSAGE;
+ case HAL_THREAD_PRIORITY_RANGE_ERROR:
+ return HAL_THREAD_PRIORITY_RANGE_ERROR_MESSAGE;
+ case HAL_SERIAL_PORT_OPEN_ERROR:
+ return HAL_SERIAL_PORT_OPEN_ERROR_MESSAGE;
+ case HAL_SERIAL_PORT_ERROR:
+ return HAL_SERIAL_PORT_ERROR_MESSAGE;
+ case HAL_CAN_TIMEOUT:
+ return HAL_CAN_TIMEOUT_MESSAGE;
+ case ERR_FRCSystem_NetCommNotResponding:
+ return ERR_FRCSystem_NetCommNotResponding_MESSAGE;
+ case ERR_FRCSystem_NoDSConnection:
+ return ERR_FRCSystem_NoDSConnection_MESSAGE;
+ case HAL_CAN_BUFFER_OVERRUN:
+ return HAL_CAN_BUFFER_OVERRUN_MESSAGE;
+ case HAL_LED_CHANNEL_ERROR:
+ return HAL_LED_CHANNEL_ERROR_MESSAGE;
+ default:
+ return "Unknown error status";
+ }
+}
+
+HAL_RuntimeType HAL_GetRuntimeType(void) { return HAL_Athena; }
+
+int32_t HAL_GetFPGAVersion(int32_t* status) {
+ if (!global) {
+ *status = NiFpga_Status_ResourceNotInitialized;
+ return 0;
+ }
+ return global->readVersion(status);
+}
+
+int64_t HAL_GetFPGARevision(int32_t* status) {
+ if (!global) {
+ *status = NiFpga_Status_ResourceNotInitialized;
+ return 0;
+ }
+ return global->readRevision(status);
+}
+
+uint64_t HAL_GetFPGATime(int32_t* status) {
+ if (!global) {
+ *status = NiFpga_Status_ResourceNotInitialized;
+ return 0;
+ }
+ *status = 0;
+ uint64_t upper1 = global->readLocalTimeUpper(status);
+ uint32_t lower = global->readLocalTime(status);
+ uint64_t upper2 = global->readLocalTimeUpper(status);
+ if (*status != 0) return 0;
+ if (upper1 != upper2) {
+ // Rolled over between the lower call, reread lower
+ lower = global->readLocalTime(status);
+ if (*status != 0) return 0;
+ }
+ return (upper2 << 32) + lower;
+}
+
+uint64_t HAL_ExpandFPGATime(uint32_t unexpanded_lower, int32_t* status) {
+ // Capture the current FPGA time. This will give us the upper half of the
+ // clock.
+ uint64_t fpga_time = HAL_GetFPGATime(status);
+ if (*status != 0) return 0;
+
+ // Now, we need to detect the case where the lower bits rolled over after we
+ // sampled. In that case, the upper bits will be 1 bigger than they should
+ // be.
+
+ // Break it into lower and upper portions.
+ uint32_t lower = fpga_time & 0xffffffffull;
+ uint64_t upper = (fpga_time >> 32) & 0xffffffff;
+
+ // The time was sampled *before* the current time, so roll it back.
+ if (lower < unexpanded_lower) {
+ --upper;
+ }
+
+ return (upper << 32) + static_cast<uint64_t>(unexpanded_lower);
+}
+
+HAL_Bool HAL_GetFPGAButton(int32_t* status) {
+ if (!global) {
+ *status = NiFpga_Status_ResourceNotInitialized;
+ return false;
+ }
+ return global->readUserButton(status);
+}
+
+HAL_Bool HAL_GetSystemActive(int32_t* status) {
+ if (!watchdog) {
+ *status = NiFpga_Status_ResourceNotInitialized;
+ return false;
+ }
+ return watchdog->readStatus_SystemActive(status);
+}
+
+HAL_Bool HAL_GetBrownedOut(int32_t* status) {
+ if (!watchdog) {
+ *status = NiFpga_Status_ResourceNotInitialized;
+ return false;
+ }
+ return !(watchdog->readStatus_PowerAlive(status));
+}
+
+static bool killExistingProgram(int timeout, int mode) {
+ // Kill any previous robot programs
+ std::fstream fs;
+ // By making this both in/out, it won't give us an error if it doesnt exist
+ fs.open("/var/lock/frc.pid", std::fstream::in | std::fstream::out);
+ if (fs.bad()) return false;
+
+ pid_t pid = 0;
+ if (!fs.eof() && !fs.fail()) {
+ fs >> pid;
+ // see if the pid is around, but we don't want to mess with init id=1, or
+ // ourselves
+ if (pid >= 2 && kill(pid, 0) == 0 && pid != getpid()) {
+ wpi::outs() << "Killing previously running FRC program...\n";
+ kill(pid, SIGTERM); // try to kill it
+ std::this_thread::sleep_for(std::chrono::milliseconds(timeout));
+ if (kill(pid, 0) == 0) {
+ // still not successfull
+ wpi::outs() << "FRC pid " << pid << " did not die within " << timeout
+ << "ms. Force killing with kill -9\n";
+ // Force kill -9
+ auto forceKill = kill(pid, SIGKILL);
+ if (forceKill != 0) {
+ auto errorMsg = std::strerror(forceKill);
+ wpi::outs() << "Kill -9 error: " << errorMsg << "\n";
+ }
+ // Give a bit of time for the kill to take place
+ std::this_thread::sleep_for(std::chrono::milliseconds(250));
+ }
+ }
+ }
+ fs.close();
+ // we will re-open it write only to truncate the file
+ fs.open("/var/lock/frc.pid", std::fstream::out | std::fstream::trunc);
+ fs.seekp(0);
+ pid = getpid();
+ fs << pid << std::endl;
+ fs.close();
+ return true;
+}
+
+HAL_Bool HAL_Initialize(int32_t timeout, int32_t mode) {
+ static std::atomic_bool initialized{false};
+ static wpi::mutex initializeMutex;
+ // Initial check, as if it's true initialization has finished
+ if (initialized) return true;
+
+ std::scoped_lock lock(initializeMutex);
+ // Second check in case another thread was waiting
+ if (initialized) return true;
+
+ hal::init::InitializeHAL();
+
+ hal::init::HAL_IsInitialized.store(true);
+
+ setlinebuf(stdin);
+ setlinebuf(stdout);
+ wpi::outs().SetUnbuffered();
+
+ prctl(PR_SET_PDEATHSIG, SIGTERM);
+
+ // Return false if program failed to kill an existing program
+ if (!killExistingProgram(timeout, mode)) {
+ return false;
+ }
+
+ FRC_NetworkCommunication_Reserve(nullptr);
+
+ std::atexit([]() {
+ // Unregister our new data condition variable.
+ setNewDataSem(nullptr);
+ });
+
+ // image 4; Fixes errors caused by multiple processes. Talk to NI about this
+ nFPGA::nRoboRIO_FPGANamespace::g_currentTargetClass =
+ nLoadOut::kTargetClass_RoboRIO;
+
+ int32_t status = 0;
+ global.reset(tGlobal::create(&status));
+ watchdog.reset(tSysWatchdog::create(&status));
+
+ if (status != 0) return false;
+
+ HAL_InitializeDriverStation();
+
+ // Set WPI_Now to use FPGA timestamp
+ wpi::SetNowImpl([]() -> uint64_t {
+ int32_t status = 0;
+ uint64_t rv = HAL_GetFPGATime(&status);
+ if (status != 0) {
+ wpi::errs()
+ << "Call to HAL_GetFPGATime failed in wpi::Now() with status "
+ << status
+ << ". Initialization might have failed. Time will not be correct\n";
+ wpi::errs().flush();
+ return 0u;
+ }
+ return rv;
+ });
+
+ initialized = true;
+ return true;
+}
+
+int64_t HAL_Report(int32_t resource, int32_t instanceNumber, int32_t context,
+ const char* feature) {
+ if (feature == nullptr) {
+ feature = "";
+ }
+
+ return FRC_NetworkCommunication_nUsageReporting_report(
+ resource, instanceNumber, context, feature);
+}
+
+// TODO: HACKS
+// No need for header definitions, as we should not run from user code.
+void NumericArrayResize(void) {}
+void RTSetCleanupProc(void) {}
+void EDVR_CreateReference(void) {}
+
+} // extern "C"
diff --git a/hal/src/main/native/athena/HALInitializer.cpp b/hal/src/main/native/athena/HALInitializer.cpp
new file mode 100644
index 0000000..a0456d4
--- /dev/null
+++ b/hal/src/main/native/athena/HALInitializer.cpp
@@ -0,0 +1,17 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 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 "HALInitializer.h"
+
+#include "hal/HAL.h"
+
+namespace hal {
+namespace init {
+std::atomic_bool HAL_IsInitialized{false};
+void RunInitialize() { HAL_Initialize(500, 0); }
+} // namespace init
+} // namespace hal
diff --git a/hal/src/main/native/athena/HALInitializer.h b/hal/src/main/native/athena/HALInitializer.h
new file mode 100644
index 0000000..cf59394
--- /dev/null
+++ b/hal/src/main/native/athena/HALInitializer.h
@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 <atomic>
+
+namespace hal {
+namespace init {
+extern std::atomic_bool HAL_IsInitialized;
+extern void RunInitialize();
+static inline void CheckInit() {
+ if (HAL_IsInitialized.load(std::memory_order_relaxed)) return;
+ RunInitialize();
+}
+
+extern void InitializeAccelerometer();
+extern void InitializeAddressableLED();
+extern void InitializeAnalogAccumulator();
+extern void InitializeAnalogInput();
+extern void InitializeAnalogInternal();
+extern void InitializeAnalogOutput();
+extern void InitializeAnalogTrigger();
+extern void InitializeCAN();
+extern void InitializeCANAPI();
+extern void InitializeCompressor();
+extern void InitializeConstants();
+extern void InitializeCounter();
+extern void InitializeDigitalInternal();
+extern void InitializeDIO();
+extern void InitializeDMA();
+extern void InitializeDutyCycle();
+extern void InitializeEncoder();
+extern void InitializeFPGAEncoder();
+extern void InitializeFRCDriverStation();
+extern void InitializeHAL();
+extern void InitializeI2C();
+extern void InitialzeInterrupts();
+extern void InitializeMain();
+extern void InitializeNotifier();
+extern void InitializePCMInternal();
+extern void InitializePDP();
+extern void InitializePorts();
+extern void InitializePower();
+extern void InitializePWM();
+extern void InitializeRelay();
+extern void InitializeSolenoid();
+extern void InitializeSPI();
+extern void InitializeThreads();
+} // namespace init
+} // namespace hal
diff --git a/hal/src/main/native/athena/I2C.cpp b/hal/src/main/native/athena/I2C.cpp
new file mode 100644
index 0000000..b72e25e
--- /dev/null
+++ b/hal/src/main/native/athena/I2C.cpp
@@ -0,0 +1,192 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 "hal/I2C.h"
+
+#include <fcntl.h>
+#include <linux/i2c-dev.h>
+#include <linux/i2c.h>
+#include <sys/ioctl.h>
+#include <unistd.h>
+
+#include <cstring>
+
+#include "DigitalInternal.h"
+#include "HALInitializer.h"
+#include "hal/DIO.h"
+#include "hal/HAL.h"
+
+using namespace hal;
+
+static wpi::mutex digitalI2COnBoardMutex;
+static wpi::mutex digitalI2CMXPMutex;
+
+static uint8_t i2COnboardObjCount{0};
+static uint8_t i2CMXPObjCount{0};
+static int i2COnBoardHandle{-1};
+static int i2CMXPHandle{-1};
+
+static HAL_DigitalHandle i2CMXPDigitalHandle1{HAL_kInvalidHandle};
+static HAL_DigitalHandle i2CMXPDigitalHandle2{HAL_kInvalidHandle};
+
+namespace hal {
+namespace init {
+void InitializeI2C() {}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+
+void HAL_InitializeI2C(HAL_I2CPort port, int32_t* status) {
+ hal::init::CheckInit();
+ initializeDigital(status);
+ if (*status != 0) return;
+
+ if (port < 0 || port > 1) {
+ // Set port out of range error here
+ return;
+ }
+
+ if (port == HAL_I2C_kOnboard) {
+ std::scoped_lock lock(digitalI2COnBoardMutex);
+ i2COnboardObjCount++;
+ if (i2COnboardObjCount > 1) return;
+ int handle = open("/dev/i2c-2", O_RDWR);
+ if (handle < 0) {
+ std::printf("Failed to open onboard i2c bus: %s\n", std::strerror(errno));
+ return;
+ }
+ i2COnBoardHandle = handle;
+ } else {
+ std::scoped_lock lock(digitalI2CMXPMutex);
+ i2CMXPObjCount++;
+ if (i2CMXPObjCount > 1) return;
+ if ((i2CMXPDigitalHandle1 = HAL_InitializeDIOPort(
+ HAL_GetPort(24), false, status)) == HAL_kInvalidHandle) {
+ return;
+ }
+ if ((i2CMXPDigitalHandle2 = HAL_InitializeDIOPort(
+ HAL_GetPort(25), false, status)) == HAL_kInvalidHandle) {
+ HAL_FreeDIOPort(i2CMXPDigitalHandle1); // free the first port allocated
+ return;
+ }
+ digitalSystem->writeEnableMXPSpecialFunction(
+ digitalSystem->readEnableMXPSpecialFunction(status) | 0xC000, status);
+ int handle = open("/dev/i2c-1", O_RDWR);
+ if (handle < 0) {
+ std::printf("Failed to open MXP i2c bus: %s\n", std::strerror(errno));
+ return;
+ }
+ i2CMXPHandle = handle;
+ }
+}
+
+int32_t HAL_TransactionI2C(HAL_I2CPort port, int32_t deviceAddress,
+ const uint8_t* dataToSend, int32_t sendSize,
+ uint8_t* dataReceived, int32_t receiveSize) {
+ if (port < 0 || port > 1) {
+ // Set port out of range error here
+ return -1;
+ }
+
+ struct i2c_msg msgs[2];
+ msgs[0].addr = deviceAddress;
+ msgs[0].flags = 0;
+ msgs[0].len = sendSize;
+ msgs[0].buf = const_cast<uint8_t*>(dataToSend);
+ msgs[1].addr = deviceAddress;
+ msgs[1].flags = I2C_M_RD;
+ msgs[1].len = receiveSize;
+ msgs[1].buf = dataReceived;
+
+ struct i2c_rdwr_ioctl_data rdwr;
+ rdwr.msgs = msgs;
+ rdwr.nmsgs = 2;
+
+ if (port == HAL_I2C_kOnboard) {
+ std::scoped_lock lock(digitalI2COnBoardMutex);
+ return ioctl(i2COnBoardHandle, I2C_RDWR, &rdwr);
+ } else {
+ std::scoped_lock lock(digitalI2CMXPMutex);
+ return ioctl(i2CMXPHandle, I2C_RDWR, &rdwr);
+ }
+}
+
+int32_t HAL_WriteI2C(HAL_I2CPort port, int32_t deviceAddress,
+ const uint8_t* dataToSend, int32_t sendSize) {
+ if (port < 0 || port > 1) {
+ // Set port out of range error here
+ return -1;
+ }
+
+ struct i2c_msg msg;
+ msg.addr = deviceAddress;
+ msg.flags = 0;
+ msg.len = sendSize;
+ msg.buf = const_cast<uint8_t*>(dataToSend);
+
+ struct i2c_rdwr_ioctl_data rdwr;
+ rdwr.msgs = &msg;
+ rdwr.nmsgs = 1;
+
+ if (port == HAL_I2C_kOnboard) {
+ std::scoped_lock lock(digitalI2COnBoardMutex);
+ return ioctl(i2COnBoardHandle, I2C_RDWR, &rdwr);
+ } else {
+ std::scoped_lock lock(digitalI2CMXPMutex);
+ return ioctl(i2CMXPHandle, I2C_RDWR, &rdwr);
+ }
+}
+
+int32_t HAL_ReadI2C(HAL_I2CPort port, int32_t deviceAddress, uint8_t* buffer,
+ int32_t count) {
+ if (port < 0 || port > 1) {
+ // Set port out of range error here
+ return -1;
+ }
+
+ struct i2c_msg msg;
+ msg.addr = deviceAddress;
+ msg.flags = I2C_M_RD;
+ msg.len = count;
+ msg.buf = buffer;
+
+ struct i2c_rdwr_ioctl_data rdwr;
+ rdwr.msgs = &msg;
+ rdwr.nmsgs = 1;
+
+ if (port == HAL_I2C_kOnboard) {
+ std::scoped_lock lock(digitalI2COnBoardMutex);
+ return ioctl(i2COnBoardHandle, I2C_RDWR, &rdwr);
+ } else {
+ std::scoped_lock lock(digitalI2CMXPMutex);
+ return ioctl(i2CMXPHandle, I2C_RDWR, &rdwr);
+ }
+}
+
+void HAL_CloseI2C(HAL_I2CPort port) {
+ if (port < 0 || port > 1) {
+ // Set port out of range error here
+ return;
+ }
+
+ if (port == HAL_I2C_kOnboard) {
+ std::scoped_lock lock(digitalI2COnBoardMutex);
+ if (i2COnboardObjCount-- == 0) {
+ close(i2COnBoardHandle);
+ }
+ } else {
+ std::scoped_lock lock(digitalI2CMXPMutex);
+ if (i2CMXPObjCount-- == 0) {
+ close(i2CMXPHandle);
+ }
+ HAL_FreeDIOPort(i2CMXPDigitalHandle1);
+ HAL_FreeDIOPort(i2CMXPDigitalHandle2);
+ }
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/athena/Interrupts.cpp b/hal/src/main/native/athena/Interrupts.cpp
new file mode 100644
index 0000000..78d518c
--- /dev/null
+++ b/hal/src/main/native/athena/Interrupts.cpp
@@ -0,0 +1,271 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 "hal/Interrupts.h"
+
+#include <memory>
+
+#include <wpi/SafeThread.h>
+
+#include "DigitalInternal.h"
+#include "HALInitializer.h"
+#include "PortsInternal.h"
+#include "hal/ChipObject.h"
+#include "hal/Errors.h"
+#include "hal/handles/HandlesInternal.h"
+#include "hal/handles/LimitedHandleResource.h"
+
+using namespace hal;
+
+namespace {
+// Safe thread to allow callbacks to run on their own thread
+class InterruptThread : public wpi::SafeThread {
+ public:
+ void Main() {
+ std::unique_lock lock(m_mutex);
+ while (m_active) {
+ m_cond.wait(lock, [&] { return !m_active || m_notify; });
+ if (!m_active) break;
+ m_notify = false;
+ HAL_InterruptHandlerFunction handler = m_handler;
+ uint32_t mask = m_mask;
+ void* param = m_param;
+ lock.unlock(); // don't hold mutex during callback execution
+ handler(mask, param);
+ lock.lock();
+ }
+ }
+
+ bool m_notify = false;
+ HAL_InterruptHandlerFunction m_handler;
+ void* m_param;
+ uint32_t m_mask;
+};
+
+class InterruptThreadOwner : public wpi::SafeThreadOwner<InterruptThread> {
+ public:
+ void SetFunc(HAL_InterruptHandlerFunction handler, void* param) {
+ auto thr = GetThread();
+ if (!thr) return;
+ thr->m_handler = handler;
+ thr->m_param = param;
+ }
+
+ void Notify(uint32_t mask) {
+ auto thr = GetThread();
+ if (!thr) return;
+ thr->m_mask = mask;
+ thr->m_notify = true;
+ thr->m_cond.notify_one();
+ }
+};
+
+struct Interrupt {
+ std::unique_ptr<tInterrupt> anInterrupt;
+ std::unique_ptr<tInterruptManager> manager;
+ std::unique_ptr<InterruptThreadOwner> threadOwner = nullptr;
+ void* param = nullptr;
+};
+
+} // namespace
+
+static void threadedInterruptHandler(uint32_t mask, void* param) {
+ static_cast<InterruptThreadOwner*>(param)->Notify(mask);
+}
+
+static LimitedHandleResource<HAL_InterruptHandle, Interrupt, kNumInterrupts,
+ HAL_HandleEnum::Interrupt>* interruptHandles;
+
+namespace hal {
+namespace init {
+void InitialzeInterrupts() {
+ static LimitedHandleResource<HAL_InterruptHandle, Interrupt, kNumInterrupts,
+ HAL_HandleEnum::Interrupt>
+ iH;
+ interruptHandles = &iH;
+}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+
+HAL_InterruptHandle HAL_InitializeInterrupts(HAL_Bool watcher,
+ int32_t* status) {
+ hal::init::CheckInit();
+ HAL_InterruptHandle handle = interruptHandles->Allocate();
+ if (handle == HAL_kInvalidHandle) {
+ *status = NO_AVAILABLE_RESOURCES;
+ return HAL_kInvalidHandle;
+ }
+ auto anInterrupt = interruptHandles->Get(handle);
+ uint32_t interruptIndex = static_cast<uint32_t>(getHandleIndex(handle));
+ // Expects the calling leaf class to allocate an interrupt index.
+ anInterrupt->anInterrupt.reset(tInterrupt::create(interruptIndex, status));
+ anInterrupt->anInterrupt->writeConfig_WaitForAck(false, status);
+ anInterrupt->manager = std::make_unique<tInterruptManager>(
+ (1u << interruptIndex) | (1u << (interruptIndex + 8u)), watcher, status);
+ return handle;
+}
+
+void* HAL_CleanInterrupts(HAL_InterruptHandle interruptHandle,
+ int32_t* status) {
+ auto anInterrupt = interruptHandles->Get(interruptHandle);
+ interruptHandles->Free(interruptHandle);
+ if (anInterrupt == nullptr) {
+ return nullptr;
+ }
+
+ if (anInterrupt->manager->isEnabled(status)) {
+ anInterrupt->manager->disable(status);
+ }
+
+ void* param = anInterrupt->param;
+ return param;
+}
+
+int64_t HAL_WaitForInterrupt(HAL_InterruptHandle interruptHandle,
+ double timeout, HAL_Bool ignorePrevious,
+ int32_t* status) {
+ uint32_t result;
+ auto anInterrupt = interruptHandles->Get(interruptHandle);
+ if (anInterrupt == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+
+ result = anInterrupt->manager->watch(static_cast<int32_t>(timeout * 1e3),
+ ignorePrevious, status);
+
+ // Don't report a timeout as an error - the return code is enough to tell
+ // that a timeout happened.
+ if (*status == -NiFpga_Status_IrqTimeout) {
+ *status = NiFpga_Status_Success;
+ }
+
+ return result;
+}
+
+void HAL_EnableInterrupts(HAL_InterruptHandle interruptHandle,
+ int32_t* status) {
+ auto anInterrupt = interruptHandles->Get(interruptHandle);
+ if (anInterrupt == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (!anInterrupt->manager->isEnabled(status)) {
+ anInterrupt->manager->enable(status);
+ }
+}
+
+void HAL_DisableInterrupts(HAL_InterruptHandle interruptHandle,
+ int32_t* status) {
+ auto anInterrupt = interruptHandles->Get(interruptHandle);
+ if (anInterrupt == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ if (anInterrupt->manager->isEnabled(status)) {
+ anInterrupt->manager->disable(status);
+ }
+}
+
+int64_t HAL_ReadInterruptRisingTimestamp(HAL_InterruptHandle interruptHandle,
+ int32_t* status) {
+ auto anInterrupt = interruptHandles->Get(interruptHandle);
+ if (anInterrupt == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+ uint32_t timestamp = anInterrupt->anInterrupt->readRisingTimeStamp(status);
+ return timestamp;
+}
+
+int64_t HAL_ReadInterruptFallingTimestamp(HAL_InterruptHandle interruptHandle,
+ int32_t* status) {
+ auto anInterrupt = interruptHandles->Get(interruptHandle);
+ if (anInterrupt == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+ uint32_t timestamp = anInterrupt->anInterrupt->readFallingTimeStamp(status);
+ return timestamp;
+}
+
+void HAL_RequestInterrupts(HAL_InterruptHandle interruptHandle,
+ HAL_Handle digitalSourceHandle,
+ HAL_AnalogTriggerType analogTriggerType,
+ int32_t* status) {
+ auto anInterrupt = interruptHandles->Get(interruptHandle);
+ if (anInterrupt == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ anInterrupt->anInterrupt->writeConfig_WaitForAck(false, status);
+ bool routingAnalogTrigger = false;
+ uint8_t routingChannel = 0;
+ uint8_t routingModule = 0;
+ bool success =
+ remapDigitalSource(digitalSourceHandle, analogTriggerType, routingChannel,
+ routingModule, routingAnalogTrigger);
+ if (!success) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ anInterrupt->anInterrupt->writeConfig_Source_AnalogTrigger(
+ routingAnalogTrigger, status);
+ anInterrupt->anInterrupt->writeConfig_Source_Channel(routingChannel, status);
+ anInterrupt->anInterrupt->writeConfig_Source_Module(routingModule, status);
+}
+
+void HAL_AttachInterruptHandler(HAL_InterruptHandle interruptHandle,
+ HAL_InterruptHandlerFunction handler,
+ void* param, int32_t* status) {
+ auto anInterrupt = interruptHandles->Get(interruptHandle);
+ if (anInterrupt == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ anInterrupt->manager->registerHandler(handler, param, status);
+ anInterrupt->param = param;
+}
+
+void HAL_AttachInterruptHandlerThreaded(HAL_InterruptHandle interrupt_handle,
+ HAL_InterruptHandlerFunction handler,
+ void* param, int32_t* status) {
+ auto anInterrupt = interruptHandles->Get(interrupt_handle);
+ if (anInterrupt == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ anInterrupt->threadOwner = std::make_unique<InterruptThreadOwner>();
+ anInterrupt->threadOwner->Start();
+ anInterrupt->threadOwner->SetFunc(handler, param);
+
+ HAL_AttachInterruptHandler(interrupt_handle, threadedInterruptHandler,
+ anInterrupt->threadOwner.get(), status);
+
+ if (*status != 0) {
+ anInterrupt->threadOwner = nullptr;
+ }
+ anInterrupt->param = param;
+}
+
+void HAL_SetInterruptUpSourceEdge(HAL_InterruptHandle interruptHandle,
+ HAL_Bool risingEdge, HAL_Bool fallingEdge,
+ int32_t* status) {
+ auto anInterrupt = interruptHandles->Get(interruptHandle);
+ if (anInterrupt == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ anInterrupt->anInterrupt->writeConfig_RisingEdge(risingEdge, status);
+ anInterrupt->anInterrupt->writeConfig_FallingEdge(fallingEdge, status);
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/athena/Notifier.cpp b/hal/src/main/native/athena/Notifier.cpp
new file mode 100644
index 0000000..c30e8d1
--- /dev/null
+++ b/hal/src/main/native/athena/Notifier.cpp
@@ -0,0 +1,236 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 "hal/Notifier.h"
+
+#include <atomic>
+#include <cstdlib> // For std::atexit()
+#include <memory>
+
+#include <wpi/condition_variable.h>
+#include <wpi/mutex.h>
+
+#include "HALInitializer.h"
+#include "hal/ChipObject.h"
+#include "hal/Errors.h"
+#include "hal/HAL.h"
+#include "hal/handles/UnlimitedHandleResource.h"
+
+using namespace hal;
+
+static constexpr int32_t kTimerInterruptNumber = 28;
+
+static wpi::mutex notifierMutex;
+static std::unique_ptr<tAlarm> notifierAlarm;
+static std::unique_ptr<tInterruptManager> notifierManager;
+static uint64_t closestTrigger{UINT64_MAX};
+
+namespace {
+
+struct Notifier {
+ uint64_t triggerTime = UINT64_MAX;
+ uint64_t triggeredTime = UINT64_MAX;
+ bool active = true;
+ wpi::mutex mutex;
+ wpi::condition_variable cond;
+};
+
+} // namespace
+
+static std::atomic_flag notifierAtexitRegistered{ATOMIC_FLAG_INIT};
+static std::atomic_int notifierRefCount{0};
+
+using namespace hal;
+
+class NotifierHandleContainer
+ : public UnlimitedHandleResource<HAL_NotifierHandle, Notifier,
+ HAL_HandleEnum::Notifier> {
+ public:
+ ~NotifierHandleContainer() {
+ ForEach([](HAL_NotifierHandle handle, Notifier* notifier) {
+ {
+ std::scoped_lock lock(notifier->mutex);
+ notifier->triggerTime = UINT64_MAX;
+ notifier->triggeredTime = 0;
+ notifier->active = false;
+ }
+ notifier->cond.notify_all(); // wake up any waiting threads
+ });
+ }
+};
+
+static NotifierHandleContainer* notifierHandles;
+
+static void alarmCallback(uint32_t, void*) {
+ std::scoped_lock lock(notifierMutex);
+ int32_t status = 0;
+ uint64_t currentTime = 0;
+
+ // the hardware disables itself after each alarm
+ closestTrigger = UINT64_MAX;
+
+ // process all notifiers
+ notifierHandles->ForEach([&](HAL_NotifierHandle handle, Notifier* notifier) {
+ if (notifier->triggerTime == UINT64_MAX) return;
+ if (currentTime == 0) currentTime = HAL_GetFPGATime(&status);
+ std::unique_lock lock(notifier->mutex);
+ if (notifier->triggerTime < currentTime) {
+ notifier->triggerTime = UINT64_MAX;
+ notifier->triggeredTime = currentTime;
+ lock.unlock();
+ notifier->cond.notify_all();
+ } else if (notifier->triggerTime < closestTrigger) {
+ closestTrigger = notifier->triggerTime;
+ }
+ });
+
+ if (notifierAlarm && closestTrigger != UINT64_MAX) {
+ // Simply truncate the hardware trigger time to 32-bit.
+ notifierAlarm->writeTriggerTime(static_cast<uint32_t>(closestTrigger),
+ &status);
+ // Enable the alarm. The hardware disables itself after each alarm.
+ notifierAlarm->writeEnable(true, &status);
+ }
+}
+
+static void cleanupNotifierAtExit() {
+ notifierAlarm = nullptr;
+ notifierManager = nullptr;
+}
+
+namespace hal {
+namespace init {
+void InitializeNotifier() {
+ static NotifierHandleContainer nH;
+ notifierHandles = &nH;
+}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+
+HAL_NotifierHandle HAL_InitializeNotifier(int32_t* status) {
+ hal::init::CheckInit();
+ if (!notifierAtexitRegistered.test_and_set())
+ std::atexit(cleanupNotifierAtExit);
+
+ if (notifierRefCount.fetch_add(1) == 0) {
+ std::scoped_lock lock(notifierMutex);
+ // create manager and alarm if not already created
+ if (!notifierManager) {
+ notifierManager = std::make_unique<tInterruptManager>(
+ 1 << kTimerInterruptNumber, false, status);
+ notifierManager->registerHandler(alarmCallback, nullptr, status);
+ notifierManager->enable(status);
+ }
+ if (!notifierAlarm) notifierAlarm.reset(tAlarm::create(status));
+ }
+
+ std::shared_ptr<Notifier> notifier = std::make_shared<Notifier>();
+ HAL_NotifierHandle handle = notifierHandles->Allocate(notifier);
+ if (handle == HAL_kInvalidHandle) {
+ *status = HAL_HANDLE_ERROR;
+ return HAL_kInvalidHandle;
+ }
+ return handle;
+}
+
+void HAL_SetNotifierName(HAL_NotifierHandle notifierHandle, const char* name,
+ int32_t* status) {}
+
+void HAL_StopNotifier(HAL_NotifierHandle notifierHandle, int32_t* status) {
+ auto notifier = notifierHandles->Get(notifierHandle);
+ if (!notifier) return;
+
+ {
+ std::scoped_lock lock(notifier->mutex);
+ notifier->triggerTime = UINT64_MAX;
+ notifier->triggeredTime = 0;
+ notifier->active = false;
+ }
+ notifier->cond.notify_all(); // wake up any waiting threads
+}
+
+void HAL_CleanNotifier(HAL_NotifierHandle notifierHandle, int32_t* status) {
+ auto notifier = notifierHandles->Free(notifierHandle);
+ if (!notifier) return;
+
+ // Just in case HAL_StopNotifier() wasn't called...
+ {
+ std::scoped_lock lock(notifier->mutex);
+ notifier->triggerTime = UINT64_MAX;
+ notifier->triggeredTime = 0;
+ notifier->active = false;
+ }
+ notifier->cond.notify_all();
+
+ if (notifierRefCount.fetch_sub(1) == 1) {
+ // if this was the last notifier, clean up alarm and manager
+ // the notifier can call back into our callback, so don't hold the lock
+ // here (the atomic fetch_sub will prevent multiple parallel entries
+ // into this function)
+
+ // Cleaning up the manager takes up to a second to complete, so don't do
+ // that here. Fix it more permanently in 2019...
+
+ // if (notifierAlarm) notifierAlarm->writeEnable(false, status);
+ // if (notifierManager) notifierManager->disable(status);
+
+ // std::scoped_lock lock(notifierMutex);
+ // notifierAlarm = nullptr;
+ // notifierManager = nullptr;
+ // closestTrigger = UINT64_MAX;
+ }
+}
+
+void HAL_UpdateNotifierAlarm(HAL_NotifierHandle notifierHandle,
+ uint64_t triggerTime, int32_t* status) {
+ auto notifier = notifierHandles->Get(notifierHandle);
+ if (!notifier) return;
+
+ {
+ std::scoped_lock lock(notifier->mutex);
+ notifier->triggerTime = triggerTime;
+ notifier->triggeredTime = UINT64_MAX;
+ }
+
+ std::scoped_lock lock(notifierMutex);
+ // Update alarm time if closer than current.
+ if (triggerTime < closestTrigger) {
+ bool wasActive = (closestTrigger != UINT64_MAX);
+ closestTrigger = triggerTime;
+ // Simply truncate the hardware trigger time to 32-bit.
+ notifierAlarm->writeTriggerTime(static_cast<uint32_t>(closestTrigger),
+ status);
+ // Enable the alarm.
+ if (!wasActive) notifierAlarm->writeEnable(true, status);
+ }
+}
+
+void HAL_CancelNotifierAlarm(HAL_NotifierHandle notifierHandle,
+ int32_t* status) {
+ auto notifier = notifierHandles->Get(notifierHandle);
+ if (!notifier) return;
+
+ {
+ std::scoped_lock lock(notifier->mutex);
+ notifier->triggerTime = UINT64_MAX;
+ }
+}
+
+uint64_t HAL_WaitForNotifierAlarm(HAL_NotifierHandle notifierHandle,
+ int32_t* status) {
+ auto notifier = notifierHandles->Get(notifierHandle);
+ if (!notifier) return 0;
+ std::unique_lock lock(notifier->mutex);
+ notifier->cond.wait(lock, [&] {
+ return !notifier->active || notifier->triggeredTime != UINT64_MAX;
+ });
+ return notifier->active ? notifier->triggeredTime : 0;
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/athena/PCMInternal.cpp b/hal/src/main/native/athena/PCMInternal.cpp
new file mode 100644
index 0000000..dee64cf
--- /dev/null
+++ b/hal/src/main/native/athena/PCMInternal.cpp
@@ -0,0 +1,38 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 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 "PCMInternal.h"
+
+#include "HALInitializer.h"
+#include "PortsInternal.h"
+#include "hal/Errors.h"
+#include "hal/Solenoid.h"
+
+namespace hal {
+
+std::unique_ptr<PCM> PCM_modules[kNumPCMModules];
+
+namespace init {
+void InitializePCMInternal() {
+ for (int i = 0; i < kNumPCMModules; i++) {
+ PCM_modules[i] = nullptr;
+ }
+}
+} // namespace init
+
+void initializePCM(int32_t module, int32_t* status) {
+ hal::init::CheckInit();
+ if (!HAL_CheckSolenoidModule(module)) {
+ *status = RESOURCE_OUT_OF_RANGE;
+ return;
+ }
+ if (!PCM_modules[module]) {
+ PCM_modules[module] = std::make_unique<PCM>(module);
+ }
+}
+
+} // namespace hal
diff --git a/hal/src/main/native/athena/PCMInternal.h b/hal/src/main/native/athena/PCMInternal.h
new file mode 100644
index 0000000..52f0f75
--- /dev/null
+++ b/hal/src/main/native/athena/PCMInternal.h
@@ -0,0 +1,38 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 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 <stdint.h>
+
+#include <memory>
+
+#include "PortsInternal.h"
+#include "ctre/PCM.h"
+#include "hal/Errors.h"
+#include "hal/Ports.h"
+#include "hal/Solenoid.h"
+
+namespace hal {
+
+extern std::unique_ptr<PCM> PCM_modules[kNumPCMModules];
+
+static inline bool checkPCMInit(int32_t module, int32_t* status) {
+ if (!HAL_CheckSolenoidModule(module)) {
+ *status = RESOURCE_OUT_OF_RANGE;
+ return false;
+ }
+ if (!PCM_modules[module]) {
+ *status = INCOMPATIBLE_STATE;
+ return false;
+ }
+ return true;
+}
+
+void initializePCM(int32_t module, int32_t* status);
+
+} // namespace hal
diff --git a/hal/src/main/native/athena/PDP.cpp b/hal/src/main/native/athena/PDP.cpp
new file mode 100644
index 0000000..f5cf92b
--- /dev/null
+++ b/hal/src/main/native/athena/PDP.cpp
@@ -0,0 +1,454 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 "hal/PDP.h"
+
+#include <memory>
+
+#include <wpi/mutex.h>
+
+#include "HALInitializer.h"
+#include "PortsInternal.h"
+#include "hal/CANAPI.h"
+#include "hal/Errors.h"
+#include "hal/Ports.h"
+#include "hal/handles/IndexedHandleResource.h"
+
+using namespace hal;
+
+static constexpr HAL_CANManufacturer manufacturer =
+ HAL_CANManufacturer::HAL_CAN_Man_kCTRE;
+
+static constexpr HAL_CANDeviceType deviceType =
+ HAL_CANDeviceType::HAL_CAN_Dev_kPowerDistribution;
+
+static constexpr int32_t Status1 = 0x50;
+static constexpr int32_t Status2 = 0x51;
+static constexpr int32_t Status3 = 0x52;
+static constexpr int32_t StatusEnergy = 0x5D;
+
+static constexpr int32_t Control1 = 0x70;
+
+static constexpr int32_t TimeoutMs = 100;
+
+/* encoder/decoders */
+union PdpStatus1 {
+ uint8_t data[8];
+ struct Bits {
+ unsigned chan1_h8 : 8;
+ unsigned chan2_h6 : 6;
+ unsigned chan1_l2 : 2;
+ unsigned chan3_h4 : 4;
+ unsigned chan2_l4 : 4;
+ unsigned chan4_h2 : 2;
+ unsigned chan3_l6 : 6;
+ unsigned chan4_l8 : 8;
+ unsigned chan5_h8 : 8;
+ unsigned chan6_h6 : 6;
+ unsigned chan5_l2 : 2;
+ unsigned reserved4 : 4;
+ unsigned chan6_l4 : 4;
+ } bits;
+};
+
+union PdpStatus2 {
+ uint8_t data[8];
+ struct Bits {
+ unsigned chan7_h8 : 8;
+ unsigned chan8_h6 : 6;
+ unsigned chan7_l2 : 2;
+ unsigned chan9_h4 : 4;
+ unsigned chan8_l4 : 4;
+ unsigned chan10_h2 : 2;
+ unsigned chan9_l6 : 6;
+ unsigned chan10_l8 : 8;
+ unsigned chan11_h8 : 8;
+ unsigned chan12_h6 : 6;
+ unsigned chan11_l2 : 2;
+ unsigned reserved4 : 4;
+ unsigned chan12_l4 : 4;
+ } bits;
+};
+
+union PdpStatus3 {
+ uint8_t data[8];
+ struct Bits {
+ unsigned chan13_h8 : 8;
+ unsigned chan14_h6 : 6;
+ unsigned chan13_l2 : 2;
+ unsigned chan15_h4 : 4;
+ unsigned chan14_l4 : 4;
+ unsigned chan16_h2 : 2;
+ unsigned chan15_l6 : 6;
+ unsigned chan16_l8 : 8;
+ unsigned internalResBattery_mOhms : 8;
+ unsigned busVoltage : 8;
+ unsigned temp : 8;
+ } bits;
+};
+
+union PdpStatusEnergy {
+ uint8_t data[8];
+ struct Bits {
+ unsigned TmeasMs_likelywillbe20ms_ : 8;
+ unsigned TotalCurrent_125mAperunit_h8 : 8;
+ unsigned Power_125mWperunit_h4 : 4;
+ unsigned TotalCurrent_125mAperunit_l4 : 4;
+ unsigned Power_125mWperunit_m8 : 8;
+ unsigned Energy_125mWPerUnitXTmeas_h4 : 4;
+ unsigned Power_125mWperunit_l4 : 4;
+ unsigned Energy_125mWPerUnitXTmeas_mh8 : 8;
+ unsigned Energy_125mWPerUnitXTmeas_ml8 : 8;
+ unsigned Energy_125mWPerUnitXTmeas_l8 : 8;
+ } bits;
+};
+
+static wpi::mutex pdpHandleMutex;
+static HAL_PDPHandle pdpHandles[kNumPDPModules];
+
+namespace hal {
+namespace init {
+void InitializePDP() {
+ for (int i = 0; i < kNumPDPModules; i++) {
+ pdpHandles[i] = HAL_kInvalidHandle;
+ }
+}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+
+HAL_PDPHandle HAL_InitializePDP(int32_t module, int32_t* status) {
+ hal::init::CheckInit();
+ if (!HAL_CheckPDPModule(module)) {
+ *status = PARAMETER_OUT_OF_RANGE;
+ return HAL_kInvalidHandle;
+ }
+
+ std::scoped_lock lock(pdpHandleMutex);
+
+ if (pdpHandles[module] != HAL_kInvalidHandle) {
+ *status = 0;
+ return pdpHandles[module];
+ }
+
+ auto handle = HAL_InitializeCAN(manufacturer, module, deviceType, status);
+
+ if (*status != 0) {
+ HAL_CleanCAN(handle);
+ return HAL_kInvalidHandle;
+ }
+
+ pdpHandles[module] = handle;
+
+ return handle;
+}
+
+void HAL_CleanPDP(HAL_PDPHandle handle) {
+ HAL_CleanCAN(handle);
+
+ for (int i = 0; i < kNumPDPModules; i++) {
+ if (pdpHandles[i] == handle) {
+ pdpHandles[i] = HAL_kInvalidHandle;
+ return;
+ }
+ }
+}
+
+HAL_Bool HAL_CheckPDPModule(int32_t module) {
+ return module < kNumPDPModules && module >= 0;
+}
+
+HAL_Bool HAL_CheckPDPChannel(int32_t channel) {
+ return channel < kNumPDPChannels && channel >= 0;
+}
+
+double HAL_GetPDPTemperature(HAL_PDPHandle handle, int32_t* status) {
+ PdpStatus3 pdpStatus;
+ int32_t length = 0;
+ uint64_t receivedTimestamp = 0;
+
+ HAL_ReadCANPacketTimeout(handle, Status3, pdpStatus.data, &length,
+ &receivedTimestamp, TimeoutMs, status);
+
+ if (*status != 0) {
+ return 0;
+ } else {
+ return pdpStatus.bits.temp * 1.03250836957542 - 67.8564500484966;
+ }
+}
+
+double HAL_GetPDPVoltage(HAL_PDPHandle handle, int32_t* status) {
+ PdpStatus3 pdpStatus;
+ int32_t length = 0;
+ uint64_t receivedTimestamp = 0;
+
+ HAL_ReadCANPacketTimeout(handle, Status3, pdpStatus.data, &length,
+ &receivedTimestamp, TimeoutMs, status);
+
+ if (*status != 0) {
+ return 0;
+ } else {
+ return pdpStatus.bits.busVoltage * 0.05 + 4.0; /* 50mV per unit plus 4V. */
+ }
+}
+
+double HAL_GetPDPChannelCurrent(HAL_PDPHandle handle, int32_t channel,
+ int32_t* status) {
+ if (!HAL_CheckPDPChannel(channel)) {
+ *status = PARAMETER_OUT_OF_RANGE;
+ return 0;
+ }
+
+ int32_t length = 0;
+ uint64_t receivedTimestamp = 0;
+
+ double raw = 0;
+
+ if (channel <= 5) {
+ PdpStatus1 pdpStatus;
+ HAL_ReadCANPacketTimeout(handle, Status1, pdpStatus.data, &length,
+ &receivedTimestamp, TimeoutMs, status);
+ if (*status != 0) {
+ return 0;
+ }
+ switch (channel) {
+ case 0:
+ raw = (static_cast<uint32_t>(pdpStatus.bits.chan1_h8) << 2) |
+ pdpStatus.bits.chan1_l2;
+ break;
+ case 1:
+ raw = (static_cast<uint32_t>(pdpStatus.bits.chan2_h6) << 4) |
+ pdpStatus.bits.chan2_l4;
+ break;
+ case 2:
+ raw = (static_cast<uint32_t>(pdpStatus.bits.chan3_h4) << 6) |
+ pdpStatus.bits.chan3_l6;
+ break;
+ case 3:
+ raw = (static_cast<uint32_t>(pdpStatus.bits.chan4_h2) << 8) |
+ pdpStatus.bits.chan4_l8;
+ break;
+ case 4:
+ raw = (static_cast<uint32_t>(pdpStatus.bits.chan5_h8) << 2) |
+ pdpStatus.bits.chan5_l2;
+ break;
+ case 5:
+ raw = (static_cast<uint32_t>(pdpStatus.bits.chan6_h6) << 4) |
+ pdpStatus.bits.chan6_l4;
+ break;
+ }
+ } else if (channel <= 11) {
+ PdpStatus2 pdpStatus;
+ HAL_ReadCANPacketTimeout(handle, Status2, pdpStatus.data, &length,
+ &receivedTimestamp, TimeoutMs, status);
+ if (*status != 0) {
+ return 0;
+ }
+ switch (channel) {
+ case 6:
+ raw = (static_cast<uint32_t>(pdpStatus.bits.chan7_h8) << 2) |
+ pdpStatus.bits.chan7_l2;
+ break;
+ case 7:
+ raw = (static_cast<uint32_t>(pdpStatus.bits.chan8_h6) << 4) |
+ pdpStatus.bits.chan8_l4;
+ break;
+ case 8:
+ raw = (static_cast<uint32_t>(pdpStatus.bits.chan9_h4) << 6) |
+ pdpStatus.bits.chan9_l6;
+ break;
+ case 9:
+ raw = (static_cast<uint32_t>(pdpStatus.bits.chan10_h2) << 8) |
+ pdpStatus.bits.chan10_l8;
+ break;
+ case 10:
+ raw = (static_cast<uint32_t>(pdpStatus.bits.chan11_h8) << 2) |
+ pdpStatus.bits.chan11_l2;
+ break;
+ case 11:
+ raw = (static_cast<uint32_t>(pdpStatus.bits.chan12_h6) << 4) |
+ pdpStatus.bits.chan12_l4;
+ break;
+ }
+ } else {
+ PdpStatus3 pdpStatus;
+ HAL_ReadCANPacketTimeout(handle, Status3, pdpStatus.data, &length,
+ &receivedTimestamp, TimeoutMs, status);
+ if (*status != 0) {
+ return 0;
+ }
+ switch (channel) {
+ case 12:
+ raw = (static_cast<uint32_t>(pdpStatus.bits.chan13_h8) << 2) |
+ pdpStatus.bits.chan13_l2;
+ break;
+ case 13:
+ raw = (static_cast<uint32_t>(pdpStatus.bits.chan14_h6) << 4) |
+ pdpStatus.bits.chan14_l4;
+ break;
+ case 14:
+ raw = (static_cast<uint32_t>(pdpStatus.bits.chan15_h4) << 6) |
+ pdpStatus.bits.chan15_l6;
+ break;
+ case 15:
+ raw = (static_cast<uint32_t>(pdpStatus.bits.chan16_h2) << 8) |
+ pdpStatus.bits.chan16_l8;
+ break;
+ }
+ }
+
+ /* convert to amps */
+ return raw * 0.125; /* 7.3 fixed pt value in Amps */
+}
+
+void HAL_GetPDPAllChannelCurrents(HAL_PDPHandle handle, double* currents,
+ int32_t* status) {
+ int32_t length = 0;
+ uint64_t receivedTimestamp = 0;
+ PdpStatus1 pdpStatus;
+ HAL_ReadCANPacketTimeout(handle, Status1, pdpStatus.data, &length,
+ &receivedTimestamp, TimeoutMs, status);
+ if (*status != 0) return;
+ PdpStatus2 pdpStatus2;
+ HAL_ReadCANPacketTimeout(handle, Status2, pdpStatus2.data, &length,
+ &receivedTimestamp, TimeoutMs, status);
+ if (*status != 0) return;
+ PdpStatus3 pdpStatus3;
+ HAL_ReadCANPacketTimeout(handle, Status3, pdpStatus3.data, &length,
+ &receivedTimestamp, TimeoutMs, status);
+ if (*status != 0) return;
+
+ currents[0] = ((static_cast<uint32_t>(pdpStatus.bits.chan1_h8) << 2) |
+ pdpStatus.bits.chan1_l2) *
+ 0.125;
+ currents[1] = ((static_cast<uint32_t>(pdpStatus.bits.chan2_h6) << 4) |
+ pdpStatus.bits.chan2_l4) *
+ 0.125;
+ currents[2] = ((static_cast<uint32_t>(pdpStatus.bits.chan3_h4) << 6) |
+ pdpStatus.bits.chan3_l6) *
+ 0.125;
+ currents[3] = ((static_cast<uint32_t>(pdpStatus.bits.chan4_h2) << 8) |
+ pdpStatus.bits.chan4_l8) *
+ 0.125;
+ currents[4] = ((static_cast<uint32_t>(pdpStatus.bits.chan5_h8) << 2) |
+ pdpStatus.bits.chan5_l2) *
+ 0.125;
+ currents[5] = ((static_cast<uint32_t>(pdpStatus.bits.chan6_h6) << 4) |
+ pdpStatus.bits.chan6_l4) *
+ 0.125;
+
+ currents[6] = ((static_cast<uint32_t>(pdpStatus2.bits.chan7_h8) << 2) |
+ pdpStatus2.bits.chan7_l2) *
+ 0.125;
+ currents[7] = ((static_cast<uint32_t>(pdpStatus2.bits.chan8_h6) << 4) |
+ pdpStatus2.bits.chan8_l4) *
+ 0.125;
+ currents[8] = ((static_cast<uint32_t>(pdpStatus2.bits.chan9_h4) << 6) |
+ pdpStatus2.bits.chan9_l6) *
+ 0.125;
+ currents[9] = ((static_cast<uint32_t>(pdpStatus2.bits.chan10_h2) << 8) |
+ pdpStatus2.bits.chan10_l8) *
+ 0.125;
+ currents[10] = ((static_cast<uint32_t>(pdpStatus2.bits.chan11_h8) << 2) |
+ pdpStatus2.bits.chan11_l2) *
+ 0.125;
+ currents[11] = ((static_cast<uint32_t>(pdpStatus2.bits.chan12_h6) << 4) |
+ pdpStatus2.bits.chan12_l4) *
+ 0.125;
+
+ currents[12] = ((static_cast<uint32_t>(pdpStatus3.bits.chan13_h8) << 2) |
+ pdpStatus3.bits.chan13_l2) *
+ 0.125;
+ currents[13] = ((static_cast<uint32_t>(pdpStatus3.bits.chan14_h6) << 4) |
+ pdpStatus3.bits.chan14_l4) *
+ 0.125;
+ currents[14] = ((static_cast<uint32_t>(pdpStatus3.bits.chan15_h4) << 6) |
+ pdpStatus3.bits.chan15_l6) *
+ 0.125;
+ currents[15] = ((static_cast<uint32_t>(pdpStatus3.bits.chan16_h2) << 8) |
+ pdpStatus3.bits.chan16_l8) *
+ 0.125;
+}
+
+double HAL_GetPDPTotalCurrent(HAL_PDPHandle handle, int32_t* status) {
+ PdpStatusEnergy pdpStatus;
+ int32_t length = 0;
+ uint64_t receivedTimestamp = 0;
+
+ HAL_ReadCANPacketTimeout(handle, StatusEnergy, pdpStatus.data, &length,
+ &receivedTimestamp, TimeoutMs, status);
+ if (*status != 0) {
+ return 0;
+ }
+
+ uint32_t raw;
+ raw = pdpStatus.bits.TotalCurrent_125mAperunit_h8;
+ raw <<= 4;
+ raw |= pdpStatus.bits.TotalCurrent_125mAperunit_l4;
+ return 0.125 * raw; /* 7.3 fixed pt value in Amps */
+}
+
+double HAL_GetPDPTotalPower(HAL_PDPHandle handle, int32_t* status) {
+ PdpStatusEnergy pdpStatus;
+ int32_t length = 0;
+ uint64_t receivedTimestamp = 0;
+
+ HAL_ReadCANPacketTimeout(handle, StatusEnergy, pdpStatus.data, &length,
+ &receivedTimestamp, TimeoutMs, status);
+ if (*status != 0) {
+ return 0;
+ }
+
+ uint32_t raw;
+ raw = pdpStatus.bits.Power_125mWperunit_h4;
+ raw <<= 8;
+ raw |= pdpStatus.bits.Power_125mWperunit_m8;
+ raw <<= 4;
+ raw |= pdpStatus.bits.Power_125mWperunit_l4;
+ return 0.125 * raw; /* 7.3 fixed pt value in Watts */
+}
+
+double HAL_GetPDPTotalEnergy(HAL_PDPHandle handle, int32_t* status) {
+ PdpStatusEnergy pdpStatus;
+ int32_t length = 0;
+ uint64_t receivedTimestamp = 0;
+
+ HAL_ReadCANPacketTimeout(handle, StatusEnergy, pdpStatus.data, &length,
+ &receivedTimestamp, TimeoutMs, status);
+ if (*status != 0) {
+ return 0;
+ }
+
+ uint32_t raw;
+ raw = pdpStatus.bits.Energy_125mWPerUnitXTmeas_h4;
+ raw <<= 8;
+ raw |= pdpStatus.bits.Energy_125mWPerUnitXTmeas_mh8;
+ raw <<= 8;
+ raw |= pdpStatus.bits.Energy_125mWPerUnitXTmeas_ml8;
+ raw <<= 8;
+ raw |= pdpStatus.bits.Energy_125mWPerUnitXTmeas_l8;
+
+ double energyJoules = 0.125 * raw; /* mW integrated every TmeasMs */
+ energyJoules *= 0.001; /* convert from mW to W */
+ energyJoules *=
+ pdpStatus.bits
+ .TmeasMs_likelywillbe20ms_; /* multiplied by TmeasMs = joules */
+ return 0.125 * raw;
+}
+
+void HAL_ResetPDPTotalEnergy(HAL_PDPHandle handle, int32_t* status) {
+ uint8_t pdpControl[] = {0x40}; /* only bit set is ResetEnergy */
+ HAL_WriteCANPacket(handle, pdpControl, 1, Control1, status);
+}
+
+void HAL_ClearPDPStickyFaults(HAL_PDPHandle handle, int32_t* status) {
+ uint8_t pdpControl[] = {0x80}; /* only bit set is ClearStickyFaults */
+ HAL_WriteCANPacket(handle, pdpControl, 1, Control1, status);
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/athena/PWM.cpp b/hal/src/main/native/athena/PWM.cpp
new file mode 100644
index 0000000..a3b141c
--- /dev/null
+++ b/hal/src/main/native/athena/PWM.cpp
@@ -0,0 +1,456 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 "hal/PWM.h"
+
+#include <cmath>
+#include <thread>
+
+#include <wpi/raw_ostream.h>
+
+#include "ConstantsInternal.h"
+#include "DigitalInternal.h"
+#include "HALInitializer.h"
+#include "PortsInternal.h"
+#include "hal/cpp/fpga_clock.h"
+#include "hal/handles/HandlesInternal.h"
+
+using namespace hal;
+
+static inline int32_t GetMaxPositivePwm(DigitalPort* port) {
+ return port->maxPwm;
+}
+
+static inline int32_t GetMinPositivePwm(DigitalPort* port) {
+ if (port->eliminateDeadband) {
+ return port->deadbandMaxPwm;
+ } else {
+ return port->centerPwm + 1;
+ }
+}
+
+static inline int32_t GetCenterPwm(DigitalPort* port) {
+ return port->centerPwm;
+}
+
+static inline int32_t GetMaxNegativePwm(DigitalPort* port) {
+ if (port->eliminateDeadband) {
+ return port->deadbandMinPwm;
+ } else {
+ return port->centerPwm - 1;
+ }
+}
+
+static inline int32_t GetMinNegativePwm(DigitalPort* port) {
+ return port->minPwm;
+}
+
+static inline int32_t GetPositiveScaleFactor(DigitalPort* port) {
+ return GetMaxPositivePwm(port) - GetMinPositivePwm(port);
+} ///< The scale for positive speeds.
+
+static inline int32_t GetNegativeScaleFactor(DigitalPort* port) {
+ return GetMaxNegativePwm(port) - GetMinNegativePwm(port);
+} ///< The scale for negative speeds.
+
+static inline int32_t GetFullRangeScaleFactor(DigitalPort* port) {
+ return GetMaxPositivePwm(port) - GetMinNegativePwm(port);
+} ///< The scale for positions.
+
+namespace hal {
+namespace init {
+void InitializePWM() {}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+
+HAL_DigitalHandle HAL_InitializePWMPort(HAL_PortHandle portHandle,
+ int32_t* status) {
+ hal::init::CheckInit();
+ initializeDigital(status);
+
+ if (*status != 0) return HAL_kInvalidHandle;
+
+ int16_t channel = getPortHandleChannel(portHandle);
+ if (channel == InvalidHandleIndex || channel >= kNumPWMChannels) {
+ *status = PARAMETER_OUT_OF_RANGE;
+ return HAL_kInvalidHandle;
+ }
+
+ uint8_t origChannel = static_cast<uint8_t>(channel);
+
+ if (origChannel < kNumPWMHeaders) {
+ channel += kNumDigitalChannels; // remap Headers to end of allocations
+ } else {
+ channel = remapMXPPWMChannel(channel) + 10; // remap MXP to proper channel
+ }
+
+ auto handle =
+ digitalChannelHandles->Allocate(channel, HAL_HandleEnum::PWM, status);
+
+ if (*status != 0)
+ return HAL_kInvalidHandle; // failed to allocate. Pass error back.
+
+ auto port = digitalChannelHandles->Get(handle, HAL_HandleEnum::PWM);
+ if (port == nullptr) { // would only occur on thread issue.
+ *status = HAL_HANDLE_ERROR;
+ return HAL_kInvalidHandle;
+ }
+
+ port->channel = origChannel;
+
+ if (port->channel > tPWM::kNumHdrRegisters - 1) {
+ int32_t bitToSet = 1 << remapMXPPWMChannel(port->channel);
+ uint16_t specialFunctions =
+ digitalSystem->readEnableMXPSpecialFunction(status);
+ digitalSystem->writeEnableMXPSpecialFunction(specialFunctions | bitToSet,
+ status);
+ }
+
+ // Defaults to allow an always valid config.
+ HAL_SetPWMConfig(handle, 2.0, 1.501, 1.5, 1.499, 1.0, status);
+
+ return handle;
+}
+void HAL_FreePWMPort(HAL_DigitalHandle pwmPortHandle, int32_t* status) {
+ auto port = digitalChannelHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ digitalChannelHandles->Free(pwmPortHandle, HAL_HandleEnum::PWM);
+
+ // Wait for no other object to hold this handle.
+ auto start = hal::fpga_clock::now();
+ while (port.use_count() != 1) {
+ auto current = hal::fpga_clock::now();
+ if (start + std::chrono::seconds(1) < current) {
+ wpi::outs() << "PWM handle free timeout\n";
+ wpi::outs().flush();
+ break;
+ }
+ std::this_thread::yield();
+ }
+
+ if (port->channel > tPWM::kNumHdrRegisters - 1) {
+ int32_t bitToUnset = 1 << remapMXPPWMChannel(port->channel);
+ uint16_t specialFunctions =
+ digitalSystem->readEnableMXPSpecialFunction(status);
+ digitalSystem->writeEnableMXPSpecialFunction(specialFunctions & ~bitToUnset,
+ status);
+ }
+}
+
+HAL_Bool HAL_CheckPWMChannel(int32_t channel) {
+ return channel < kNumPWMChannels && channel >= 0;
+}
+
+void HAL_SetPWMConfig(HAL_DigitalHandle pwmPortHandle, double max,
+ double deadbandMax, double center, double deadbandMin,
+ double min, int32_t* status) {
+ auto port = digitalChannelHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ // calculate the loop time in milliseconds
+ double loopTime =
+ HAL_GetPWMLoopTiming(status) / (kSystemClockTicksPerMicrosecond * 1e3);
+ if (*status != 0) return;
+
+ int32_t maxPwm = static_cast<int32_t>((max - kDefaultPwmCenter) / loopTime +
+ kDefaultPwmStepsDown - 1);
+ int32_t deadbandMaxPwm = static_cast<int32_t>(
+ (deadbandMax - kDefaultPwmCenter) / loopTime + kDefaultPwmStepsDown - 1);
+ int32_t centerPwm = static_cast<int32_t>(
+ (center - kDefaultPwmCenter) / loopTime + kDefaultPwmStepsDown - 1);
+ int32_t deadbandMinPwm = static_cast<int32_t>(
+ (deadbandMin - kDefaultPwmCenter) / loopTime + kDefaultPwmStepsDown - 1);
+ int32_t minPwm = static_cast<int32_t>((min - kDefaultPwmCenter) / loopTime +
+ kDefaultPwmStepsDown - 1);
+
+ port->maxPwm = maxPwm;
+ port->deadbandMaxPwm = deadbandMaxPwm;
+ port->deadbandMinPwm = deadbandMinPwm;
+ port->centerPwm = centerPwm;
+ port->minPwm = minPwm;
+ port->configSet = true;
+}
+
+void HAL_SetPWMConfigRaw(HAL_DigitalHandle pwmPortHandle, int32_t maxPwm,
+ int32_t deadbandMaxPwm, int32_t centerPwm,
+ int32_t deadbandMinPwm, int32_t minPwm,
+ int32_t* status) {
+ auto port = digitalChannelHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ port->maxPwm = maxPwm;
+ port->deadbandMaxPwm = deadbandMaxPwm;
+ port->deadbandMinPwm = deadbandMinPwm;
+ port->centerPwm = centerPwm;
+ port->minPwm = minPwm;
+}
+
+void HAL_GetPWMConfigRaw(HAL_DigitalHandle pwmPortHandle, int32_t* maxPwm,
+ int32_t* deadbandMaxPwm, int32_t* centerPwm,
+ int32_t* deadbandMinPwm, int32_t* minPwm,
+ int32_t* status) {
+ auto port = digitalChannelHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ *maxPwm = port->maxPwm;
+ *deadbandMaxPwm = port->deadbandMaxPwm;
+ *deadbandMinPwm = port->deadbandMinPwm;
+ *centerPwm = port->centerPwm;
+ *minPwm = port->minPwm;
+}
+
+void HAL_SetPWMEliminateDeadband(HAL_DigitalHandle pwmPortHandle,
+ HAL_Bool eliminateDeadband, int32_t* status) {
+ auto port = digitalChannelHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ port->eliminateDeadband = eliminateDeadband;
+}
+
+HAL_Bool HAL_GetPWMEliminateDeadband(HAL_DigitalHandle pwmPortHandle,
+ int32_t* status) {
+ auto port = digitalChannelHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return false;
+ }
+ return port->eliminateDeadband;
+}
+
+void HAL_SetPWMRaw(HAL_DigitalHandle pwmPortHandle, int32_t value,
+ int32_t* status) {
+ auto port = digitalChannelHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (port->channel < tPWM::kNumHdrRegisters) {
+ pwmSystem->writeHdr(port->channel, value, status);
+ } else {
+ pwmSystem->writeMXP(port->channel - tPWM::kNumHdrRegisters, value, status);
+ }
+}
+
+void HAL_SetPWMSpeed(HAL_DigitalHandle pwmPortHandle, double speed,
+ int32_t* status) {
+ auto port = digitalChannelHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ if (!port->configSet) {
+ *status = INCOMPATIBLE_STATE;
+ return;
+ }
+
+ DigitalPort* dPort = port.get();
+
+ if (std::isfinite(speed)) {
+ speed = std::clamp(speed, -1.0, 1.0);
+ } else {
+ speed = 0.0;
+ }
+
+ // calculate the desired output pwm value by scaling the speed appropriately
+ int32_t rawValue;
+ if (speed == 0.0) {
+ rawValue = GetCenterPwm(dPort);
+ } else if (speed > 0.0) {
+ rawValue = static_cast<int32_t>(
+ speed * static_cast<double>(GetPositiveScaleFactor(dPort)) +
+ static_cast<double>(GetMinPositivePwm(dPort)) + 0.5);
+ } else {
+ rawValue = static_cast<int32_t>(
+ speed * static_cast<double>(GetNegativeScaleFactor(dPort)) +
+ static_cast<double>(GetMaxNegativePwm(dPort)) + 0.5);
+ }
+
+ if (!((rawValue >= GetMinNegativePwm(dPort)) &&
+ (rawValue <= GetMaxPositivePwm(dPort))) ||
+ rawValue == kPwmDisabled) {
+ *status = HAL_PWM_SCALE_ERROR;
+ return;
+ }
+
+ HAL_SetPWMRaw(pwmPortHandle, rawValue, status);
+}
+
+void HAL_SetPWMPosition(HAL_DigitalHandle pwmPortHandle, double pos,
+ int32_t* status) {
+ auto port = digitalChannelHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ if (!port->configSet) {
+ *status = INCOMPATIBLE_STATE;
+ return;
+ }
+ DigitalPort* dPort = port.get();
+
+ if (pos < 0.0) {
+ pos = 0.0;
+ } else if (pos > 1.0) {
+ pos = 1.0;
+ }
+
+ // note, need to perform the multiplication below as floating point before
+ // converting to int
+ int32_t rawValue = static_cast<int32_t>(
+ (pos * static_cast<double>(GetFullRangeScaleFactor(dPort))) +
+ GetMinNegativePwm(dPort));
+
+ if (rawValue == kPwmDisabled) {
+ *status = HAL_PWM_SCALE_ERROR;
+ return;
+ }
+
+ HAL_SetPWMRaw(pwmPortHandle, rawValue, status);
+}
+
+void HAL_SetPWMDisabled(HAL_DigitalHandle pwmPortHandle, int32_t* status) {
+ HAL_SetPWMRaw(pwmPortHandle, kPwmDisabled, status);
+}
+
+int32_t HAL_GetPWMRaw(HAL_DigitalHandle pwmPortHandle, int32_t* status) {
+ auto port = digitalChannelHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+
+ if (port->channel < tPWM::kNumHdrRegisters) {
+ return pwmSystem->readHdr(port->channel, status);
+ } else {
+ return pwmSystem->readMXP(port->channel - tPWM::kNumHdrRegisters, status);
+ }
+}
+
+double HAL_GetPWMSpeed(HAL_DigitalHandle pwmPortHandle, int32_t* status) {
+ auto port = digitalChannelHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+ if (!port->configSet) {
+ *status = INCOMPATIBLE_STATE;
+ return 0;
+ }
+
+ int32_t value = HAL_GetPWMRaw(pwmPortHandle, status);
+ if (*status != 0) return 0;
+ DigitalPort* dPort = port.get();
+
+ if (value == kPwmDisabled) {
+ return 0.0;
+ } else if (value > GetMaxPositivePwm(dPort)) {
+ return 1.0;
+ } else if (value < GetMinNegativePwm(dPort)) {
+ return -1.0;
+ } else if (value > GetMinPositivePwm(dPort)) {
+ return static_cast<double>(value - GetMinPositivePwm(dPort)) /
+ static_cast<double>(GetPositiveScaleFactor(dPort));
+ } else if (value < GetMaxNegativePwm(dPort)) {
+ return static_cast<double>(value - GetMaxNegativePwm(dPort)) /
+ static_cast<double>(GetNegativeScaleFactor(dPort));
+ } else {
+ return 0.0;
+ }
+}
+
+double HAL_GetPWMPosition(HAL_DigitalHandle pwmPortHandle, int32_t* status) {
+ auto port = digitalChannelHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+ if (!port->configSet) {
+ *status = INCOMPATIBLE_STATE;
+ return 0;
+ }
+
+ int32_t value = HAL_GetPWMRaw(pwmPortHandle, status);
+ if (*status != 0) return 0;
+ DigitalPort* dPort = port.get();
+
+ if (value < GetMinNegativePwm(dPort)) {
+ return 0.0;
+ } else if (value > GetMaxPositivePwm(dPort)) {
+ return 1.0;
+ } else {
+ return static_cast<double>(value - GetMinNegativePwm(dPort)) /
+ static_cast<double>(GetFullRangeScaleFactor(dPort));
+ }
+}
+
+void HAL_LatchPWMZero(HAL_DigitalHandle pwmPortHandle, int32_t* status) {
+ auto port = digitalChannelHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ pwmSystem->writeZeroLatch(port->channel, true, status);
+ pwmSystem->writeZeroLatch(port->channel, false, status);
+}
+
+void HAL_SetPWMPeriodScale(HAL_DigitalHandle pwmPortHandle, int32_t squelchMask,
+ int32_t* status) {
+ auto port = digitalChannelHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (port->channel < tPWM::kNumPeriodScaleHdrElements) {
+ pwmSystem->writePeriodScaleHdr(port->channel, squelchMask, status);
+ } else {
+ pwmSystem->writePeriodScaleMXP(
+ port->channel - tPWM::kNumPeriodScaleHdrElements, squelchMask, status);
+ }
+}
+
+int32_t HAL_GetPWMLoopTiming(int32_t* status) {
+ initializeDigital(status);
+ if (*status != 0) return 0;
+ return pwmSystem->readLoopTiming(status);
+}
+
+uint64_t HAL_GetPWMCycleStartTime(int32_t* status) {
+ initializeDigital(status);
+ if (*status != 0) return 0;
+
+ uint64_t upper1 = pwmSystem->readCycleStartTimeUpper(status);
+ uint32_t lower = pwmSystem->readCycleStartTime(status);
+ uint64_t upper2 = pwmSystem->readCycleStartTimeUpper(status);
+ if (*status != 0) return 0;
+ if (upper1 != upper2) {
+ // Rolled over between the lower call, reread lower
+ lower = pwmSystem->readCycleStartTime(status);
+ if (*status != 0) return 0;
+ }
+ return (upper2 << 32) + lower;
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/athena/Ports.cpp b/hal/src/main/native/athena/Ports.cpp
new file mode 100644
index 0000000..47bd400
--- /dev/null
+++ b/hal/src/main/native/athena/Ports.cpp
@@ -0,0 +1,43 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 "hal/Ports.h"
+
+#include "PortsInternal.h"
+
+using namespace hal;
+
+namespace hal {
+namespace init {
+void InitializePorts() {}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+
+int32_t HAL_GetNumAccumulators(void) { return kNumAccumulators; }
+int32_t HAL_GetNumAnalogTriggers(void) { return kNumAnalogTriggers; }
+int32_t HAL_GetNumAnalogInputs(void) { return kNumAnalogInputs; }
+int32_t HAL_GetNumAnalogOutputs(void) { return kNumAnalogOutputs; }
+int32_t HAL_GetNumCounters(void) { return kNumCounters; }
+int32_t HAL_GetNumDigitalHeaders(void) { return kNumDigitalHeaders; }
+int32_t HAL_GetNumPWMHeaders(void) { return kNumPWMHeaders; }
+int32_t HAL_GetNumDigitalChannels(void) { return kNumDigitalChannels; }
+int32_t HAL_GetNumPWMChannels(void) { return kNumPWMChannels; }
+int32_t HAL_GetNumDigitalPWMOutputs(void) { return kNumDigitalPWMOutputs; }
+int32_t HAL_GetNumEncoders(void) { return kNumEncoders; }
+int32_t HAL_GetNumInterrupts(void) { return kNumInterrupts; }
+int32_t HAL_GetNumRelayChannels(void) { return kNumRelayChannels; }
+int32_t HAL_GetNumRelayHeaders(void) { return kNumRelayHeaders; }
+int32_t HAL_GetNumPCMModules(void) { return kNumPCMModules; }
+int32_t HAL_GetNumSolenoidChannels(void) { return kNumSolenoidChannels; }
+int32_t HAL_GetNumPDPModules(void) { return kNumPDPModules; }
+int32_t HAL_GetNumPDPChannels(void) { return kNumPDPChannels; }
+int32_t HAL_GetNumDutyCycles(void) { return kNumDutyCycles; }
+int32_t HAL_GetNumAddressableLEDs(void) { return kNumAddressableLEDs; }
+
+} // extern "C"
diff --git a/hal/src/main/native/athena/PortsInternal.h b/hal/src/main/native/athena/PortsInternal.h
new file mode 100644
index 0000000..98fc690
--- /dev/null
+++ b/hal/src/main/native/athena/PortsInternal.h
@@ -0,0 +1,41 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 <stdint.h>
+
+#include "hal/ChipObject.h"
+
+namespace hal {
+
+constexpr int32_t kNumAccumulators = tAccumulator::kNumSystems;
+constexpr int32_t kNumAnalogTriggers = tAnalogTrigger::kNumSystems;
+constexpr int32_t kNumAnalogInputs = 8;
+constexpr int32_t kNumAnalogOutputs = tAO::kNumMXPRegisters;
+constexpr int32_t kNumCounters = tCounter::kNumSystems;
+constexpr int32_t kNumDigitalHeaders = 10;
+constexpr int32_t kNumDigitalMXPChannels = 16;
+constexpr int32_t kNumDigitalSPIPortChannels = 5;
+constexpr int32_t kNumPWMHeaders = tPWM::kNumHdrRegisters;
+constexpr int32_t kNumDigitalChannels =
+ kNumDigitalHeaders + kNumDigitalMXPChannels + kNumDigitalSPIPortChannels;
+constexpr int32_t kNumPWMChannels = tPWM::kNumMXPRegisters + kNumPWMHeaders;
+constexpr int32_t kNumDigitalPWMOutputs =
+ tDIO::kNumPWMDutyCycleAElements + tDIO::kNumPWMDutyCycleBElements;
+constexpr int32_t kNumEncoders = tEncoder::kNumSystems;
+constexpr int32_t kNumInterrupts = tInterrupt::kNumSystems;
+constexpr int32_t kNumRelayChannels = 8;
+constexpr int32_t kNumRelayHeaders = kNumRelayChannels / 2;
+constexpr int32_t kNumPCMModules = 63;
+constexpr int32_t kNumSolenoidChannels = 8;
+constexpr int32_t kNumPDPModules = 63;
+constexpr int32_t kNumPDPChannels = 16;
+constexpr int32_t kNumDutyCycles = tDutyCycle::kNumSystems;
+constexpr int32_t kNumAddressableLEDs = tLED::kNumSystems;
+
+} // namespace hal
diff --git a/hal/src/main/native/athena/Power.cpp b/hal/src/main/native/athena/Power.cpp
new file mode 100644
index 0000000..2cf4d33
--- /dev/null
+++ b/hal/src/main/native/athena/Power.cpp
@@ -0,0 +1,111 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 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 "hal/Power.h"
+
+#include <memory>
+
+#include "HALInitializer.h"
+#include "hal/ChipObject.h"
+
+using namespace hal;
+
+namespace hal {
+
+static std::unique_ptr<tPower> power{nullptr};
+
+static void initializePower(int32_t* status) {
+ hal::init::CheckInit();
+ if (power == nullptr) {
+ power.reset(tPower::create(status));
+ }
+}
+
+} // namespace hal
+
+namespace hal {
+namespace init {
+void InitializePower() {}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+
+double HAL_GetVinVoltage(int32_t* status) {
+ initializePower(status);
+ return power->readVinVoltage(status) / 4.096 * 0.025733 - 0.029;
+}
+
+double HAL_GetVinCurrent(int32_t* status) {
+ initializePower(status);
+ return power->readVinCurrent(status) / 4.096 * 0.017042 - 0.071;
+}
+
+double HAL_GetUserVoltage6V(int32_t* status) {
+ initializePower(status);
+ return power->readUserVoltage6V(status) / 4.096 * 0.007019 - 0.014;
+}
+
+double HAL_GetUserCurrent6V(int32_t* status) {
+ initializePower(status);
+ return power->readUserCurrent6V(status) / 4.096 * 0.005566 - 0.009;
+}
+
+HAL_Bool HAL_GetUserActive6V(int32_t* status) {
+ initializePower(status);
+ return power->readStatus_User6V(status) == 4;
+}
+
+int32_t HAL_GetUserCurrentFaults6V(int32_t* status) {
+ initializePower(status);
+ return static_cast<int32_t>(
+ power->readFaultCounts_OverCurrentFaultCount6V(status));
+}
+
+double HAL_GetUserVoltage5V(int32_t* status) {
+ initializePower(status);
+ return power->readUserVoltage5V(status) / 4.096 * 0.005962 - 0.013;
+}
+
+double HAL_GetUserCurrent5V(int32_t* status) {
+ initializePower(status);
+ return power->readUserCurrent5V(status) / 4.096 * 0.001996 - 0.002;
+}
+
+HAL_Bool HAL_GetUserActive5V(int32_t* status) {
+ initializePower(status);
+ return power->readStatus_User5V(status) == 4;
+}
+
+int32_t HAL_GetUserCurrentFaults5V(int32_t* status) {
+ initializePower(status);
+ return static_cast<int32_t>(
+ power->readFaultCounts_OverCurrentFaultCount5V(status));
+}
+
+double HAL_GetUserVoltage3V3(int32_t* status) {
+ initializePower(status);
+ return power->readUserVoltage3V3(status) / 4.096 * 0.004902 - 0.01;
+}
+
+double HAL_GetUserCurrent3V3(int32_t* status) {
+ initializePower(status);
+ return power->readUserCurrent3V3(status) / 4.096 * 0.002486 - 0.003;
+}
+
+HAL_Bool HAL_GetUserActive3V3(int32_t* status) {
+ initializePower(status);
+ return power->readStatus_User3V3(status) == 4;
+}
+
+int32_t HAL_GetUserCurrentFaults3V3(int32_t* status) {
+ initializePower(status);
+ return static_cast<int32_t>(
+ power->readFaultCounts_OverCurrentFaultCount3V3(status));
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/athena/Relay.cpp b/hal/src/main/native/athena/Relay.cpp
new file mode 100644
index 0000000..71880a8
--- /dev/null
+++ b/hal/src/main/native/athena/Relay.cpp
@@ -0,0 +1,143 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 "hal/Relay.h"
+
+#include "DigitalInternal.h"
+#include "HALInitializer.h"
+#include "PortsInternal.h"
+#include "hal/handles/IndexedHandleResource.h"
+
+using namespace hal;
+
+namespace {
+
+struct Relay {
+ uint8_t channel;
+ bool fwd;
+};
+
+} // namespace
+
+static IndexedHandleResource<HAL_RelayHandle, Relay, kNumRelayChannels,
+ HAL_HandleEnum::Relay>* relayHandles;
+
+// Create a mutex to protect changes to the relay values
+static wpi::mutex digitalRelayMutex;
+
+namespace hal {
+namespace init {
+void InitializeRelay() {
+ static IndexedHandleResource<HAL_RelayHandle, Relay, kNumRelayChannels,
+ HAL_HandleEnum::Relay>
+ rH;
+ relayHandles = &rH;
+}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+
+HAL_RelayHandle HAL_InitializeRelayPort(HAL_PortHandle portHandle, HAL_Bool fwd,
+ int32_t* status) {
+ hal::init::CheckInit();
+ initializeDigital(status);
+
+ if (*status != 0) return HAL_kInvalidHandle;
+
+ int16_t channel = getPortHandleChannel(portHandle);
+ if (channel == InvalidHandleIndex) {
+ *status = PARAMETER_OUT_OF_RANGE;
+ return HAL_kInvalidHandle;
+ }
+
+ if (!fwd) channel += kNumRelayHeaders; // add 4 to reverse channels
+
+ auto handle = relayHandles->Allocate(channel, status);
+
+ if (*status != 0)
+ return HAL_kInvalidHandle; // failed to allocate. Pass error back.
+
+ auto port = relayHandles->Get(handle);
+ if (port == nullptr) { // would only occur on thread issue.
+ *status = HAL_HANDLE_ERROR;
+ return HAL_kInvalidHandle;
+ }
+
+ if (!fwd) {
+ // Subtract number of headers to put channel in range
+ channel -= kNumRelayHeaders;
+
+ port->fwd = false; // set to reverse
+ } else {
+ port->fwd = true; // set to forward
+ }
+
+ port->channel = static_cast<uint8_t>(channel);
+ return handle;
+}
+
+void HAL_FreeRelayPort(HAL_RelayHandle relayPortHandle) {
+ // no status, so no need to check for a proper free.
+ relayHandles->Free(relayPortHandle);
+}
+
+HAL_Bool HAL_CheckRelayChannel(int32_t channel) {
+ // roboRIO only has 4 headers, and the FPGA has
+ // seperate functions for forward and reverse,
+ // instead of seperate channel IDs
+ return channel < kNumRelayHeaders && channel >= 0;
+}
+
+void HAL_SetRelay(HAL_RelayHandle relayPortHandle, HAL_Bool on,
+ int32_t* status) {
+ auto port = relayHandles->Get(relayPortHandle);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ std::scoped_lock lock(digitalRelayMutex);
+ uint8_t relays = 0;
+ if (port->fwd) {
+ relays = relaySystem->readValue_Forward(status);
+ } else {
+ relays = relaySystem->readValue_Reverse(status);
+ }
+
+ if (*status != 0) return; // bad status read
+
+ if (on) {
+ relays |= 1 << port->channel;
+ } else {
+ relays &= ~(1 << port->channel);
+ }
+
+ if (port->fwd) {
+ relaySystem->writeValue_Forward(relays, status);
+ } else {
+ relaySystem->writeValue_Reverse(relays, status);
+ }
+}
+
+HAL_Bool HAL_GetRelay(HAL_RelayHandle relayPortHandle, int32_t* status) {
+ auto port = relayHandles->Get(relayPortHandle);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return false;
+ }
+
+ uint8_t relays = 0;
+ if (port->fwd) {
+ relays = relaySystem->readValue_Forward(status);
+ } else {
+ relays = relaySystem->readValue_Reverse(status);
+ }
+
+ return (relays & (1 << port->channel)) != 0;
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/athena/SPI.cpp b/hal/src/main/native/athena/SPI.cpp
new file mode 100644
index 0000000..37c5f0e
--- /dev/null
+++ b/hal/src/main/native/athena/SPI.cpp
@@ -0,0 +1,651 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 "hal/SPI.h"
+
+#include <fcntl.h>
+#include <linux/spi/spidev.h>
+#include <sys/ioctl.h>
+#include <unistd.h>
+
+#include <array>
+#include <atomic>
+#include <cstring>
+
+#include <wpi/mutex.h>
+#include <wpi/raw_ostream.h>
+
+#include "DigitalInternal.h"
+#include "HALInitializer.h"
+#include "hal/DIO.h"
+#include "hal/HAL.h"
+#include "hal/handles/HandlesInternal.h"
+
+using namespace hal;
+
+static int32_t m_spiCS0Handle{0};
+static int32_t m_spiCS1Handle{0};
+static int32_t m_spiCS2Handle{0};
+static int32_t m_spiCS3Handle{0};
+static int32_t m_spiMXPHandle{0};
+
+static constexpr int32_t kSpiMaxHandles = 5;
+
+// Indices 0-3 are for onboard CS0-CS2. Index 4 is for MXP.
+static std::array<wpi::mutex, kSpiMaxHandles> spiHandleMutexes;
+static std::array<wpi::mutex, kSpiMaxHandles> spiApiMutexes;
+static std::array<wpi::mutex, kSpiMaxHandles> spiAccumulatorMutexes;
+
+// MXP SPI does not count towards this
+static std::atomic<int32_t> spiPortCount{0};
+
+static HAL_DigitalHandle digitalHandles[9]{HAL_kInvalidHandle};
+
+static wpi::mutex spiAutoMutex;
+static int32_t spiAutoPort = kSpiMaxHandles;
+static std::atomic_bool spiAutoRunning{false};
+static std::unique_ptr<tDMAManager> spiAutoDMA;
+
+static bool SPIInUseByAuto(HAL_SPIPort port) {
+ // SPI engine conflicts with any other chip selects on the same SPI device.
+ // There are two SPI devices: one for ports 0-3 (onboard), the other for port
+ // 4 (MXP).
+ if (!spiAutoRunning) return false;
+ std::scoped_lock lock(spiAutoMutex);
+ return (spiAutoPort >= 0 && spiAutoPort <= 3 && port >= 0 && port <= 3) ||
+ (spiAutoPort == 4 && port == 4);
+}
+
+namespace hal {
+namespace init {
+void InitializeSPI() {}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+
+static void CommonSPIPortInit(int32_t* status) {
+ // All false cases will set
+ if (spiPortCount.fetch_add(1) == 0) {
+ // Have not been initialized yet
+ initializeDigital(status);
+ if (*status != 0) return;
+ // MISO
+ if ((digitalHandles[3] = HAL_InitializeDIOPort(createPortHandleForSPI(29),
+ false, status)) ==
+ HAL_kInvalidHandle) {
+ std::printf("Failed to allocate DIO 29 (MISO)\n");
+ return;
+ }
+ // MOSI
+ if ((digitalHandles[4] = HAL_InitializeDIOPort(createPortHandleForSPI(30),
+ false, status)) ==
+ HAL_kInvalidHandle) {
+ std::printf("Failed to allocate DIO 30 (MOSI)\n");
+ HAL_FreeDIOPort(digitalHandles[3]); // free the first port allocated
+ return;
+ }
+ }
+}
+
+static void CommonSPIPortFree(void) {
+ if (spiPortCount.fetch_sub(1) == 1) {
+ // Clean up SPI Handles
+ HAL_FreeDIOPort(digitalHandles[3]);
+ HAL_FreeDIOPort(digitalHandles[4]);
+ }
+}
+
+void HAL_InitializeSPI(HAL_SPIPort port, int32_t* status) {
+ hal::init::CheckInit();
+ if (port < 0 || port >= kSpiMaxHandles) {
+ *status = PARAMETER_OUT_OF_RANGE;
+ return;
+ }
+
+ int handle;
+ if (HAL_GetSPIHandle(port) != 0) return;
+ switch (port) {
+ case HAL_SPI_kOnboardCS0:
+ CommonSPIPortInit(status);
+ if (*status != 0) return;
+ // CS0 is not a DIO port, so nothing to allocate
+ handle = open("/dev/spidev0.0", O_RDWR);
+ if (handle < 0) {
+ std::printf("Failed to open SPI port %d: %s\n", port,
+ std::strerror(errno));
+ CommonSPIPortFree();
+ return;
+ }
+ HAL_SetSPIHandle(HAL_SPI_kOnboardCS0, handle);
+ break;
+ case HAL_SPI_kOnboardCS1:
+ CommonSPIPortInit(status);
+ if (*status != 0) return;
+ // CS1, Allocate
+ if ((digitalHandles[0] = HAL_InitializeDIOPort(createPortHandleForSPI(26),
+ false, status)) ==
+ HAL_kInvalidHandle) {
+ std::printf("Failed to allocate DIO 26 (CS1)\n");
+ CommonSPIPortFree();
+ return;
+ }
+ handle = open("/dev/spidev0.1", O_RDWR);
+ if (handle < 0) {
+ std::printf("Failed to open SPI port %d: %s\n", port,
+ std::strerror(errno));
+ CommonSPIPortFree();
+ HAL_FreeDIOPort(digitalHandles[0]);
+ return;
+ }
+ HAL_SetSPIHandle(HAL_SPI_kOnboardCS1, handle);
+ break;
+ case HAL_SPI_kOnboardCS2:
+ CommonSPIPortInit(status);
+ if (*status != 0) return;
+ // CS2, Allocate
+ if ((digitalHandles[1] = HAL_InitializeDIOPort(createPortHandleForSPI(27),
+ false, status)) ==
+ HAL_kInvalidHandle) {
+ std::printf("Failed to allocate DIO 27 (CS2)\n");
+ CommonSPIPortFree();
+ return;
+ }
+ handle = open("/dev/spidev0.2", O_RDWR);
+ if (handle < 0) {
+ std::printf("Failed to open SPI port %d: %s\n", port,
+ std::strerror(errno));
+ CommonSPIPortFree();
+ HAL_FreeDIOPort(digitalHandles[1]);
+ return;
+ }
+ HAL_SetSPIHandle(HAL_SPI_kOnboardCS2, handle);
+ break;
+ case HAL_SPI_kOnboardCS3:
+ CommonSPIPortInit(status);
+ if (*status != 0) return;
+ // CS3, Allocate
+ if ((digitalHandles[2] = HAL_InitializeDIOPort(createPortHandleForSPI(28),
+ false, status)) ==
+ HAL_kInvalidHandle) {
+ std::printf("Failed to allocate DIO 28 (CS3)\n");
+ CommonSPIPortFree();
+ return;
+ }
+ handle = open("/dev/spidev0.3", O_RDWR);
+ if (handle < 0) {
+ std::printf("Failed to open SPI port %d: %s\n", port,
+ std::strerror(errno));
+ CommonSPIPortFree();
+ HAL_FreeDIOPort(digitalHandles[2]);
+ return;
+ }
+ HAL_SetSPIHandle(HAL_SPI_kOnboardCS3, handle);
+ break;
+ case HAL_SPI_kMXP:
+ initializeDigital(status);
+ if (*status != 0) return;
+ if ((digitalHandles[5] = HAL_InitializeDIOPort(createPortHandleForSPI(14),
+ false, status)) ==
+ HAL_kInvalidHandle) {
+ wpi::outs() << "Failed to allocate DIO 14\n";
+ return;
+ }
+ if ((digitalHandles[6] = HAL_InitializeDIOPort(createPortHandleForSPI(15),
+ false, status)) ==
+ HAL_kInvalidHandle) {
+ wpi::outs() << "Failed to allocate DIO 15\n";
+ HAL_FreeDIOPort(digitalHandles[5]); // free the first port allocated
+ return;
+ }
+ if ((digitalHandles[7] = HAL_InitializeDIOPort(createPortHandleForSPI(16),
+ false, status)) ==
+ HAL_kInvalidHandle) {
+ wpi::outs() << "Failed to allocate DIO 16\n";
+ HAL_FreeDIOPort(digitalHandles[5]); // free the first port allocated
+ HAL_FreeDIOPort(digitalHandles[6]); // free the second port allocated
+ return;
+ }
+ if ((digitalHandles[8] = HAL_InitializeDIOPort(createPortHandleForSPI(17),
+ false, status)) ==
+ HAL_kInvalidHandle) {
+ wpi::outs() << "Failed to allocate DIO 17\n";
+ HAL_FreeDIOPort(digitalHandles[5]); // free the first port allocated
+ HAL_FreeDIOPort(digitalHandles[6]); // free the second port allocated
+ HAL_FreeDIOPort(digitalHandles[7]); // free the third port allocated
+ return;
+ }
+ digitalSystem->writeEnableMXPSpecialFunction(
+ digitalSystem->readEnableMXPSpecialFunction(status) | 0x00F0, status);
+ handle = open("/dev/spidev1.0", O_RDWR);
+ if (handle < 0) {
+ std::printf("Failed to open SPI port %d: %s\n", port,
+ std::strerror(errno));
+ HAL_FreeDIOPort(digitalHandles[5]); // free the first port allocated
+ HAL_FreeDIOPort(digitalHandles[6]); // free the second port allocated
+ HAL_FreeDIOPort(digitalHandles[7]); // free the third port allocated
+ HAL_FreeDIOPort(digitalHandles[8]); // free the fourth port allocated
+ return;
+ }
+ HAL_SetSPIHandle(HAL_SPI_kMXP, handle);
+ break;
+ default:
+ *status = PARAMETER_OUT_OF_RANGE;
+ break;
+ }
+}
+
+int32_t HAL_TransactionSPI(HAL_SPIPort port, const uint8_t* dataToSend,
+ uint8_t* dataReceived, int32_t size) {
+ if (port < 0 || port >= kSpiMaxHandles) {
+ return -1;
+ }
+
+ if (SPIInUseByAuto(port)) return -1;
+
+ struct spi_ioc_transfer xfer;
+ std::memset(&xfer, 0, sizeof(xfer));
+ xfer.tx_buf = (__u64)dataToSend;
+ xfer.rx_buf = (__u64)dataReceived;
+ xfer.len = size;
+
+ std::scoped_lock lock(spiApiMutexes[port]);
+ return ioctl(HAL_GetSPIHandle(port), SPI_IOC_MESSAGE(1), &xfer);
+}
+
+int32_t HAL_WriteSPI(HAL_SPIPort port, const uint8_t* dataToSend,
+ int32_t sendSize) {
+ if (port < 0 || port >= kSpiMaxHandles) {
+ return -1;
+ }
+
+ if (SPIInUseByAuto(port)) return -1;
+
+ struct spi_ioc_transfer xfer;
+ std::memset(&xfer, 0, sizeof(xfer));
+ xfer.tx_buf = (__u64)dataToSend;
+ xfer.len = sendSize;
+
+ std::scoped_lock lock(spiApiMutexes[port]);
+ return ioctl(HAL_GetSPIHandle(port), SPI_IOC_MESSAGE(1), &xfer);
+}
+
+int32_t HAL_ReadSPI(HAL_SPIPort port, uint8_t* buffer, int32_t count) {
+ if (port < 0 || port >= kSpiMaxHandles) {
+ return -1;
+ }
+
+ if (SPIInUseByAuto(port)) return -1;
+
+ struct spi_ioc_transfer xfer;
+ std::memset(&xfer, 0, sizeof(xfer));
+ xfer.rx_buf = (__u64)buffer;
+ xfer.len = count;
+
+ std::scoped_lock lock(spiApiMutexes[port]);
+ return ioctl(HAL_GetSPIHandle(port), SPI_IOC_MESSAGE(1), &xfer);
+}
+
+void HAL_CloseSPI(HAL_SPIPort port) {
+ if (port < 0 || port >= kSpiMaxHandles) {
+ return;
+ }
+
+ int32_t status = 0;
+ HAL_FreeSPIAuto(port, &status);
+
+ {
+ std::scoped_lock lock(spiApiMutexes[port]);
+ close(HAL_GetSPIHandle(port));
+ }
+
+ HAL_SetSPIHandle(port, 0);
+ if (port < 4) {
+ CommonSPIPortFree();
+ }
+
+ switch (port) {
+ // Case 0 does not need to do anything
+ case 1:
+ HAL_FreeDIOPort(digitalHandles[0]);
+ break;
+ case 2:
+ HAL_FreeDIOPort(digitalHandles[1]);
+ break;
+ case 3:
+ HAL_FreeDIOPort(digitalHandles[2]);
+ break;
+ case 4:
+ HAL_FreeDIOPort(digitalHandles[5]);
+ HAL_FreeDIOPort(digitalHandles[6]);
+ HAL_FreeDIOPort(digitalHandles[7]);
+ HAL_FreeDIOPort(digitalHandles[8]);
+ break;
+ default:
+ break;
+ }
+}
+
+void HAL_SetSPISpeed(HAL_SPIPort port, int32_t speed) {
+ if (port < 0 || port >= kSpiMaxHandles) {
+ return;
+ }
+
+ std::scoped_lock lock(spiApiMutexes[port]);
+ ioctl(HAL_GetSPIHandle(port), SPI_IOC_WR_MAX_SPEED_HZ, &speed);
+}
+
+void HAL_SetSPIOpts(HAL_SPIPort port, HAL_Bool msbFirst,
+ HAL_Bool sampleOnTrailing, HAL_Bool clkIdleHigh) {
+ if (port < 0 || port >= kSpiMaxHandles) {
+ return;
+ }
+
+ uint8_t mode = 0;
+ mode |= (!msbFirst ? 8 : 0);
+ mode |= (clkIdleHigh ? 2 : 0);
+ mode |= (sampleOnTrailing ? 1 : 0);
+
+ std::scoped_lock lock(spiApiMutexes[port]);
+ ioctl(HAL_GetSPIHandle(port), SPI_IOC_WR_MODE, &mode);
+}
+
+void HAL_SetSPIChipSelectActiveHigh(HAL_SPIPort port, int32_t* status) {
+ if (port < 0 || port >= kSpiMaxHandles) {
+ *status = PARAMETER_OUT_OF_RANGE;
+ return;
+ }
+
+ std::scoped_lock lock(spiApiMutexes[port]);
+ if (port < 4) {
+ spiSystem->writeChipSelectActiveHigh_Hdr(
+ spiSystem->readChipSelectActiveHigh_Hdr(status) | (1 << port), status);
+ } else {
+ spiSystem->writeChipSelectActiveHigh_MXP(1, status);
+ }
+}
+
+void HAL_SetSPIChipSelectActiveLow(HAL_SPIPort port, int32_t* status) {
+ if (port < 0 || port >= kSpiMaxHandles) {
+ *status = PARAMETER_OUT_OF_RANGE;
+ return;
+ }
+
+ std::scoped_lock lock(spiApiMutexes[port]);
+ if (port < 4) {
+ spiSystem->writeChipSelectActiveHigh_Hdr(
+ spiSystem->readChipSelectActiveHigh_Hdr(status) & ~(1 << port), status);
+ } else {
+ spiSystem->writeChipSelectActiveHigh_MXP(0, status);
+ }
+}
+
+int32_t HAL_GetSPIHandle(HAL_SPIPort port) {
+ if (port < 0 || port >= kSpiMaxHandles) {
+ return 0;
+ }
+
+ std::scoped_lock lock(spiHandleMutexes[port]);
+ switch (port) {
+ case 0:
+ return m_spiCS0Handle;
+ case 1:
+ return m_spiCS1Handle;
+ case 2:
+ return m_spiCS2Handle;
+ case 3:
+ return m_spiCS3Handle;
+ case 4:
+ return m_spiMXPHandle;
+ default:
+ return 0;
+ }
+}
+
+void HAL_SetSPIHandle(HAL_SPIPort port, int32_t handle) {
+ if (port < 0 || port >= kSpiMaxHandles) {
+ return;
+ }
+
+ std::scoped_lock lock(spiHandleMutexes[port]);
+ switch (port) {
+ case 0:
+ m_spiCS0Handle = handle;
+ break;
+ case 1:
+ m_spiCS1Handle = handle;
+ break;
+ case 2:
+ m_spiCS2Handle = handle;
+ break;
+ case 3:
+ m_spiCS3Handle = handle;
+ break;
+ case 4:
+ m_spiMXPHandle = handle;
+ break;
+ default:
+ break;
+ }
+}
+
+void HAL_InitSPIAuto(HAL_SPIPort port, int32_t bufferSize, int32_t* status) {
+ if (port < 0 || port >= kSpiMaxHandles) {
+ *status = PARAMETER_OUT_OF_RANGE;
+ return;
+ }
+
+ std::scoped_lock lock(spiAutoMutex);
+ // FPGA only has one auto SPI engine
+ if (spiAutoPort != kSpiMaxHandles) {
+ *status = RESOURCE_IS_ALLOCATED;
+ return;
+ }
+
+ // remember the initialized port for other entry points
+ spiAutoPort = port;
+
+ // configure the correct chip select
+ if (port < 4) {
+ spiSystem->writeAutoSPI1Select(false, status);
+ spiSystem->writeAutoChipSelect(port, status);
+ } else {
+ spiSystem->writeAutoSPI1Select(true, status);
+ spiSystem->writeAutoChipSelect(0, status);
+ }
+
+ // configure DMA
+ tDMAChannelDescriptor desc;
+ spiSystem->getSystemInterface()->getDmaDescriptor(g_SpiAutoData_index, &desc);
+ spiAutoDMA = std::make_unique<tDMAManager>(desc.channel, bufferSize, status);
+}
+
+void HAL_FreeSPIAuto(HAL_SPIPort port, int32_t* status) {
+ if (port < 0 || port >= kSpiMaxHandles) {
+ *status = PARAMETER_OUT_OF_RANGE;
+ return;
+ }
+
+ std::scoped_lock lock(spiAutoMutex);
+ if (spiAutoPort != port) return;
+ spiAutoPort = kSpiMaxHandles;
+
+ // disable by setting to internal clock and setting rate=0
+ spiSystem->writeAutoRate(0, status);
+ spiSystem->writeAutoTriggerConfig_ExternalClock(false, status);
+
+ // stop the DMA
+ spiAutoDMA->stop(status);
+
+ spiAutoDMA.reset(nullptr);
+
+ spiAutoRunning = false;
+}
+
+void HAL_StartSPIAutoRate(HAL_SPIPort port, double period, int32_t* status) {
+ std::scoped_lock lock(spiAutoMutex);
+ // FPGA only has one auto SPI engine
+ if (port != spiAutoPort) {
+ *status = INCOMPATIBLE_STATE;
+ return;
+ }
+
+ spiAutoRunning = true;
+
+ // start the DMA
+ spiAutoDMA->start(status);
+
+ // auto rate is in microseconds
+ spiSystem->writeAutoRate(period * 1000000, status);
+
+ // disable the external clock
+ spiSystem->writeAutoTriggerConfig_ExternalClock(false, status);
+}
+
+void HAL_StartSPIAutoTrigger(HAL_SPIPort port, HAL_Handle digitalSourceHandle,
+ HAL_AnalogTriggerType analogTriggerType,
+ HAL_Bool triggerRising, HAL_Bool triggerFalling,
+ int32_t* status) {
+ std::scoped_lock lock(spiAutoMutex);
+ // FPGA only has one auto SPI engine
+ if (port != spiAutoPort) {
+ *status = INCOMPATIBLE_STATE;
+ return;
+ }
+
+ spiAutoRunning = true;
+
+ // start the DMA
+ spiAutoDMA->start(status);
+
+ // get channel routing
+ bool routingAnalogTrigger = false;
+ uint8_t routingChannel = 0;
+ uint8_t routingModule = 0;
+ if (!remapDigitalSource(digitalSourceHandle, analogTriggerType,
+ routingChannel, routingModule,
+ routingAnalogTrigger)) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ // configure external trigger and enable it
+ tSPI::tAutoTriggerConfig config;
+ config.ExternalClock = 1;
+ config.FallingEdge = triggerFalling ? 1 : 0;
+ config.RisingEdge = triggerRising ? 1 : 0;
+ config.ExternalClockSource_AnalogTrigger = routingAnalogTrigger ? 1 : 0;
+ config.ExternalClockSource_Module = routingModule;
+ config.ExternalClockSource_Channel = routingChannel;
+ spiSystem->writeAutoTriggerConfig(config, status);
+}
+
+void HAL_StopSPIAuto(HAL_SPIPort port, int32_t* status) {
+ std::scoped_lock lock(spiAutoMutex);
+ // FPGA only has one auto SPI engine
+ if (port != spiAutoPort) {
+ *status = INCOMPATIBLE_STATE;
+ return;
+ }
+
+ // disable by setting to internal clock and setting rate=0
+ spiSystem->writeAutoRate(0, status);
+ spiSystem->writeAutoTriggerConfig_ExternalClock(false, status);
+
+ // stop the DMA
+ spiAutoDMA->stop(status);
+
+ spiAutoRunning = false;
+}
+
+void HAL_SetSPIAutoTransmitData(HAL_SPIPort port, const uint8_t* dataToSend,
+ int32_t dataSize, int32_t zeroSize,
+ int32_t* status) {
+ if (dataSize < 0 || dataSize > 32) {
+ *status = PARAMETER_OUT_OF_RANGE;
+ return;
+ }
+
+ if (zeroSize < 0 || zeroSize > 127) {
+ *status = PARAMETER_OUT_OF_RANGE;
+ return;
+ }
+
+ std::scoped_lock lock(spiAutoMutex);
+ // FPGA only has one auto SPI engine
+ if (port != spiAutoPort) {
+ *status = INCOMPATIBLE_STATE;
+ return;
+ }
+
+ // set tx data registers
+ for (int32_t i = 0; i < dataSize; ++i)
+ spiSystem->writeAutoTx(i >> 2, i & 3, dataToSend[i], status);
+
+ // set byte counts
+ tSPI::tAutoByteCount config;
+ config.ZeroByteCount = static_cast<unsigned>(zeroSize) & 0x7f;
+ config.TxByteCount = static_cast<unsigned>(dataSize) & 0x1f;
+ spiSystem->writeAutoByteCount(config, status);
+}
+
+void HAL_ForceSPIAutoRead(HAL_SPIPort port, int32_t* status) {
+ std::scoped_lock lock(spiAutoMutex);
+ // FPGA only has one auto SPI engine
+ if (port != spiAutoPort) {
+ *status = INCOMPATIBLE_STATE;
+ return;
+ }
+
+ spiSystem->strobeAutoForceOne(status);
+}
+
+int32_t HAL_ReadSPIAutoReceivedData(HAL_SPIPort port, uint32_t* buffer,
+ int32_t numToRead, double timeout,
+ int32_t* status) {
+ std::scoped_lock lock(spiAutoMutex);
+ // FPGA only has one auto SPI engine
+ if (port != spiAutoPort) {
+ *status = INCOMPATIBLE_STATE;
+ return 0;
+ }
+
+ size_t numRemaining = 0;
+ // timeout is in ms
+ spiAutoDMA->read(buffer, numToRead, timeout * 1000, &numRemaining, status);
+ return numRemaining;
+}
+
+int32_t HAL_GetSPIAutoDroppedCount(HAL_SPIPort port, int32_t* status) {
+ std::scoped_lock lock(spiAutoMutex);
+ // FPGA only has one auto SPI engine
+ if (port != spiAutoPort) {
+ *status = INCOMPATIBLE_STATE;
+ return 0;
+ }
+
+ return spiSystem->readTransferSkippedFullCount(status);
+}
+
+void HAL_ConfigureSPIAutoStall(HAL_SPIPort port, int32_t csToSclkTicks,
+ int32_t stallTicks, int32_t pow2BytesPerRead,
+ int32_t* status) {
+ std::scoped_lock lock(spiAutoMutex);
+ // FPGA only has one auto SPI engine
+ if (port != spiAutoPort) {
+ *status = INCOMPATIBLE_STATE;
+ return;
+ }
+
+ tSPI::tStallConfig stallConfig;
+ stallConfig.CsToSclkTicks = static_cast<uint8_t>(csToSclkTicks);
+ stallConfig.StallTicks = static_cast<uint16_t>(stallTicks);
+ stallConfig.Pow2BytesPerRead = static_cast<uint8_t>(pow2BytesPerRead);
+ spiSystem->writeStallConfig(stallConfig, status);
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/athena/SerialPort.cpp b/hal/src/main/native/athena/SerialPort.cpp
new file mode 100644
index 0000000..7ef9b70
--- /dev/null
+++ b/hal/src/main/native/athena/SerialPort.cpp
@@ -0,0 +1,499 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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 "hal/SerialPort.h"
+
+#include <fcntl.h>
+#include <sys/ioctl.h>
+#include <termios.h>
+#include <unistd.h>
+
+#include <cerrno>
+#include <chrono>
+#include <cstdio>
+#include <cstdlib>
+#include <cstring>
+#include <iostream>
+#include <stdexcept>
+#include <string>
+#include <thread>
+
+#include "hal/cpp/SerialHelper.h"
+#include "hal/handles/HandlesInternal.h"
+#include "hal/handles/IndexedHandleResource.h"
+
+namespace {
+struct SerialPort {
+ int portId;
+ struct termios tty;
+ int baudRate;
+
+ double timeout = 0;
+
+ bool termination = false;
+ char terminationChar = '\n';
+};
+} // namespace
+
+namespace hal {
+IndexedHandleResource<HAL_SerialPortHandle, SerialPort, 4,
+ HAL_HandleEnum::SerialPort>* serialPortHandles;
+} // namespace hal
+
+namespace hal {
+namespace init {
+void InitializeSerialPort() {
+ static IndexedHandleResource<HAL_SerialPortHandle, SerialPort, 4,
+ HAL_HandleEnum::SerialPort>
+ spH;
+ serialPortHandles = &spH;
+}
+} // namespace init
+} // namespace hal
+
+using namespace hal;
+
+extern "C" {
+HAL_SerialPortHandle HAL_InitializeSerialPort(HAL_SerialPort port,
+ int32_t* status) {
+ // hal::init::CheckInit();
+
+ hal::SerialHelper serialHelper;
+
+ std::string portName = serialHelper.GetOSSerialPortName(port, status);
+
+ if (*status < 0) {
+ return HAL_kInvalidHandle;
+ }
+
+ return HAL_InitializeSerialPortDirect(port, portName.c_str(), status);
+}
+HAL_SerialPortHandle HAL_InitializeSerialPortDirect(HAL_SerialPort port,
+ const char* portName,
+ int32_t* status) {
+ auto handle = serialPortHandles->Allocate(static_cast<int16_t>(port), status);
+
+ if (*status != 0) {
+ return HAL_kInvalidHandle;
+ }
+
+ auto serialPort = serialPortHandles->Get(handle);
+
+ if (serialPort == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return HAL_kInvalidHandle;
+ }
+
+ serialPort->portId = open(portName, O_RDWR | O_NOCTTY);
+ if (serialPort->portId < 0) {
+ *status = errno;
+ serialPortHandles->Free(handle);
+ return HAL_kInvalidHandle;
+ }
+
+ std::memset(&serialPort->tty, 0, sizeof(serialPort->tty));
+
+ serialPort->baudRate = B9600;
+ cfsetospeed(&serialPort->tty, static_cast<speed_t>(serialPort->baudRate));
+ cfsetispeed(&serialPort->tty, static_cast<speed_t>(serialPort->baudRate));
+
+ serialPort->tty.c_cflag &= ~PARENB;
+ serialPort->tty.c_cflag &= ~CSTOPB;
+ serialPort->tty.c_cflag &= ~CSIZE;
+ serialPort->tty.c_cflag |= CS8;
+
+ serialPort->tty.c_cc[VMIN] = 0;
+ serialPort->tty.c_cc[VTIME] = 10;
+
+ serialPort->tty.c_cflag |= CREAD | CLOCAL;
+
+ serialPort->tty.c_lflag &= ~(ICANON | ECHO | ISIG);
+ serialPort->tty.c_iflag &= ~(IXON | IXOFF | IXANY);
+ /* Raw output mode, sends the raw and unprocessed data (send as it is).
+ * If it is in canonical mode and sending new line char then CR
+ * will be added as prefix and send as CR LF
+ */
+ serialPort->tty.c_oflag = ~OPOST;
+
+ tcflush(serialPort->portId, TCIOFLUSH);
+ if (tcsetattr(serialPort->portId, TCSANOW, &serialPort->tty) != 0) {
+ *status = errno;
+ close(serialPort->portId);
+ serialPortHandles->Free(handle);
+ return HAL_kInvalidHandle;
+ }
+ return handle;
+}
+
+void HAL_CloseSerial(HAL_SerialPortHandle handle, int32_t* status) {
+ auto port = serialPortHandles->Get(handle);
+ serialPortHandles->Free(handle);
+
+ if (port) {
+ close(port->portId);
+ }
+}
+
+int HAL_GetSerialFD(HAL_SerialPortHandle handle, int32_t* status) {
+ auto port = serialPortHandles->Get(handle);
+ if (!port) {
+ *status = HAL_HANDLE_ERROR;
+ return -1;
+ }
+ return port->portId;
+}
+
+#define BAUDCASE(BAUD) \
+ case BAUD: \
+ port->baudRate = B##BAUD; \
+ break;
+
+void HAL_SetSerialBaudRate(HAL_SerialPortHandle handle, int32_t baud,
+ int32_t* status) {
+ auto port = serialPortHandles->Get(handle);
+ if (!port) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ switch (baud) {
+ BAUDCASE(50)
+ BAUDCASE(75)
+ BAUDCASE(110)
+ BAUDCASE(134)
+ BAUDCASE(150)
+ BAUDCASE(200)
+ BAUDCASE(300)
+ BAUDCASE(600)
+ BAUDCASE(1200)
+ BAUDCASE(1800)
+ BAUDCASE(2400)
+ BAUDCASE(4800)
+ BAUDCASE(9600)
+ BAUDCASE(19200)
+ BAUDCASE(38400)
+ BAUDCASE(57600)
+ BAUDCASE(115200)
+ BAUDCASE(230400)
+ BAUDCASE(460800)
+ BAUDCASE(500000)
+ BAUDCASE(576000)
+ BAUDCASE(921600)
+ BAUDCASE(1000000)
+ BAUDCASE(1152000)
+ BAUDCASE(1500000)
+ BAUDCASE(2000000)
+ BAUDCASE(2500000)
+ BAUDCASE(3000000)
+ BAUDCASE(3500000)
+ BAUDCASE(4000000)
+ default:
+ *status = PARAMETER_OUT_OF_RANGE;
+ return;
+ }
+ int err = cfsetospeed(&port->tty, static_cast<speed_t>(port->baudRate));
+ if (err < 0) {
+ *status = errno;
+ return;
+ }
+ err = cfsetispeed(&port->tty, static_cast<speed_t>(port->baudRate));
+ if (err < 0) {
+ *status = errno;
+ return;
+ }
+ err = tcsetattr(port->portId, TCSANOW, &port->tty);
+ if (err < 0) {
+ *status = errno;
+ }
+}
+
+void HAL_SetSerialDataBits(HAL_SerialPortHandle handle, int32_t bits,
+ int32_t* status) {
+ auto port = serialPortHandles->Get(handle);
+ if (!port) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ int bitFlag = -1;
+ switch (bits) {
+ case 5:
+ bitFlag = CS5;
+ break;
+ case 6:
+ bitFlag = CS6;
+ break;
+ case 7:
+ bitFlag = CS7;
+ break;
+ case 8:
+ bitFlag = CS8;
+ break;
+ default:
+ *status = PARAMETER_OUT_OF_RANGE;
+ return;
+ }
+
+ port->tty.c_cflag &= ~CSIZE;
+ port->tty.c_cflag |= bitFlag;
+
+ int err = tcsetattr(port->portId, TCSANOW, &port->tty);
+ if (err < 0) {
+ *status = errno;
+ }
+}
+
+void HAL_SetSerialParity(HAL_SerialPortHandle handle, int32_t parity,
+ int32_t* status) {
+ auto port = serialPortHandles->Get(handle);
+ if (!port) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ switch (parity) {
+ case 0: // None
+ port->tty.c_cflag &= ~PARENB;
+ port->tty.c_cflag &= ~CMSPAR;
+ break;
+ case 1: // Odd
+ port->tty.c_cflag |= PARENB;
+ port->tty.c_cflag &= ~CMSPAR;
+ port->tty.c_cflag &= ~PARODD;
+ break;
+ case 2: // Even
+ port->tty.c_cflag |= PARENB;
+ port->tty.c_cflag &= ~CMSPAR;
+ port->tty.c_cflag |= PARODD;
+ break;
+ case 3: // Mark
+ port->tty.c_cflag |= PARENB;
+ port->tty.c_cflag |= CMSPAR;
+ port->tty.c_cflag |= PARODD;
+ break;
+ case 4: // Space
+ port->tty.c_cflag |= PARENB;
+ port->tty.c_cflag |= CMSPAR;
+ port->tty.c_cflag &= ~PARODD;
+ break;
+ default:
+ *status = PARAMETER_OUT_OF_RANGE;
+ return;
+ }
+
+ int err = tcsetattr(port->portId, TCSANOW, &port->tty);
+ if (err < 0) {
+ *status = errno;
+ }
+}
+
+void HAL_SetSerialStopBits(HAL_SerialPortHandle handle, int32_t stopBits,
+ int32_t* status) {
+ auto port = serialPortHandles->Get(handle);
+ if (!port) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ switch (stopBits) {
+ case 10: // 1
+ port->tty.c_cflag &= ~CSTOPB;
+ break;
+ case 15: // 1.5
+ case 20: // 2
+ port->tty.c_cflag |= CSTOPB;
+ break;
+ default:
+ *status = PARAMETER_OUT_OF_RANGE;
+ return;
+ }
+
+ int err = tcsetattr(port->portId, TCSANOW, &port->tty);
+ if (err < 0) {
+ *status = errno;
+ }
+}
+
+void HAL_SetSerialWriteMode(HAL_SerialPortHandle handle, int32_t mode,
+ int32_t* status) {
+ // This seems to be a no op on the NI serial port driver
+}
+
+void HAL_SetSerialFlowControl(HAL_SerialPortHandle handle, int32_t flow,
+ int32_t* status) {
+ auto port = serialPortHandles->Get(handle);
+ if (!port) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ switch (flow) {
+ case 0:
+ port->tty.c_cflag &= ~CRTSCTS;
+ break;
+ case 1:
+ port->tty.c_cflag &= ~CRTSCTS;
+ port->tty.c_iflag &= IXON | IXOFF;
+ break;
+ case 2:
+ port->tty.c_cflag |= CRTSCTS;
+ break;
+ default:
+ *status = PARAMETER_OUT_OF_RANGE;
+ return;
+ }
+
+ int err = tcsetattr(port->portId, TCSANOW, &port->tty);
+ if (err < 0) {
+ *status = errno;
+ }
+}
+
+void HAL_SetSerialTimeout(HAL_SerialPortHandle handle, double timeout,
+ int32_t* status) {
+ auto port = serialPortHandles->Get(handle);
+ if (!port) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ port->timeout = timeout;
+ port->tty.c_cc[VTIME] = static_cast<int>(timeout * 10);
+ int err = tcsetattr(port->portId, TCSANOW, &port->tty);
+ if (err < 0) {
+ *status = errno;
+ }
+}
+
+void HAL_EnableSerialTermination(HAL_SerialPortHandle handle, char terminator,
+ int32_t* status) {
+ auto port = serialPortHandles->Get(handle);
+ if (!port) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ port->termination = true;
+ port->terminationChar = terminator;
+}
+
+void HAL_DisableSerialTermination(HAL_SerialPortHandle handle,
+ int32_t* status) {
+ auto port = serialPortHandles->Get(handle);
+ if (!port) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ port->termination = false;
+}
+
+void HAL_SetSerialReadBufferSize(HAL_SerialPortHandle handle, int32_t size,
+ int32_t* status) {
+ // NO OP currently
+}
+
+void HAL_SetSerialWriteBufferSize(HAL_SerialPortHandle handle, int32_t size,
+ int32_t* status) {
+ // NO OP currently
+}
+
+int32_t HAL_GetSerialBytesReceived(HAL_SerialPortHandle handle,
+ int32_t* status) {
+ auto port = serialPortHandles->Get(handle);
+ if (!port) {
+ *status = HAL_HANDLE_ERROR;
+ return -1;
+ }
+ int bytes = 0;
+ int err = ioctl(port->portId, FIONREAD, &bytes);
+ if (err < 0) {
+ *status = errno;
+ }
+ return bytes;
+}
+
+int32_t HAL_ReadSerial(HAL_SerialPortHandle handle, char* buffer, int32_t count,
+ int32_t* status) {
+ // Don't do anything if 0 bytes were requested
+ if (count == 0) return 0;
+
+ auto port = serialPortHandles->Get(handle);
+ if (!port) {
+ *status = HAL_HANDLE_ERROR;
+ return -1;
+ }
+
+ int n = 0, loc = 0;
+ char buf = '\0';
+ std::memset(buffer, '\0', count);
+ *status = 0;
+
+ do {
+ n = read(port->portId, &buf, 1);
+ if (n == 1) {
+ buffer[loc] = buf;
+ loc++;
+ // If buffer is full, return
+ if (loc == count) {
+ return loc;
+ }
+ // If terminating, and termination was hit return;
+ if (port->termination && buf == port->terminationChar) {
+ return loc;
+ }
+ } else if (n == -1) {
+ // ERROR
+ *status = errno;
+ return loc;
+ } else {
+ // If nothing read, timeout
+ return loc;
+ }
+ } while (true);
+}
+
+int32_t HAL_WriteSerial(HAL_SerialPortHandle handle, const char* buffer,
+ int32_t count, int32_t* status) {
+ auto port = serialPortHandles->Get(handle);
+ if (!port) {
+ *status = HAL_HANDLE_ERROR;
+ return -1;
+ }
+
+ int written = 0, spot = 0;
+ do {
+ written = write(port->portId, buffer + spot, count - spot);
+ if (written < 0) {
+ *status = errno;
+ return spot;
+ }
+ spot += written;
+ } while (spot < count);
+ return spot;
+}
+
+void HAL_FlushSerial(HAL_SerialPortHandle handle, int32_t* status) {
+ auto port = serialPortHandles->Get(handle);
+ if (!port) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ int err = tcdrain(port->portId);
+ if (err < 0) {
+ *status = errno;
+ }
+}
+void HAL_ClearSerial(HAL_SerialPortHandle handle, int32_t* status) {
+ auto port = serialPortHandles->Get(handle);
+ if (!port) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ int err = tcflush(port->portId, TCIOFLUSH);
+ if (err < 0) {
+ *status = errno;
+ }
+}
+} // extern "C"
diff --git a/hal/src/main/native/athena/SimDevice.cpp b/hal/src/main/native/athena/SimDevice.cpp
new file mode 100644
index 0000000..94a65d4
--- /dev/null
+++ b/hal/src/main/native/athena/SimDevice.cpp
@@ -0,0 +1,41 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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 "hal/SimDevice.h"
+
+extern "C" {
+
+HAL_SimDeviceHandle HAL_CreateSimDevice(const char* name) { return 0; }
+
+void HAL_FreeSimDevice(HAL_SimDeviceHandle handle) {}
+
+HAL_SimValueHandle HAL_CreateSimValue(HAL_SimDeviceHandle device,
+ const char* name, HAL_Bool readonly,
+ const struct HAL_Value* initialValue) {
+ return 0;
+}
+
+HAL_SimValueHandle HAL_CreateSimValueEnum(HAL_SimDeviceHandle device,
+ const char* name, HAL_Bool readonly,
+ int32_t numOptions,
+ const char** options,
+ int32_t initialValue) {
+ return 0;
+}
+
+void HAL_GetSimValue(HAL_SimValueHandle handle, struct HAL_Value* value) {
+ value->type = HAL_UNASSIGNED;
+}
+
+void HAL_SetSimValue(HAL_SimValueHandle handle, const struct HAL_Value* value) {
+}
+
+hal::SimDevice::SimDevice(const char* name, int index) {}
+
+hal::SimDevice::SimDevice(const char* name, int index, int channel) {}
+
+} // extern "C"
diff --git a/hal/src/main/native/athena/Solenoid.cpp b/hal/src/main/native/athena/Solenoid.cpp
new file mode 100644
index 0000000..c53db11
--- /dev/null
+++ b/hal/src/main/native/athena/Solenoid.cpp
@@ -0,0 +1,191 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 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 "hal/Solenoid.h"
+
+#include <FRC_NetworkCommunication/LoadOut.h>
+
+#include "HALInitializer.h"
+#include "PCMInternal.h"
+#include "PortsInternal.h"
+#include "ctre/PCM.h"
+#include "hal/ChipObject.h"
+#include "hal/Errors.h"
+#include "hal/Ports.h"
+#include "hal/handles/HandlesInternal.h"
+#include "hal/handles/IndexedHandleResource.h"
+
+namespace {
+
+struct Solenoid {
+ uint8_t module;
+ uint8_t channel;
+};
+
+} // namespace
+
+using namespace hal;
+
+static IndexedHandleResource<HAL_SolenoidHandle, Solenoid,
+ kNumPCMModules * kNumSolenoidChannels,
+ HAL_HandleEnum::Solenoid>* solenoidHandles;
+
+namespace hal {
+namespace init {
+void InitializeSolenoid() {
+ static IndexedHandleResource<HAL_SolenoidHandle, Solenoid,
+ kNumPCMModules * kNumSolenoidChannels,
+ HAL_HandleEnum::Solenoid>
+ sH;
+ solenoidHandles = &sH;
+}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+
+HAL_SolenoidHandle HAL_InitializeSolenoidPort(HAL_PortHandle portHandle,
+ int32_t* status) {
+ hal::init::CheckInit();
+ int16_t channel = getPortHandleChannel(portHandle);
+ int16_t module = getPortHandleModule(portHandle);
+ if (channel == InvalidHandleIndex) {
+ *status = HAL_HANDLE_ERROR;
+ return HAL_kInvalidHandle;
+ }
+
+ // initializePCM will check the module
+ if (!HAL_CheckSolenoidChannel(channel)) {
+ *status = RESOURCE_OUT_OF_RANGE;
+ return HAL_kInvalidHandle;
+ }
+
+ initializePCM(module, status);
+ if (*status != 0) {
+ return HAL_kInvalidHandle;
+ }
+
+ auto handle = solenoidHandles->Allocate(
+ module * kNumSolenoidChannels + channel, status);
+ if (*status != 0) {
+ return HAL_kInvalidHandle;
+ }
+ auto solenoidPort = solenoidHandles->Get(handle);
+ if (solenoidPort == nullptr) { // would only occur on thread issues
+ *status = HAL_HANDLE_ERROR;
+ return HAL_kInvalidHandle;
+ }
+ solenoidPort->module = static_cast<uint8_t>(module);
+ solenoidPort->channel = static_cast<uint8_t>(channel);
+
+ return handle;
+}
+
+void HAL_FreeSolenoidPort(HAL_SolenoidHandle solenoidPortHandle) {
+ solenoidHandles->Free(solenoidPortHandle);
+}
+
+HAL_Bool HAL_CheckSolenoidModule(int32_t module) {
+ return module < kNumPCMModules && module >= 0;
+}
+
+HAL_Bool HAL_CheckSolenoidChannel(int32_t channel) {
+ return channel < kNumSolenoidChannels && channel >= 0;
+}
+
+HAL_Bool HAL_GetSolenoid(HAL_SolenoidHandle solenoidPortHandle,
+ int32_t* status) {
+ auto port = solenoidHandles->Get(solenoidPortHandle);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return false;
+ }
+ bool value;
+
+ *status = PCM_modules[port->module]->GetSolenoid(port->channel, value);
+
+ return value;
+}
+
+int32_t HAL_GetAllSolenoids(int32_t module, int32_t* status) {
+ if (!checkPCMInit(module, status)) return 0;
+ uint8_t value;
+
+ *status = PCM_modules[module]->GetAllSolenoids(value);
+
+ return value;
+}
+
+void HAL_SetSolenoid(HAL_SolenoidHandle solenoidPortHandle, HAL_Bool value,
+ int32_t* status) {
+ auto port = solenoidHandles->Get(solenoidPortHandle);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ *status = PCM_modules[port->module]->SetSolenoid(port->channel, value);
+}
+
+void HAL_SetAllSolenoids(int32_t module, int32_t state, int32_t* status) {
+ if (!checkPCMInit(module, status)) return;
+
+ *status = PCM_modules[module]->SetAllSolenoids(state);
+}
+
+int32_t HAL_GetPCMSolenoidBlackList(int32_t module, int32_t* status) {
+ if (!checkPCMInit(module, status)) return 0;
+ uint8_t value;
+
+ *status = PCM_modules[module]->GetSolenoidBlackList(value);
+
+ return value;
+}
+HAL_Bool HAL_GetPCMSolenoidVoltageStickyFault(int32_t module, int32_t* status) {
+ if (!checkPCMInit(module, status)) return 0;
+ bool value;
+
+ *status = PCM_modules[module]->GetSolenoidStickyFault(value);
+
+ return value;
+}
+HAL_Bool HAL_GetPCMSolenoidVoltageFault(int32_t module, int32_t* status) {
+ if (!checkPCMInit(module, status)) return false;
+ bool value;
+
+ *status = PCM_modules[module]->GetSolenoidFault(value);
+
+ return value;
+}
+void HAL_ClearAllPCMStickyFaults(int32_t module, int32_t* status) {
+ if (!checkPCMInit(module, status)) return;
+
+ *status = PCM_modules[module]->ClearStickyFaults();
+}
+
+void HAL_SetOneShotDuration(HAL_SolenoidHandle solenoidPortHandle,
+ int32_t durMS, int32_t* status) {
+ auto port = solenoidHandles->Get(solenoidPortHandle);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ *status =
+ PCM_modules[port->module]->SetOneShotDurationMs(port->channel, durMS);
+}
+
+void HAL_FireOneShot(HAL_SolenoidHandle solenoidPortHandle, int32_t* status) {
+ auto port = solenoidHandles->Get(solenoidPortHandle);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ *status = PCM_modules[port->module]->FireOneShotSolenoid(port->channel);
+}
+} // extern "C"
diff --git a/hal/src/main/native/athena/Threads.cpp b/hal/src/main/native/athena/Threads.cpp
new file mode 100644
index 0000000..95d1f59
--- /dev/null
+++ b/hal/src/main/native/athena/Threads.cpp
@@ -0,0 +1,93 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 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 "hal/Threads.h"
+
+#include <pthread.h>
+#include <sched.h>
+
+#include "hal/Errors.h"
+
+namespace hal {
+namespace init {
+void InitializeThreads() {}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+
+int32_t HAL_GetThreadPriority(NativeThreadHandle handle, HAL_Bool* isRealTime,
+ int32_t* status) {
+ sched_param sch;
+ int policy;
+ int success = pthread_getschedparam(
+ *reinterpret_cast<const pthread_t*>(handle), &policy, &sch);
+ if (success == 0) {
+ *status = 0;
+ } else {
+ *status = HAL_THREAD_PRIORITY_ERROR;
+ return -1;
+ }
+ if (policy == SCHED_FIFO || policy == SCHED_RR) {
+ *isRealTime = true;
+ return sch.sched_priority;
+ } else {
+ *isRealTime = false;
+ // 0 is the only suppored priority for non-realtime, so scale to 1
+ return 1;
+ }
+}
+
+int32_t HAL_GetCurrentThreadPriority(HAL_Bool* isRealTime, int32_t* status) {
+ auto thread = pthread_self();
+ return HAL_GetThreadPriority(&thread, isRealTime, status);
+}
+
+HAL_Bool HAL_SetThreadPriority(NativeThreadHandle handle, HAL_Bool realTime,
+ int32_t priority, int32_t* status) {
+ if (handle == nullptr) {
+ *status = NULL_PARAMETER;
+ return false;
+ }
+
+ int scheduler = realTime ? SCHED_FIFO : SCHED_OTHER;
+ if (realTime) {
+ // We don't support setting priorities for non RT threads
+ // so we don't need to check for proper range
+ if (priority < sched_get_priority_min(scheduler) ||
+ priority > sched_get_priority_max(scheduler)) {
+ *status = HAL_THREAD_PRIORITY_RANGE_ERROR;
+ return false;
+ }
+ }
+
+ sched_param sch;
+ int policy;
+ pthread_getschedparam(*reinterpret_cast<const pthread_t*>(handle), &policy,
+ &sch);
+ if (scheduler == SCHED_FIFO || scheduler == SCHED_RR)
+ sch.sched_priority = priority;
+ else
+ // Only need to set 0 priority for non RT thread
+ sch.sched_priority = 0;
+ if (pthread_setschedparam(*reinterpret_cast<const pthread_t*>(handle),
+ scheduler, &sch)) {
+ *status = HAL_THREAD_PRIORITY_ERROR;
+ return false;
+ } else {
+ *status = 0;
+ return true;
+ }
+}
+
+HAL_Bool HAL_SetCurrentThreadPriority(HAL_Bool realTime, int32_t priority,
+ int32_t* status) {
+ auto thread = pthread_self();
+ return HAL_SetThreadPriority(&thread, realTime, priority, status);
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/athena/cpp/SerialHelper.cpp b/hal/src/main/native/athena/cpp/SerialHelper.cpp
new file mode 100644
index 0000000..e312e2a
--- /dev/null
+++ b/hal/src/main/native/athena/cpp/SerialHelper.cpp
@@ -0,0 +1,333 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 "hal/cpp/SerialHelper.h"
+
+#include <algorithm>
+#include <cstdio>
+#include <cstring>
+
+#include <wpi/FileSystem.h>
+#include <wpi/StringRef.h>
+
+#include "hal/Errors.h"
+#include "visa/visa.h"
+
+constexpr const char* OnboardResourceVISA = "ASRL1::INSTR";
+constexpr const char* MxpResourceVISA = "ASRL2::INSTR";
+
+constexpr const char* OnboardResourceOS = "/dev/ttyS0";
+constexpr const char* MxpResourceOS = "/dev/ttyS1";
+
+namespace hal {
+
+std::string SerialHelper::m_usbNames[2]{"", ""};
+
+wpi::mutex SerialHelper::m_nameMutex;
+
+SerialHelper::SerialHelper() {
+ viOpenDefaultRM(reinterpret_cast<ViSession*>(&m_resourceHandle));
+}
+
+std::string SerialHelper::GetVISASerialPortName(HAL_SerialPort port,
+ int32_t* status) {
+ if (port == HAL_SerialPort::HAL_SerialPort_Onboard) {
+ return OnboardResourceVISA;
+ } else if (port == HAL_SerialPort::HAL_SerialPort_MXP) {
+ return MxpResourceVISA;
+ }
+
+ QueryHubPaths(status);
+
+ // If paths are empty or status error, return error
+ if (*status != 0 || m_visaResource.empty() || m_osResource.empty() ||
+ m_sortedHubPath.empty()) {
+ *status = HAL_SERIAL_PORT_NOT_FOUND;
+ return "";
+ }
+
+ int32_t visaIndex = GetIndexForPort(port, status);
+ if (visaIndex == -1) {
+ *status = HAL_SERIAL_PORT_NOT_FOUND;
+ return "";
+ // Error
+ } else {
+ return m_visaResource[visaIndex].str();
+ }
+}
+
+std::string SerialHelper::GetOSSerialPortName(HAL_SerialPort port,
+ int32_t* status) {
+ if (port == HAL_SerialPort::HAL_SerialPort_Onboard) {
+ return OnboardResourceOS;
+ } else if (port == HAL_SerialPort::HAL_SerialPort_MXP) {
+ return MxpResourceOS;
+ }
+
+ QueryHubPaths(status);
+
+ // If paths are empty or status error, return error
+ if (*status != 0 || m_visaResource.empty() || m_osResource.empty() ||
+ m_sortedHubPath.empty()) {
+ *status = HAL_SERIAL_PORT_NOT_FOUND;
+ return "";
+ }
+
+ int32_t osIndex = GetIndexForPort(port, status);
+ if (osIndex == -1) {
+ *status = HAL_SERIAL_PORT_NOT_FOUND;
+ return "";
+ // Error
+ } else {
+ return m_osResource[osIndex].str();
+ }
+}
+
+std::vector<std::string> SerialHelper::GetVISASerialPortList(int32_t* status) {
+ std::vector<std::string> retVec;
+
+ // Always add 2 onboard ports
+ retVec.emplace_back(OnboardResourceVISA);
+ retVec.emplace_back(MxpResourceVISA);
+
+ QueryHubPaths(status);
+
+ // If paths are empty or status error, return only onboard list
+ if (*status != 0 || m_visaResource.empty() || m_osResource.empty() ||
+ m_sortedHubPath.empty()) {
+ *status = 0;
+ return retVec;
+ }
+
+ for (auto& i : m_visaResource) {
+ retVec.emplace_back(i.str());
+ }
+
+ return retVec;
+}
+
+std::vector<std::string> SerialHelper::GetOSSerialPortList(int32_t* status) {
+ std::vector<std::string> retVec;
+
+ // Always add 2 onboard ports
+ retVec.emplace_back(OnboardResourceOS);
+ retVec.emplace_back(MxpResourceOS);
+
+ QueryHubPaths(status);
+
+ // If paths are empty or status error, return only onboard list
+ if (*status != 0 || m_visaResource.empty() || m_osResource.empty() ||
+ m_sortedHubPath.empty()) {
+ *status = 0;
+ return retVec;
+ }
+
+ for (auto& i : m_osResource) {
+ retVec.emplace_back(i.str());
+ }
+
+ return retVec;
+}
+
+void SerialHelper::SortHubPathVector() {
+ m_sortedHubPath.clear();
+ m_sortedHubPath = m_unsortedHubPath;
+ std::sort(m_sortedHubPath.begin(), m_sortedHubPath.end(),
+ [](const wpi::SmallVectorImpl<char>& lhs,
+ const wpi::SmallVectorImpl<char>& rhs) -> int {
+ wpi::StringRef lhsRef(lhs.begin(), lhs.size());
+ wpi::StringRef rhsRef(rhs.begin(), rhs.size());
+ return lhsRef.compare(rhsRef);
+ });
+}
+
+void SerialHelper::CoiteratedSort(
+ wpi::SmallVectorImpl<wpi::SmallString<16>>& vec) {
+ wpi::SmallVector<wpi::SmallString<16>, 4> sortedVec;
+ for (auto& str : m_sortedHubPath) {
+ for (size_t i = 0; i < m_unsortedHubPath.size(); i++) {
+ if (wpi::StringRef{m_unsortedHubPath[i].begin(),
+ m_unsortedHubPath[i].size()}
+ .equals(wpi::StringRef{str.begin(), str.size()})) {
+ sortedVec.push_back(vec[i]);
+ break;
+ }
+ }
+ }
+ vec = sortedVec;
+}
+
+void SerialHelper::QueryHubPaths(int32_t* status) {
+ // VISA resource matching string
+ const char* str = "?*";
+ // Items needed for VISA
+ ViUInt32 retCnt = 0;
+ ViFindList viList = 0;
+ ViChar desc[VI_FIND_BUFLEN];
+ *status = viFindRsrc(m_resourceHandle, const_cast<char*>(str), &viList,
+ &retCnt, desc);
+
+ if (*status < 0) {
+ // Handle the bad status elsewhere
+ // Note let positive statii (warnings) continue
+ goto done;
+ }
+ // Status might be positive, so reset it to 0
+ *status = 0;
+
+ // Storage buffer for Visa call
+ char osName[256];
+
+ // Loop through all returned VISA objects.
+ // Increment the internal VISA ptr every loop
+ for (size_t i = 0; i < retCnt; i++, viFindNext(viList, desc)) {
+ // Ignore any matches to the 2 onboard ports
+ if (std::strcmp(OnboardResourceVISA, desc) == 0 ||
+ std::strcmp(MxpResourceVISA, desc) == 0) {
+ continue;
+ }
+
+ // Open the resource, grab its interface name, and close it.
+ ViSession vSession;
+ *status = viOpen(m_resourceHandle, desc, VI_NULL, VI_NULL, &vSession);
+ if (*status < 0) goto done;
+ *status = 0;
+
+ *status = viGetAttribute(vSession, VI_ATTR_INTF_INST_NAME, &osName);
+ // Ignore an error here, as we want to close the session on an error
+ // Use a seperate close variable so we can check
+ ViStatus closeStatus = viClose(vSession);
+ if (*status < 0) goto done;
+ if (closeStatus < 0) goto done;
+ *status = 0;
+
+ // split until (/dev/
+ wpi::StringRef devNameRef = wpi::StringRef{osName}.split("(/dev/").second;
+ // String not found, continue
+ if (devNameRef.equals("")) continue;
+
+ // Split at )
+ wpi::StringRef matchString = devNameRef.split(')').first;
+ if (matchString.equals(devNameRef)) continue;
+
+ // Search directories to get a list of system accessors
+ // The directories we need are not symbolic, so we can safely
+ // disable symbolic links.
+ std::error_code ec;
+ for (auto p = wpi::sys::fs::recursive_directory_iterator(
+ "/sys/devices/soc0", ec, false);
+ p != wpi::sys::fs::recursive_directory_iterator(); p.increment(ec)) {
+ if (ec) break;
+ wpi::StringRef path{p->path()};
+ if (path.find("amba") == wpi::StringRef::npos) continue;
+ if (path.find("usb") == wpi::StringRef::npos) continue;
+ if (path.find(matchString) == wpi::StringRef::npos) continue;
+
+ wpi::SmallVector<wpi::StringRef, 16> pathSplitVec;
+ // Split path into individual directories
+ path.split(pathSplitVec, '/', -1, false);
+
+ // Find each individual item index
+ int findusb = -1;
+ int findtty = -1;
+ int findregex = -1;
+ for (size_t i = 0; i < pathSplitVec.size(); i++) {
+ if (findusb == -1 && pathSplitVec[i].equals("usb1")) {
+ findusb = i;
+ }
+ if (findtty == -1 && pathSplitVec[i].equals("tty")) {
+ findtty = i;
+ }
+ if (findregex == -1 && pathSplitVec[i].equals(matchString)) {
+ findregex = i;
+ }
+ }
+
+ // Get the index for our device
+ int hubIndex = findtty;
+ if (findtty == -1) hubIndex = findregex;
+
+ int devStart = findusb + 1;
+
+ if (hubIndex < devStart) continue;
+
+ // Add our devices to our list
+ m_unsortedHubPath.emplace_back(
+ wpi::StringRef{pathSplitVec[hubIndex - 2]});
+ m_visaResource.emplace_back(desc);
+ m_osResource.emplace_back(
+ wpi::StringRef{osName}.split("(").second.split(")").first);
+ break;
+ }
+ }
+
+ SortHubPathVector();
+
+ CoiteratedSort(m_visaResource);
+ CoiteratedSort(m_osResource);
+done:
+ viClose(viList);
+}
+
+int32_t SerialHelper::GetIndexForPort(HAL_SerialPort port, int32_t* status) {
+ // Hold lock whenever we're using the names array
+ std::scoped_lock lock(m_nameMutex);
+
+ std::string portString = m_usbNames[port - 2];
+
+ wpi::SmallVector<int32_t, 4> indices;
+
+ // If port has not been assigned, find the one to assign
+ if (portString.empty()) {
+ for (size_t i = 0; i < 2; i++) {
+ // Remove all used ports
+ auto idx = std::find(m_sortedHubPath.begin(), m_sortedHubPath.end(),
+ m_usbNames[i]);
+ if (idx != m_sortedHubPath.end()) {
+ // found
+ m_sortedHubPath.erase(idx);
+ }
+ if (m_usbNames[i] == "") {
+ indices.push_back(i);
+ }
+ }
+
+ int32_t idx = -1;
+ for (size_t i = 0; i < indices.size(); i++) {
+ if (indices[i] == port - 2) {
+ idx = i;
+ break;
+ }
+ }
+
+ if (idx == -1) {
+ *status = HAL_SERIAL_PORT_NOT_FOUND;
+ return -1;
+ }
+
+ if (idx >= static_cast<int32_t>(m_sortedHubPath.size())) {
+ *status = HAL_SERIAL_PORT_NOT_FOUND;
+ return -1;
+ }
+
+ portString = m_sortedHubPath[idx].str();
+ m_usbNames[port - 2] = portString;
+ }
+
+ int retIndex = -1;
+
+ for (size_t i = 0; i < m_sortedHubPath.size(); i++) {
+ if (m_sortedHubPath[i].equals(portString)) {
+ retIndex = i;
+ break;
+ }
+ }
+
+ return retIndex;
+}
+
+} // namespace hal
diff --git a/hal/src/main/native/athena/ctre/CtreCanNode.cpp b/hal/src/main/native/athena/ctre/CtreCanNode.cpp
new file mode 100644
index 0000000..440bebd
--- /dev/null
+++ b/hal/src/main/native/athena/ctre/CtreCanNode.cpp
@@ -0,0 +1,157 @@
+#pragma GCC diagnostic ignored "-Wmissing-field-initializers"
+
+#include "CtreCanNode.h"
+#include "FRC_NetworkCommunication/CANSessionMux.h"
+#include <string.h> // memset
+
+static const UINT32 kFullMessageIDMask = 0x1fffffff;
+
+CtreCanNode::CtreCanNode(UINT8 deviceNumber)
+{
+ _deviceNumber = deviceNumber;
+}
+CtreCanNode::~CtreCanNode()
+{
+}
+void CtreCanNode::RegisterRx(uint32_t arbId)
+{
+ /* no need to do anything, we just use new API to poll last received message */
+}
+/**
+ * Schedule a CAN Frame for periodic transmit.
+ * @param arbId CAN Frame Arbitration ID. Set BIT31 for 11bit ids, otherwise we use 29bit ids.
+ * @param periodMs Period to transmit CAN frame. Pass 0 for one-shot, which also disables that ArbID's preceding periodic transmit.
+ * @param dlc Number of bytes to transmit (0 to 8).
+ * @param initialFrame Ptr to the frame data to schedule for transmitting. Passing null will result
+ * in defaulting to zero data value.
+ */
+void CtreCanNode::RegisterTx(uint32_t arbId, uint32_t periodMs, uint32_t dlc, const uint8_t * initialFrame)
+{
+ int32_t status = 0;
+ if(dlc > 8)
+ dlc = 8;
+ txJob_t job = {0};
+ job.arbId = arbId;
+ job.periodMs = periodMs;
+ job.dlc = dlc;
+ if(initialFrame){
+ /* caller wants to specify original data */
+ memcpy(job.toSend, initialFrame, dlc);
+ }
+ _txJobs[arbId] = job;
+ FRC_NetworkCommunication_CANSessionMux_sendMessage( job.arbId,
+ job.toSend,
+ job.dlc,
+ job.periodMs,
+ &status);
+}
+/**
+ * Schedule a CAN Frame for periodic transmit. Assume eight byte DLC and zero value for initial transmission.
+ * @param arbId CAN Frame Arbitration ID. Set BIT31 for 11bit ids, otherwise we use 29bit ids.
+ * @param periodMs Period to transmit CAN frame. Pass 0 for one-shot, which also disables that ArbID's preceding periodic transmit.
+ */
+void CtreCanNode::RegisterTx(uint32_t arbId, uint32_t periodMs)
+{
+ RegisterTx(arbId,periodMs, 8, 0);
+}
+/**
+ * Remove a CAN frame Arbid to stop transmission.
+ * @param arbId CAN Frame Arbitration ID. Set BIT31 for 11bit ids, otherwise we use 29bit ids.
+ */
+void CtreCanNode::UnregisterTx(uint32_t arbId)
+{
+ /* set period to zero */
+ ChangeTxPeriod(arbId, 0);
+ /* look and remove */
+ txJobs_t::iterator iter = _txJobs.find(arbId);
+ if(iter != _txJobs.end()) {
+ _txJobs.erase(iter);
+ }
+}
+static int64_t GetTimeMs() {
+ std::chrono::time_point < std::chrono::system_clock > now;
+ now = std::chrono::system_clock::now();
+ auto duration = now.time_since_epoch();
+ auto millis = std::chrono::duration_cast < std::chrono::milliseconds
+ > (duration).count();
+ return (int64_t) millis;
+}
+CTR_Code CtreCanNode::GetRx(uint32_t arbId,uint8_t * dataBytes, uint32_t timeoutMs)
+{
+ CTR_Code retval = CTR_OKAY;
+ int32_t status = 0;
+ uint8_t len = 0;
+ uint32_t timeStamp;
+ /* cap timeout at 999ms */
+ if(timeoutMs > 999)
+ timeoutMs = 999;
+ FRC_NetworkCommunication_CANSessionMux_receiveMessage(&arbId,kFullMessageIDMask,dataBytes,&len,&timeStamp,&status);
+ std::scoped_lock lock(_lck);
+ if(status == 0){
+ /* fresh update */
+ rxEvent_t & r = _rxRxEvents[arbId]; /* lookup entry or make a default new one with all zeroes */
+ r.time = GetTimeMs();
+ memcpy(r.bytes, dataBytes, 8); /* fill in databytes */
+ }else{
+ /* did not get the message */
+ rxRxEvents_t::iterator i = _rxRxEvents.find(arbId);
+ if(i == _rxRxEvents.end()){
+ /* we've never gotten this mesage */
+ retval = CTR_RxTimeout;
+ /* fill caller's buffer with zeros */
+ memset(dataBytes,0,8);
+ }else{
+ /* we've gotten this message before but not recently */
+ memcpy(dataBytes,i->second.bytes,8);
+ /* get the time now */
+ int64_t now = GetTimeMs(); /* get now */
+ /* how long has it been? */
+ int64_t temp = now - i->second.time; /* temp = now - last */
+ if (temp > ((int64_t) timeoutMs)) {
+ retval = CTR_RxTimeout;
+ } else {
+ /* our last update was recent enough */
+ }
+ }
+ }
+
+ return retval;
+}
+void CtreCanNode::FlushTx(uint32_t arbId)
+{
+ int32_t status = 0;
+ txJobs_t::iterator iter = _txJobs.find(arbId);
+ if(iter != _txJobs.end())
+ FRC_NetworkCommunication_CANSessionMux_sendMessage( iter->second.arbId,
+ iter->second.toSend,
+ iter->second.dlc,
+ iter->second.periodMs,
+ &status);
+}
+/**
+ * Change the transmit period of an already scheduled CAN frame.
+ * This keeps the frame payload contents the same without caller having to perform
+ * a read-modify-write.
+ * @param arbId CAN Frame Arbitration ID. Set BIT31 for 11bit ids, otherwise we use 29bit ids.
+ * @param periodMs Period to transmit CAN frame. Pass 0 for one-shot, which also disables that ArbID's preceding periodic transmit.
+ * @return true if scheduled job was found and updated, false if there was no preceding job for the specified arbID.
+ */
+bool CtreCanNode::ChangeTxPeriod(uint32_t arbId, uint32_t periodMs)
+{
+ int32_t status = 0;
+ /* lookup the data bytes and period for this message */
+ txJobs_t::iterator iter = _txJobs.find(arbId);
+ if(iter != _txJobs.end()) {
+ /* modify th periodMs */
+ iter->second.periodMs = periodMs;
+ /* reinsert into scheduler with the same data bytes, only the period changed. */
+ FRC_NetworkCommunication_CANSessionMux_sendMessage( iter->second.arbId,
+ iter->second.toSend,
+ iter->second.dlc,
+ iter->second.periodMs,
+ &status);
+ return true;
+ }
+ return false;
+}
+
diff --git a/hal/src/main/native/athena/ctre/CtreCanNode.h b/hal/src/main/native/athena/ctre/CtreCanNode.h
new file mode 100644
index 0000000..f00060d
--- /dev/null
+++ b/hal/src/main/native/athena/ctre/CtreCanNode.h
@@ -0,0 +1,134 @@
+#ifndef CtreCanNode_H_
+#define CtreCanNode_H_
+#include "ctre.h" //BIT Defines + Typedefs
+#include <map>
+#include <string.h> // memcpy
+#include <sys/time.h>
+#include <wpi/mutex.h>
+class CtreCanNode
+{
+public:
+ CtreCanNode(UINT8 deviceNumber);
+ ~CtreCanNode();
+
+ UINT8 GetDeviceNumber()
+ {
+ return _deviceNumber;
+ }
+protected:
+
+
+ template <typename T> class txTask{
+ public:
+ uint32_t arbId;
+ T * toSend;
+ T * operator -> ()
+ {
+ return toSend;
+ }
+ T & operator*()
+ {
+ return *toSend;
+ }
+ bool IsEmpty()
+ {
+ if(toSend == 0)
+ return true;
+ return false;
+ }
+ };
+ template <typename T> class recMsg{
+ public:
+ uint32_t arbId;
+ uint8_t bytes[8];
+ CTR_Code err;
+ T * operator -> ()
+ {
+ return (T *)bytes;
+ }
+ T & operator*()
+ {
+ return *(T *)bytes;
+ }
+ };
+ UINT8 _deviceNumber;
+ void RegisterRx(uint32_t arbId);
+ /**
+ * Schedule a CAN Frame for periodic transmit. Assume eight byte DLC and zero value for initial transmission.
+ * @param arbId CAN Frame Arbitration ID. Set BIT31 for 11bit ids, otherwise we use 29bit ids.
+ * @param periodMs Period to transmit CAN frame. Pass 0 for one-shot, which also disables that ArbID's preceding periodic transmit.
+ */
+ void RegisterTx(uint32_t arbId, uint32_t periodMs);
+ /**
+ * Schedule a CAN Frame for periodic transmit.
+ * @param arbId CAN Frame Arbitration ID. Set BIT31 for 11bit ids, otherwise we use 29bit ids.
+ * @param periodMs Period to transmit CAN frame. Pass 0 for one-shot, which also disables that ArbID's preceding periodic transmit.
+ * @param dlc Number of bytes to transmit (0 to 8).
+ * @param initialFrame Ptr to the frame data to schedule for transmitting. Passing null will result
+ * in defaulting to zero data value.
+ */
+ void RegisterTx(uint32_t arbId, uint32_t periodMs, uint32_t dlc, const uint8_t * initialFrame);
+ void UnregisterTx(uint32_t arbId);
+
+ CTR_Code GetRx(uint32_t arbId,uint8_t * dataBytes,uint32_t timeoutMs);
+ void FlushTx(uint32_t arbId);
+ bool ChangeTxPeriod(uint32_t arbId, uint32_t periodMs);
+
+ template<typename T> txTask<T> GetTx(uint32_t arbId)
+ {
+ txTask<T> retval = {0, nullptr};
+ txJobs_t::iterator i = _txJobs.find(arbId);
+ if(i != _txJobs.end()){
+ retval.arbId = i->second.arbId;
+ retval.toSend = (T*)i->second.toSend;
+ }
+ return retval;
+ }
+ template<class T> void FlushTx(T & par)
+ {
+ FlushTx(par.arbId);
+ }
+
+ template<class T> recMsg<T> GetRx(uint32_t arbId, uint32_t timeoutMs)
+ {
+ recMsg<T> retval;
+ retval.err = GetRx(arbId,retval.bytes, timeoutMs);
+ return retval;
+ }
+
+private:
+
+ class txJob_t {
+ public:
+ uint32_t arbId;
+ uint8_t toSend[8];
+ uint32_t periodMs;
+ uint8_t dlc;
+ };
+
+ class rxEvent_t{
+ public:
+ uint8_t bytes[8];
+ int64_t time;
+ rxEvent_t()
+ {
+ bytes[0] = 0;
+ bytes[1] = 0;
+ bytes[2] = 0;
+ bytes[3] = 0;
+ bytes[4] = 0;
+ bytes[5] = 0;
+ bytes[6] = 0;
+ bytes[7] = 0;
+ }
+ };
+
+ typedef std::map<uint32_t,txJob_t> txJobs_t;
+ txJobs_t _txJobs;
+
+ typedef std::map<uint32_t,rxEvent_t> rxRxEvents_t;
+ rxRxEvents_t _rxRxEvents;
+
+ wpi::mutex _lck;
+};
+#endif
diff --git a/hal/src/main/native/athena/ctre/PCM.cpp b/hal/src/main/native/athena/ctre/PCM.cpp
new file mode 100644
index 0000000..e05d4d4
--- /dev/null
+++ b/hal/src/main/native/athena/ctre/PCM.cpp
@@ -0,0 +1,574 @@
+#pragma GCC diagnostic ignored "-Wmissing-field-initializers"
+
+#include "PCM.h"
+#include "FRC_NetworkCommunication/CANSessionMux.h"
+#include <string.h> // memset
+/* This can be a constant, as long as nobody needs to update solenoids within
+ 1/50 of a second. */
+static const INT32 kCANPeriod = 20;
+
+#define STATUS_1 0x9041400
+#define STATUS_SOL_FAULTS 0x9041440
+#define STATUS_DEBUG 0x9041480
+
+#define EXPECTED_RESPONSE_TIMEOUT_MS (50)
+#define GET_PCM_STATUS() CtreCanNode::recMsg<PcmStatus_t> rx = GetRx<PcmStatus_t> (STATUS_1|GetDeviceNumber(),EXPECTED_RESPONSE_TIMEOUT_MS)
+#define GET_PCM_SOL_FAULTS() CtreCanNode::recMsg<PcmStatusFault_t> rx = GetRx<PcmStatusFault_t> (STATUS_SOL_FAULTS|GetDeviceNumber(),EXPECTED_RESPONSE_TIMEOUT_MS)
+#define GET_PCM_DEBUG() CtreCanNode::recMsg<PcmDebug_t> rx = GetRx<PcmDebug_t> (STATUS_DEBUG|GetDeviceNumber(),EXPECTED_RESPONSE_TIMEOUT_MS)
+
+#define CONTROL_1 0x09041C00 /* PCM_Control */
+#define CONTROL_2 0x09041C40 /* PCM_SupplemControl */
+#define CONTROL_3 0x09041C80 /* PcmControlSetOneShotDur_t */
+
+/* encoder/decoders */
+typedef struct _PcmStatus_t{
+ /* Byte 0 */
+ unsigned SolenoidBits:8;
+ /* Byte 1 */
+ unsigned compressorOn:1;
+ unsigned stickyFaultFuseTripped:1;
+ unsigned stickyFaultCompCurrentTooHigh:1;
+ unsigned faultFuseTripped:1;
+ unsigned faultCompCurrentTooHigh:1;
+ unsigned faultHardwareFailure:1;
+ unsigned isCloseloopEnabled:1;
+ unsigned pressureSwitchEn:1;
+ /* Byte 2*/
+ unsigned battVoltage:8;
+ /* Byte 3 */
+ unsigned solenoidVoltageTop8:8;
+ /* Byte 4 */
+ unsigned compressorCurrentTop6:6;
+ unsigned solenoidVoltageBtm2:2;
+ /* Byte 5 */
+ unsigned StickyFault_dItooHigh :1;
+ unsigned Fault_dItooHigh :1;
+ unsigned moduleEnabled:1;
+ unsigned closedLoopOutput:1;
+ unsigned compressorCurrentBtm4:4;
+ /* Byte 6 */
+ unsigned tokenSeedTop8:8;
+ /* Byte 7 */
+ unsigned tokenSeedBtm8:8;
+}PcmStatus_t;
+
+typedef struct _PcmControl_t{
+ /* Byte 0 */
+ unsigned tokenTop8:8;
+ /* Byte 1 */
+ unsigned tokenBtm8:8;
+ /* Byte 2 */
+ unsigned solenoidBits:8;
+ /* Byte 3*/
+ unsigned reserved:4;
+ unsigned closeLoopOutput:1;
+ unsigned compressorOn:1;
+ unsigned closedLoopEnable:1;
+ unsigned clearStickyFaults:1;
+ /* Byte 4 */
+ unsigned OneShotField_h8:8;
+ /* Byte 5 */
+ unsigned OneShotField_l8:8;
+}PcmControl_t;
+
+typedef struct _PcmControlSetOneShotDur_t{
+ uint8_t sol10MsPerUnit[8];
+}PcmControlSetOneShotDur_t;
+
+typedef struct _PcmStatusFault_t{
+ /* Byte 0 */
+ unsigned SolenoidBlacklist:8;
+ /* Byte 1 */
+ unsigned reserved_bit0 :1;
+ unsigned reserved_bit1 :1;
+ unsigned reserved_bit2 :1;
+ unsigned reserved_bit3 :1;
+ unsigned StickyFault_CompNoCurrent :1;
+ unsigned Fault_CompNoCurrent :1;
+ unsigned StickyFault_SolenoidJumper :1;
+ unsigned Fault_SolenoidJumper :1;
+}PcmStatusFault_t;
+
+typedef struct _PcmDebug_t{
+ unsigned tokFailsTop8:8;
+ unsigned tokFailsBtm8:8;
+ unsigned lastFailedTokTop8:8;
+ unsigned lastFailedTokBtm8:8;
+ unsigned tokSuccessTop8:8;
+ unsigned tokSuccessBtm8:8;
+}PcmDebug_t;
+
+
+/* PCM Constructor - Clears all vars, establishes default settings, starts PCM background process
+ *
+ * @Return - void
+ *
+ * @Param - deviceNumber - Device ID of PCM to be controlled
+ */
+PCM::PCM(UINT8 deviceNumber): CtreCanNode(deviceNumber)
+{
+ RegisterRx(STATUS_1 | deviceNumber );
+ RegisterRx(STATUS_SOL_FAULTS | deviceNumber );
+ RegisterRx(STATUS_DEBUG | deviceNumber );
+ RegisterTx(CONTROL_1 | deviceNumber, kCANPeriod);
+ /* enable close loop */
+ SetClosedLoopControl(1);
+}
+/* PCM D'tor
+ */
+PCM::~PCM()
+{
+
+}
+
+/* Set PCM solenoid state
+ *
+ * @Return - CTR_Code - Error code (if any) for setting solenoid
+ *
+ * @Param - idx - ID of solenoid (0-7)
+ * @Param - en - Enable / Disable identified solenoid
+ */
+CTR_Code PCM::SetSolenoid(unsigned char idx, bool en)
+{
+ CtreCanNode::txTask<PcmControl_t> toFill = GetTx<PcmControl_t>(CONTROL_1 | GetDeviceNumber());
+ if(toFill.IsEmpty())return CTR_UnexpectedArbId;
+ if (en)
+ toFill->solenoidBits |= (1ul << (idx));
+ else
+ toFill->solenoidBits &= ~(1ul << (idx));
+ FlushTx(toFill);
+ return CTR_OKAY;
+}
+
+/* Set all PCM solenoid states
+ *
+ * @Return - CTR_Code - Error code (if any) for setting solenoids
+ * @Param - state Bitfield to set all solenoids to
+ */
+CTR_Code PCM::SetAllSolenoids(UINT8 state) {
+ CtreCanNode::txTask<PcmControl_t> toFill = GetTx<PcmControl_t>(CONTROL_1 | GetDeviceNumber());
+ if(toFill.IsEmpty())return CTR_UnexpectedArbId;
+ toFill->solenoidBits = state;
+ FlushTx(toFill);
+ return CTR_OKAY;
+}
+
+/* Clears PCM sticky faults (indicators of past faults
+ *
+ * @Return - CTR_Code - Error code (if any) for setting solenoid
+ *
+ * @Param - clr - Clear / do not clear faults
+ */
+CTR_Code PCM::ClearStickyFaults()
+{
+ int32_t status = 0;
+ uint8_t pcmSupplemControl[] = { 0, 0, 0, 0x80 }; /* only bit set is ClearStickyFaults */
+ FRC_NetworkCommunication_CANSessionMux_sendMessage(CONTROL_2 | GetDeviceNumber(), pcmSupplemControl, sizeof(pcmSupplemControl), 0, &status);
+ if(status)
+ return CTR_TxFailed;
+ return CTR_OKAY;
+}
+
+/* Enables PCM Closed Loop Control of Compressor via pressure switch
+ *
+ * @Return - CTR_Code - Error code (if any) for setting solenoid
+ *
+ * @Param - en - Enable / Disable Closed Loop Control
+ */
+CTR_Code PCM::SetClosedLoopControl(bool en)
+{
+ CtreCanNode::txTask<PcmControl_t> toFill = GetTx<PcmControl_t>(CONTROL_1 | GetDeviceNumber());
+ if(toFill.IsEmpty())return CTR_UnexpectedArbId;
+ toFill->closedLoopEnable = en;
+ FlushTx(toFill);
+ return CTR_OKAY;
+}
+/* Get solenoid Blacklist status
+ * @Return - CTR_Code - Error code (if any)
+ * @Param - idx - ID of solenoid [0,7] to fire one shot pulse.
+ */
+CTR_Code PCM::FireOneShotSolenoid(UINT8 idx)
+{
+ CtreCanNode::txTask<PcmControl_t> toFill = GetTx<PcmControl_t>(CONTROL_1 | GetDeviceNumber());
+ if(toFill.IsEmpty())return CTR_UnexpectedArbId;
+ /* grab field as it is now */
+ uint16_t oneShotField;
+ oneShotField = toFill->OneShotField_h8;
+ oneShotField <<= 8;
+ oneShotField |= toFill->OneShotField_l8;
+ /* get the caller's channel */
+ uint16_t shift = 2*idx;
+ uint16_t mask = 3; /* two bits wide */
+ uint8_t chBits = (oneShotField >> shift) & mask;
+ /* flip it */
+ chBits = (chBits)%3 + 1;
+ /* clear out 2bits for this channel*/
+ oneShotField &= ~(mask << shift);
+ /* put new field in */
+ oneShotField |= chBits << shift;
+ /* apply field as it is now */
+ toFill->OneShotField_h8 = oneShotField >> 8;
+ toFill->OneShotField_l8 = oneShotField;
+ FlushTx(toFill);
+ return CTR_OKAY;
+}
+/* Configure the pulse width of a solenoid channel for one-shot pulse.
+ * Preprogrammed pulsewidth is 10ms resolution and can be between 10ms and
+ * 2.55s.
+ *
+ * @Return - CTR_Code - Error code (if any)
+ * @Param - idx - ID of solenoid [0,7] to configure.
+ * @Param - durMs - pulse width in ms.
+ */
+CTR_Code PCM::SetOneShotDurationMs(UINT8 idx,uint32_t durMs)
+{
+ /* sanity check caller's param */
+ if(idx > 7)
+ return CTR_InvalidParamValue;
+ /* get latest tx frame */
+ CtreCanNode::txTask<PcmControlSetOneShotDur_t> toFill = GetTx<PcmControlSetOneShotDur_t>(CONTROL_3 | GetDeviceNumber());
+ if(toFill.IsEmpty()){
+ /* only send this out if caller wants to do one-shots */
+ RegisterTx(CONTROL_3 | _deviceNumber, kCANPeriod);
+ /* grab it */
+ toFill = GetTx<PcmControlSetOneShotDur_t>(CONTROL_3 | GetDeviceNumber());
+ }
+ toFill->sol10MsPerUnit[idx] = std::min(durMs/10,(uint32_t)0xFF);
+ /* apply the new data bytes */
+ FlushTx(toFill);
+ return CTR_OKAY;
+}
+
+/* Get solenoid state
+ *
+ * @Return - True/False - True if solenoid enabled, false otherwise
+ *
+ * @Param - idx - ID of solenoid (0-7) to return status of
+ */
+CTR_Code PCM::GetSolenoid(UINT8 idx, bool &status)
+{
+ GET_PCM_STATUS();
+ status = (rx->SolenoidBits & (1ul<<(idx)) ) ? 1 : 0;
+ return rx.err;
+}
+
+/* Get solenoid state for all solenoids on the PCM
+ *
+ * @Return - Bitfield of solenoid states
+ */
+CTR_Code PCM::GetAllSolenoids(UINT8 &status)
+{
+ GET_PCM_STATUS();
+ status = rx->SolenoidBits;
+ return rx.err;
+}
+
+/* Get pressure switch state
+ *
+ * @Return - True/False - True if pressure adequate, false if low
+ */
+CTR_Code PCM::GetPressure(bool &status)
+{
+ GET_PCM_STATUS();
+ status = (rx->pressureSwitchEn ) ? 1 : 0;
+ return rx.err;
+}
+
+/* Get compressor state
+ *
+ * @Return - True/False - True if enabled, false if otherwise
+ */
+CTR_Code PCM::GetCompressor(bool &status)
+{
+ GET_PCM_STATUS();
+ status = (rx->compressorOn);
+ return rx.err;
+}
+
+/* Get closed loop control state
+ *
+ * @Return - True/False - True if closed loop enabled, false if otherwise
+ */
+CTR_Code PCM::GetClosedLoopControl(bool &status)
+{
+ GET_PCM_STATUS();
+ status = (rx->isCloseloopEnabled);
+ return rx.err;
+}
+
+/* Get compressor current draw
+ *
+ * @Return - Amperes - Compressor current
+ */
+CTR_Code PCM::GetCompressorCurrent(float &status)
+{
+ GET_PCM_STATUS();
+ uint32_t temp =(rx->compressorCurrentTop6);
+ temp <<= 4;
+ temp |= rx->compressorCurrentBtm4;
+ status = temp * 0.03125; /* 5.5 fixed pt value in Amps */
+ return rx.err;
+}
+
+/* Get voltage across solenoid rail
+ *
+ * @Return - Volts - Voltage across solenoid rail
+ */
+CTR_Code PCM::GetSolenoidVoltage(float &status)
+{
+ GET_PCM_STATUS();
+ uint32_t raw =(rx->solenoidVoltageTop8);
+ raw <<= 2;
+ raw |= rx->solenoidVoltageBtm2;
+ status = (double) raw * 0.03125; /* 5.5 fixed pt value in Volts */
+ return rx.err;
+}
+
+/* Get hardware fault value
+ *
+ * @Return - True/False - True if hardware failure detected, false if otherwise
+ */
+CTR_Code PCM::GetHardwareFault(bool &status)
+{
+ GET_PCM_STATUS();
+ status = rx->faultHardwareFailure;
+ return rx.err;
+}
+
+/* Get compressor fault value
+ *
+ * @Return - True/False - True if shorted compressor detected, false if otherwise
+ */
+CTR_Code PCM::GetCompressorCurrentTooHighFault(bool &status)
+{
+ GET_PCM_STATUS();
+ status = rx->faultCompCurrentTooHigh;
+ return rx.err;
+}
+CTR_Code PCM::GetCompressorShortedStickyFault(bool &status)
+{
+ GET_PCM_STATUS();
+ status = rx->StickyFault_dItooHigh;
+ return rx.err;
+}
+CTR_Code PCM::GetCompressorShortedFault(bool &status)
+{
+ GET_PCM_STATUS();
+ status = rx->Fault_dItooHigh;
+ return rx.err;
+}
+CTR_Code PCM::GetCompressorNotConnectedStickyFault(bool &status)
+{
+ GET_PCM_SOL_FAULTS();
+ status = rx->StickyFault_CompNoCurrent;
+ return rx.err;
+}
+CTR_Code PCM::GetCompressorNotConnectedFault(bool &status)
+{
+ GET_PCM_SOL_FAULTS();
+ status = rx->Fault_CompNoCurrent;
+ return rx.err;
+}
+
+/* Get solenoid fault value
+ *
+ * @Return - True/False - True if shorted solenoid detected, false if otherwise
+ */
+CTR_Code PCM::GetSolenoidFault(bool &status)
+{
+ GET_PCM_STATUS();
+ status = rx->faultFuseTripped;
+ return rx.err;
+}
+
+/* Get compressor sticky fault value
+ *
+ * @Return - True/False - True if solenoid had previously been shorted
+ * (and sticky fault was not cleared), false if otherwise
+ */
+CTR_Code PCM::GetCompressorCurrentTooHighStickyFault(bool &status)
+{
+ GET_PCM_STATUS();
+ status = rx->stickyFaultCompCurrentTooHigh;
+ return rx.err;
+}
+
+/* Get solenoid sticky fault value
+ *
+ * @Return - True/False - True if compressor had previously been shorted
+ * (and sticky fault was not cleared), false if otherwise
+ */
+CTR_Code PCM::GetSolenoidStickyFault(bool &status)
+{
+ GET_PCM_STATUS();
+ status = rx->stickyFaultFuseTripped;
+ return rx.err;
+}
+/* Get battery voltage
+ *
+ * @Return - Volts - Voltage across PCM power ports
+ */
+CTR_Code PCM::GetBatteryVoltage(float &status)
+{
+ GET_PCM_STATUS();
+ status = (float)rx->battVoltage * 0.05 + 4.0; /* 50mV per unit plus 4V. */
+ return rx.err;
+}
+/* Return status of module enable/disable
+ *
+ * @Return - bool - Returns TRUE if PCM is enabled, FALSE if disabled
+ */
+CTR_Code PCM::isModuleEnabled(bool &status)
+{
+ GET_PCM_STATUS();
+ status = rx->moduleEnabled;
+ return rx.err;
+}
+/* Get number of total failed PCM Control Frame
+ *
+ * @Return - Failed Control Frames - Number of failed control frames (tokenization fails)
+ *
+ * @WARNING - Return only valid if [SeekDebugFrames] is enabled
+ * See function SeekDebugFrames
+ * See function EnableSeekDebugFrames
+ */
+CTR_Code PCM::GetNumberOfFailedControlFrames(UINT16 &status)
+{
+ GET_PCM_DEBUG();
+ status = rx->tokFailsTop8;
+ status <<= 8;
+ status |= rx->tokFailsBtm8;
+ return rx.err;
+}
+/* Get raw Solenoid Blacklist
+ *
+ * @Return - BINARY - Raw binary breakdown of Solenoid Blacklist
+ * BIT7 = Solenoid 1, BIT6 = Solenoid 2, etc.
+ *
+ * @WARNING - Return only valid if [SeekStatusFaultFrames] is enabled
+ * See function SeekStatusFaultFrames
+ * See function EnableSeekStatusFaultFrames
+ */
+CTR_Code PCM::GetSolenoidBlackList(UINT8 &status)
+{
+ GET_PCM_SOL_FAULTS();
+ status = rx->SolenoidBlacklist;
+ return rx.err;
+}
+/* Get solenoid Blacklist status
+ * - Blacklisted solenoids cannot be enabled until PCM is power cycled
+ *
+ * @Return - True/False - True if Solenoid is blacklisted, false if otherwise
+ *
+ * @Param - idx - ID of solenoid [0,7]
+ *
+ * @WARNING - Return only valid if [SeekStatusFaultFrames] is enabled
+ * See function SeekStatusFaultFrames
+ * See function EnableSeekStatusFaultFrames
+ */
+CTR_Code PCM::IsSolenoidBlacklisted(UINT8 idx, bool &status)
+{
+ GET_PCM_SOL_FAULTS();
+ status = (rx->SolenoidBlacklist & (1ul<<(idx)) )? 1 : 0;
+ return rx.err;
+}
+//------------------ C interface --------------------------------------------//
+extern "C" {
+ void * c_PCM_Init(void) {
+ return new PCM();
+ }
+ CTR_Code c_SetSolenoid(void * handle, unsigned char idx, INT8 param) {
+ return ((PCM*) handle)->SetSolenoid(idx, param);
+ }
+ CTR_Code c_SetAllSolenoids(void * handle, UINT8 state) {
+ return ((PCM*) handle)->SetAllSolenoids(state);
+ }
+ CTR_Code c_SetClosedLoopControl(void * handle, INT8 param) {
+ return ((PCM*) handle)->SetClosedLoopControl(param);
+ }
+ CTR_Code c_ClearStickyFaults(void * handle, INT8 param) {
+ return ((PCM*) handle)->ClearStickyFaults();
+ }
+ CTR_Code c_GetSolenoid(void * handle, UINT8 idx, INT8 * status) {
+ bool bstatus;
+ CTR_Code retval = ((PCM*) handle)->GetSolenoid(idx, bstatus);
+ *status = bstatus;
+ return retval;
+ }
+ CTR_Code c_GetAllSolenoids(void * handle, UINT8 * status) {
+ return ((PCM*) handle)->GetAllSolenoids(*status);
+ }
+ CTR_Code c_GetPressure(void * handle, INT8 * status) {
+ bool bstatus;
+ CTR_Code retval = ((PCM*) handle)->GetPressure(bstatus);
+ *status = bstatus;
+ return retval;
+ }
+ CTR_Code c_GetCompressor(void * handle, INT8 * status) {
+ bool bstatus;
+ CTR_Code retval = ((PCM*) handle)->GetCompressor(bstatus);
+ *status = bstatus;
+ return retval;
+ }
+ CTR_Code c_GetClosedLoopControl(void * handle, INT8 * status) {
+ bool bstatus;
+ CTR_Code retval = ((PCM*) handle)->GetClosedLoopControl(bstatus);
+ *status = bstatus;
+ return retval;
+ }
+ CTR_Code c_GetCompressorCurrent(void * handle, float * status) {
+ CTR_Code retval = ((PCM*) handle)->GetCompressorCurrent(*status);
+ return retval;
+ }
+ CTR_Code c_GetSolenoidVoltage(void * handle, float*status) {
+ return ((PCM*) handle)->GetSolenoidVoltage(*status);
+ }
+ CTR_Code c_GetHardwareFault(void * handle, INT8*status) {
+ bool bstatus;
+ CTR_Code retval = ((PCM*) handle)->GetHardwareFault(bstatus);
+ *status = bstatus;
+ return retval;
+ }
+ CTR_Code c_GetCompressorFault(void * handle, INT8*status) {
+ bool bstatus;
+ CTR_Code retval = ((PCM*) handle)->GetCompressorCurrentTooHighFault(bstatus);
+ *status = bstatus;
+ return retval;
+ }
+ CTR_Code c_GetSolenoidFault(void * handle, INT8*status) {
+ bool bstatus;
+ CTR_Code retval = ((PCM*) handle)->GetSolenoidFault(bstatus);
+ *status = bstatus;
+ return retval;
+ }
+ CTR_Code c_GetCompressorStickyFault(void * handle, INT8*status) {
+ bool bstatus;
+ CTR_Code retval = ((PCM*) handle)->GetCompressorCurrentTooHighStickyFault(bstatus);
+ *status = bstatus;
+ return retval;
+ }
+ CTR_Code c_GetSolenoidStickyFault(void * handle, INT8*status) {
+ bool bstatus;
+ CTR_Code retval = ((PCM*) handle)->GetSolenoidStickyFault(bstatus);
+ *status = bstatus;
+ return retval;
+ }
+ CTR_Code c_GetBatteryVoltage(void * handle, float*status) {
+ CTR_Code retval = ((PCM*) handle)->GetBatteryVoltage(*status);
+ return retval;
+ }
+ void c_SetDeviceNumber_PCM(void * handle, UINT8 deviceNumber) {
+ }
+ CTR_Code c_GetNumberOfFailedControlFrames(void * handle, UINT16*status) {
+ return ((PCM*) handle)->GetNumberOfFailedControlFrames(*status);
+ }
+ CTR_Code c_GetSolenoidBlackList(void * handle, UINT8 *status) {
+ return ((PCM*) handle)->GetSolenoidBlackList(*status);
+ }
+ CTR_Code c_IsSolenoidBlacklisted(void * handle, UINT8 idx, INT8*status) {
+ bool bstatus;
+ CTR_Code retval = ((PCM*) handle)->IsSolenoidBlacklisted(idx, bstatus);
+ *status = bstatus;
+ return retval;
+ }
+}
diff --git a/hal/src/main/native/athena/ctre/PCM.h b/hal/src/main/native/athena/ctre/PCM.h
new file mode 100644
index 0000000..b485219
--- /dev/null
+++ b/hal/src/main/native/athena/ctre/PCM.h
@@ -0,0 +1,226 @@
+#ifndef PCM_H_
+#define PCM_H_
+#include "ctre.h" //BIT Defines + Typedefs
+#include "CtreCanNode.h"
+class PCM : public CtreCanNode
+{
+public:
+ PCM(UINT8 deviceNumber=0);
+ ~PCM();
+
+ /* Set PCM solenoid state
+ *
+ * @Return - CTR_Code - Error code (if any) for setting solenoid
+ * @Param - idx - ID of solenoid (0-7)
+ * @Param - en - Enable / Disable identified solenoid
+ */
+ CTR_Code SetSolenoid(unsigned char idx, bool en);
+
+ /* Set all PCM solenoid states
+ *
+ * @Return - CTR_Code - Error code (if any) for setting solenoids
+ * @Param - state Bitfield to set all solenoids to
+ */
+ CTR_Code SetAllSolenoids(UINT8 state);
+
+ /* Enables PCM Closed Loop Control of Compressor via pressure switch
+ * @Return - CTR_Code - Error code (if any) for setting solenoid
+ * @Param - en - Enable / Disable Closed Loop Control
+ */
+ CTR_Code SetClosedLoopControl(bool en);
+
+ /* Clears PCM sticky faults (indicators of past faults
+ * @Return - CTR_Code - Error code (if any) for setting solenoid
+ */
+ CTR_Code ClearStickyFaults();
+
+ /* Get solenoid state
+ *
+ * @Return - CTR_Code - Error code (if any)
+ * @Param - idx - ID of solenoid (0-7) to return if solenoid is on.
+ * @Param - status - true if solenoid enabled, false otherwise
+ */
+ CTR_Code GetSolenoid(UINT8 idx, bool &status);
+
+ /* Get state of all solenoids
+ *
+ * @Return - CTR_Code - Error code (if any)
+ * @Param - status - bitfield of solenoid states
+ */
+ CTR_Code GetAllSolenoids(UINT8 &status);
+
+ /* Get pressure switch state
+ * @Return - CTR_Code - Error code (if any)
+ * @Param - status - True if pressure adequate, false if low
+ */
+ CTR_Code GetPressure(bool &status);
+
+ /* Get compressor state
+ * @Return - CTR_Code - Error code (if any)
+ * @Param - status - True if compress ouput is on, false if otherwise
+ */
+ CTR_Code GetCompressor(bool &status);
+
+ /* Get closed loop control state
+ * @Return - CTR_Code - Error code (if any)
+ * @Param - status - True if closed loop enabled, false if otherwise
+ */
+ CTR_Code GetClosedLoopControl(bool &status);
+
+ /* Get compressor current draw
+ * @Return - CTR_Code - Error code (if any)
+ * @Param - status - Compressor current returned in Amperes (A)
+ */
+ CTR_Code GetCompressorCurrent(float &status);
+
+ /* Get voltage across solenoid rail
+ * @Return - CTR_Code - Error code (if any)
+ * @Param - status - Voltage across solenoid rail in Volts (V)
+ */
+ CTR_Code GetSolenoidVoltage(float &status);
+
+ /* Get hardware fault value
+ * @Return - CTR_Code - Error code (if any)
+ * @Param - status - True if hardware failure detected, false if otherwise
+ */
+ CTR_Code GetHardwareFault(bool &status);
+
+ /* Get compressor fault value
+ * @Return - CTR_Code - Error code (if any)
+ * @Param - status - True if abnormally high compressor current detected, false if otherwise
+ */
+ CTR_Code GetCompressorCurrentTooHighFault(bool &status);
+
+ /* Get solenoid fault value
+ * @Return - CTR_Code - Error code (if any)
+ * @Param - status - True if shorted solenoid detected, false if otherwise
+ */
+ CTR_Code GetSolenoidFault(bool &status);
+
+ /* Get compressor sticky fault value
+ * @Return - CTR_Code - Error code (if any)
+ * @Param - status - True if solenoid had previously been shorted
+ * (and sticky fault was not cleared), false if otherwise
+ */
+ CTR_Code GetCompressorCurrentTooHighStickyFault(bool &status);
+ /* Get compressor shorted sticky fault value
+ * @Return - CTR_Code - Error code (if any)
+ * @Param - status - True if compressor output is shorted, false if otherwise
+ */
+ CTR_Code GetCompressorShortedStickyFault(bool &status);
+ /* Get compressor shorted fault value
+ * @Return - CTR_Code - Error code (if any)
+ * @Param - status - True if compressor output is shorted, false if otherwise
+ */
+ CTR_Code GetCompressorShortedFault(bool &status);
+ /* Get compressor is not connected sticky fault value
+ * @Return - CTR_Code - Error code (if any)
+ * @Param - status - True if compressor current is too low,
+ * indicating compressor is not connected, false if otherwise
+ */
+ CTR_Code GetCompressorNotConnectedStickyFault(bool &status);
+ /* Get compressor is not connected fault value
+ * @Return - CTR_Code - Error code (if any)
+ * @Param - status - True if compressor current is too low,
+ * indicating compressor is not connected, false if otherwise
+ */
+ CTR_Code GetCompressorNotConnectedFault(bool &status);
+
+ /* Get solenoid sticky fault value
+ * @Return - CTR_Code - Error code (if any)
+ * @Param - status - True if compressor had previously been shorted
+ * (and sticky fault was not cleared), false if otherwise
+ */
+ CTR_Code GetSolenoidStickyFault(bool &status);
+
+ /* Get battery voltage
+ * @Return - CTR_Code - Error code (if any)
+ * @Param - status - Voltage across PCM power ports in Volts (V)
+ */
+ CTR_Code GetBatteryVoltage(float &status);
+
+ /* Set PCM Device Number and according CAN frame IDs
+ * @Return - void
+ * @Param - deviceNumber - Device number of PCM to control
+ */
+ void SetDeviceNumber(UINT8 deviceNumber);
+ /* Get number of total failed PCM Control Frame
+ * @Return - CTR_Code - Error code (if any)
+ * @Param - status - Number of failed control frames (tokenization fails)
+ * @WARNING - Return only valid if [SeekDebugFrames] is enabled
+ * See function SeekDebugFrames
+ * See function EnableSeekDebugFrames
+ */
+ CTR_Code GetNumberOfFailedControlFrames(UINT16 &status);
+
+ /* Get raw Solenoid Blacklist
+ * @Return - CTR_Code - Error code (if any)
+ * @Param - status - Raw binary breakdown of Solenoid Blacklist
+ * BIT7 = Solenoid 1, BIT6 = Solenoid 2, etc.
+ * @WARNING - Return only valid if [SeekStatusFaultFrames] is enabled
+ * See function SeekStatusFaultFrames
+ * See function EnableSeekStatusFaultFrames
+ */
+ CTR_Code GetSolenoidBlackList(UINT8 &status);
+
+ /* Get solenoid Blacklist status
+ * - Blacklisted solenoids cannot be enabled until PCM is power cycled
+ * @Return - CTR_Code - Error code (if any)
+ * @Param - idx - ID of solenoid [0,7]
+ * @Param - status - True if Solenoid is blacklisted, false if otherwise
+ * @WARNING - Return only valid if [SeekStatusFaultFrames] is enabled
+ * See function SeekStatusFaultFrames
+ * See function EnableSeekStatusFaultFrames
+ */
+ CTR_Code IsSolenoidBlacklisted(UINT8 idx, bool &status);
+
+ /* Return status of module enable/disable
+ * @Return - CTR_Code - Error code (if any)
+ * @Param - status - Returns TRUE if PCM is enabled, FALSE if disabled
+ */
+ CTR_Code isModuleEnabled(bool &status);
+
+ /* Get solenoid Blacklist status
+ * @Return - CTR_Code - Error code (if any)
+ * @Param - idx - ID of solenoid [0,7] to fire one shot pulse.
+ */
+ CTR_Code FireOneShotSolenoid(UINT8 idx);
+
+ /* Configure the pulse width of a solenoid channel for one-shot pulse.
+ * Preprogrammed pulsewidth is 10ms resolute and can be between 20ms and 5.1s.
+ * @Return - CTR_Code - Error code (if any)
+ * @Param - idx - ID of solenoid [0,7] to configure.
+ * @Param - durMs - pulse width in ms.
+ */
+ CTR_Code SetOneShotDurationMs(UINT8 idx,uint32_t durMs);
+
+};
+//------------------ C interface --------------------------------------------//
+extern "C" {
+ void * c_PCM_Init(void);
+ CTR_Code c_SetSolenoid(void * handle,unsigned char idx,INT8 param);
+ CTR_Code c_SetAllSolenoids(void * handle,UINT8 state);
+ CTR_Code c_SetClosedLoopControl(void * handle,INT8 param);
+ CTR_Code c_ClearStickyFaults(void * handle,INT8 param);
+ CTR_Code c_GetSolenoid(void * handle,UINT8 idx,INT8 * status);
+ CTR_Code c_GetAllSolenoids(void * handle,UINT8 * status);
+ CTR_Code c_GetPressure(void * handle,INT8 * status);
+ CTR_Code c_GetCompressor(void * handle,INT8 * status);
+ CTR_Code c_GetClosedLoopControl(void * handle,INT8 * status);
+ CTR_Code c_GetCompressorCurrent(void * handle,float * status);
+ CTR_Code c_GetSolenoidVoltage(void * handle,float*status);
+ CTR_Code c_GetHardwareFault(void * handle,INT8*status);
+ CTR_Code c_GetCompressorFault(void * handle,INT8*status);
+ CTR_Code c_GetSolenoidFault(void * handle,INT8*status);
+ CTR_Code c_GetCompressorStickyFault(void * handle,INT8*status);
+ CTR_Code c_GetSolenoidStickyFault(void * handle,INT8*status);
+ CTR_Code c_GetBatteryVoltage(void * handle,float*status);
+ void c_SetDeviceNumber_PCM(void * handle,UINT8 deviceNumber);
+ void c_EnableSeekStatusFrames(void * handle,INT8 enable);
+ void c_EnableSeekStatusFaultFrames(void * handle,INT8 enable);
+ void c_EnableSeekDebugFrames(void * handle,INT8 enable);
+ CTR_Code c_GetNumberOfFailedControlFrames(void * handle,UINT16*status);
+ CTR_Code c_GetSolenoidBlackList(void * handle,UINT8 *status);
+ CTR_Code c_IsSolenoidBlacklisted(void * handle,UINT8 idx,INT8*status);
+}
+#endif
diff --git a/hal/src/main/native/athena/ctre/ctre.h b/hal/src/main/native/athena/ctre/ctre.h
new file mode 100644
index 0000000..90d33c1
--- /dev/null
+++ b/hal/src/main/native/athena/ctre/ctre.h
@@ -0,0 +1,55 @@
+/**
+ * @file ctre.h
+ * Common header for all CTRE HAL modules.
+ */
+#ifndef CTRE_H
+#define CTRE_H
+
+//Bit Defines
+#define BIT0 0x01
+#define BIT1 0x02
+#define BIT2 0x04
+#define BIT3 0x08
+#define BIT4 0x10
+#define BIT5 0x20
+#define BIT6 0x40
+#define BIT7 0x80
+#define BIT8 0x0100
+#define BIT9 0x0200
+#define BIT10 0x0400
+#define BIT11 0x0800
+#define BIT12 0x1000
+#define BIT13 0x2000
+#define BIT14 0x4000
+#define BIT15 0x8000
+
+//Signed
+typedef signed char INT8;
+typedef signed short INT16;
+typedef signed int INT32;
+typedef signed long long INT64;
+
+//Unsigned
+typedef unsigned char UINT8;
+typedef unsigned short UINT16;
+typedef unsigned int UINT32;
+typedef unsigned long long UINT64;
+
+//Other
+typedef unsigned char UCHAR;
+typedef unsigned short USHORT;
+typedef unsigned int UINT;
+typedef unsigned long ULONG;
+
+typedef enum {
+ CTR_OKAY, //!< No Error - Function executed as expected
+ CTR_RxTimeout, //!< CAN frame has not been received within specified period of time.
+ CTR_TxTimeout, //!< Not used.
+ CTR_InvalidParamValue, //!< Caller passed an invalid param
+ CTR_UnexpectedArbId, //!< Specified CAN Id is invalid.
+ CTR_TxFailed, //!< Could not transmit the CAN frame.
+ CTR_SigNotUpdated, //!< Have not received an value response for signal.
+ CTR_BufferFull, //!< Caller attempted to insert data into a buffer that is full.
+}CTR_Code;
+
+#endif /* CTRE_H */
diff --git a/hal/src/main/native/athena/frccansae/CANDeviceInterface.h b/hal/src/main/native/athena/frccansae/CANDeviceInterface.h
new file mode 100644
index 0000000..8fe4235
--- /dev/null
+++ b/hal/src/main/native/athena/frccansae/CANDeviceInterface.h
@@ -0,0 +1,156 @@
+#ifndef __CAN_DEVICE_INTERFACE_H__
+#define __CAN_DEVICE_INTERFACE_H__
+
+#define MAX_STRING_LEN 64
+
+#define SUPPORT_UNIQUE_ID (1) /* depends entirely on old vs new build */
+#define USE_NTH_ORDER (0) /* zero to user deviceId */
+#define SUPPORT_MOTOR_CONTROLLER_PROFILE (1)
+namespace CANDeviceInterface1
+{
+
+struct PIDSlot
+{
+ // Proportional gain
+ float pGain;
+ // Integral gain
+ float iGain;
+ // Differential gain
+ float dGain;
+ // Feed-forward gain
+ float fGain;
+ // Integral zone
+ float iZone;
+ // Closed-loop ramp rate
+ float clRampRate;
+};
+
+struct DeviceDescriptor
+{
+ // The full device ID, including the device number, manufacturer, and device type.
+ // The mask of a message the device supports is 0x1FFF003F.
+ unsigned int deviceID;
+#if SUPPORT_UNIQUE_ID != 0
+ // This is the ID that uniquely identifies the device node in the UI.
+ // The purpose of this is to be able to track the device across renames or deviceID changes.
+ unsigned int uniqueID;
+#endif
+ // An dynamically assigned ID that will make setting deviceIDs robust,
+ // Never again will you need to isolate a CAN node just to fix it's ID.
+ unsigned int dynamicID;
+ // User visible name. This can be customized by the user, but should have a
+ // reasonable default.
+ char name[MAX_STRING_LEN];
+ // This is a user visible model name that should match the can_devices.ini section.
+ char model[MAX_STRING_LEN];
+ // This is a version number that represents the version of firmware currently
+ // installed on the device.
+ char currentVersion[MAX_STRING_LEN];
+ // Hardware revision.
+ char hardwareRev[MAX_STRING_LEN];
+ // Bootloader version. Will not change for the life of the product, but additional
+ // field upgrade features could be added in newer hardware.
+ char bootloaderRev[MAX_STRING_LEN];
+ // Manufacture Date. Could be a calender date or just the FRC season year.
+ // Also helps troubleshooting "old ones" vs "new ones".
+ char manufactureDate[MAX_STRING_LEN];
+ // General status of the hardware. For example if the device is in bootloader
+ // due to a bad flash UI could emphasize that.
+ char softwareStatus[MAX_STRING_LEN];
+ // Is the LED currently on?
+ bool led;
+ // Reserved fields for future use by CTRE. Not touched by frccansae
+ unsigned int dynFlags;
+ unsigned int flags; /* bitfield */
+ unsigned int ptrToString;
+ //unsigned int reserved0;
+ //unsigned int reserved1;
+ //unsigned int reserved2;
+#if SUPPORT_MOTOR_CONTROLLER_PROFILE != 0
+ // Motor controller properties (ignored if SupportsMotorControllerProperties is false or unset for this model)
+ unsigned int brakeMode; // 0=Coast, 1=Brake
+ unsigned int limitSwitchFwdMode; // 0=disabled, 1=Normally Closed, 2=Normally Open
+ unsigned int limitSwitchRevMode; // 0=disabled, 1=Normally Closed, 2=Normally Open
+ // Limit-switch soft limits
+ bool bFwdSoftLimitEnable;
+ bool bRevSoftLimitEnable;
+ float softLimitFwd;
+ float softLimitRev;
+ // PID constants for slot 0
+ struct PIDSlot slot0;
+ // PID constants for slot 1
+ struct PIDSlot slot1;
+#endif
+};
+
+#define kLimitSwitchMode_Disabled (0)
+#define kLimitSwitchMode_NormallyClosed (1)
+#define kLimitSwitchMode_NormallyOpen (2)
+
+// Interface functions that must be implemented by the CAN Firmware Update Library
+
+// Returns the number of devices that will be returned in a call to
+// getListOfDevices(). The calling library will use this info to allocate enough
+// memory to accept all device info.
+int getNumberOfDevices();
+
+// Return info about discovered devices. The array of structs should be
+// populated before returning. The numDescriptors input describes how many
+// elements were allocated to prevent memory corruption. The number of devices
+// populated should be returned from this function as well.
+int getListOfDevices(DeviceDescriptor *devices, int numDescriptors);
+
+// When the user requests to update the firmware of a device a thread will be
+// spawned and this function is called from that thread. This function should
+// complete the firmware update process before returning. The image
+// contents and size are directly from the file selected by the user. The
+// error message string can be filled with a NULL-terminated message to show the
+// user if there was a problem updating firmware. The error message is only
+// displayed if a non-zero value is returned from this function.
+int updateFirmware(const DeviceDescriptor *device, const unsigned char *imageContents, unsigned int imageSize, char *errorMessage, int errorMessageMaxSize);
+
+// This function is called periodically from the UI thread while the firmware
+// update is in progress. The percentComplete parameter should the filled in
+// with the current progress of the firmware update process to update a progress
+// bar in the UI.
+void checkUpdateProgress(const DeviceDescriptor *device, int *percentComplete);
+
+// This is called when the user selects a new ID to assign on the bus and
+// chooses to save. The newDeviceID is really just the device number. The
+// manufacturer and device type will remain unchanged. If a problem is detected
+// when assigning a new ID, this function should return a non-zero value.
+int assignBroadcastDeviceID(unsigned int newDeviceID);
+// The device descriptor should be updated with the new device ID. The name may
+// also change in the descriptor and will be updated in the UI immediately.
+// Be sure to modify the descriptor first since the refresh from the UI is
+// asynchronous.
+int assignDeviceID(DeviceDescriptor *device, unsigned int newDeviceID);
+
+// This entry-point will get called when the user chooses to change the value
+// of the device's LED. This will allow the user to identify devices which
+// support dynamic addresses or are otherwise unknown. If this function returns
+// a non-zero value, the UI will report an error.
+int saveLightLed(const DeviceDescriptor *device, bool newLEDStatus);
+
+// This entry-point will get called when the user chooses to change the alias
+// of the device with the device specified. If this function returns a non-
+// zero value, the UI will report an error. The device descriptor must be
+// updated with the new name that was selected. If a different name is saved
+// to the descriptor than the user specified, this will require a manual
+// refresh by the user. This is reported as CAR #505139
+int saveDeviceName(DeviceDescriptor *device, const char *newName);
+
+// This entry-point will get called when the user changes any of the motor
+// controller specific properties. If this function returns a non-zero value,
+// the UI will report an error. The device descriptor may be updated with
+// coerced values.
+int saveMotorParameters(DeviceDescriptor *device);
+
+// Run some sort of self-test functionality on the device. This can be anything
+// and the results will be displayed to the user. A non-zero return value
+// indicates an error.
+int selfTest(const DeviceDescriptor *device, char *detailedResults, int detailedResultsMaxSize);
+
+} /* CANDeviceInterface */
+
+#endif /* __CAN_DEVICE_INTERFACE_H__ */
diff --git a/hal/src/main/native/cpp/Main.cpp b/hal/src/main/native/cpp/Main.cpp
new file mode 100644
index 0000000..a37c2b0
--- /dev/null
+++ b/hal/src/main/native/cpp/Main.cpp
@@ -0,0 +1,64 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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 "hal/Main.h"
+
+#include <wpi/condition_variable.h>
+#include <wpi/mutex.h>
+
+static void DefaultMain(void*);
+static void DefaultExit(void*);
+
+static bool gHasMain = false;
+static void* gMainParam = nullptr;
+static void (*gMainFunc)(void*) = DefaultMain;
+static void (*gExitFunc)(void*) = DefaultExit;
+static bool gExited = false;
+struct MainObj {
+ wpi::mutex gExitMutex;
+ wpi::condition_variable gExitCv;
+};
+
+static MainObj* mainObj;
+
+static void DefaultMain(void*) {
+ std::unique_lock lock{mainObj->gExitMutex};
+ mainObj->gExitCv.wait(lock, [] { return gExited; });
+}
+
+static void DefaultExit(void*) {
+ std::lock_guard lock{mainObj->gExitMutex};
+ gExited = true;
+ mainObj->gExitCv.notify_all();
+}
+
+namespace hal {
+namespace init {
+void InitializeMain() {
+ static MainObj mO;
+ mainObj = &mO;
+}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+
+void HAL_SetMain(void* param, void (*mainFunc)(void*),
+ void (*exitFunc)(void*)) {
+ gHasMain = true;
+ gMainParam = param;
+ gMainFunc = mainFunc;
+ gExitFunc = exitFunc;
+}
+
+HAL_Bool HAL_HasMain(void) { return gHasMain; }
+
+void HAL_RunMain(void) { gMainFunc(gMainParam); }
+
+void HAL_ExitMain(void) { gExitFunc(gMainParam); }
+
+} // extern "C"
diff --git a/hal/src/main/native/cpp/cpp/fpga_clock.cpp b/hal/src/main/native/cpp/cpp/fpga_clock.cpp
new file mode 100644
index 0000000..bcec155
--- /dev/null
+++ b/hal/src/main/native/cpp/cpp/fpga_clock.cpp
@@ -0,0 +1,32 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "hal/cpp/fpga_clock.h"
+
+#include <wpi/raw_ostream.h>
+
+#include "hal/HAL.h"
+
+namespace hal {
+const fpga_clock::time_point fpga_clock::min_time =
+ fpga_clock::time_point(fpga_clock::duration(
+ std::numeric_limits<fpga_clock::duration::rep>::min()));
+
+fpga_clock::time_point fpga_clock::now() noexcept {
+ int32_t status = 0;
+ uint64_t currentTime = HAL_GetFPGATime(&status);
+ if (status != 0) {
+ wpi::errs()
+ << "Call to HAL_GetFPGATime failed in fpga_clock::now() with status "
+ << status
+ << ". Initialization might have failed. Time will not be correct\n";
+ wpi::errs().flush();
+ return epoch();
+ }
+ return time_point(std::chrono::microseconds(currentTime));
+}
+} // namespace hal
diff --git a/hal/src/main/native/cpp/handles/HandlesInternal.cpp b/hal/src/main/native/cpp/handles/HandlesInternal.cpp
new file mode 100644
index 0000000..5e66ce1
--- /dev/null
+++ b/hal/src/main/native/cpp/handles/HandlesInternal.cpp
@@ -0,0 +1,95 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 "hal/handles/HandlesInternal.h"
+
+#include <algorithm>
+
+#include <wpi/SmallVector.h>
+#include <wpi/mutex.h>
+
+namespace hal {
+static wpi::SmallVector<HandleBase*, 32>* globalHandles = nullptr;
+static wpi::mutex globalHandleMutex;
+HandleBase::HandleBase() {
+ static wpi::SmallVector<HandleBase*, 32> gH;
+ std::scoped_lock lock(globalHandleMutex);
+ if (!globalHandles) {
+ globalHandles = &gH;
+ }
+
+ auto index = std::find(globalHandles->begin(), globalHandles->end(), this);
+ if (index == globalHandles->end()) {
+ globalHandles->push_back(this);
+ } else {
+ *index = this;
+ }
+}
+HandleBase::~HandleBase() {
+ std::scoped_lock lock(globalHandleMutex);
+ auto index = std::find(globalHandles->begin(), globalHandles->end(), this);
+ if (index != globalHandles->end()) {
+ *index = nullptr;
+ }
+}
+void HandleBase::ResetHandles() {
+ m_version++;
+ if (m_version > 255) {
+ m_version = 0;
+ }
+}
+void HandleBase::ResetGlobalHandles() {
+ std::unique_lock lock(globalHandleMutex);
+ for (auto&& i : *globalHandles) {
+ if (i != nullptr) {
+ lock.unlock();
+ i->ResetHandles();
+ lock.lock();
+ }
+ }
+}
+HAL_PortHandle createPortHandle(uint8_t channel, uint8_t module) {
+ // set last 8 bits, then shift to first 8 bits
+ HAL_PortHandle handle = static_cast<HAL_PortHandle>(HAL_HandleEnum::Port);
+ handle = handle << 24;
+ // shift module and add to 3rd set of 8 bits
+ int32_t temp = module;
+ temp = (temp << 8) & 0xff00;
+ handle += temp;
+ // add channel to last 8 bits
+ handle += channel;
+ return handle;
+}
+HAL_PortHandle createPortHandleForSPI(uint8_t channel) {
+ // set last 8 bits, then shift to first 8 bits
+ HAL_PortHandle handle = static_cast<HAL_PortHandle>(HAL_HandleEnum::Port);
+ handle = handle << 16;
+ // set second set up bits to 1
+ int32_t temp = 1;
+ temp = (temp << 8) & 0xff00;
+ handle += temp;
+ // shift to last set of bits
+ handle = handle << 8;
+ // add channel to last 8 bits
+ handle += channel;
+ return handle;
+}
+HAL_Handle createHandle(int16_t index, HAL_HandleEnum handleType,
+ int16_t version) {
+ if (index < 0) return HAL_kInvalidHandle;
+ uint8_t hType = static_cast<uint8_t>(handleType);
+ if (hType == 0 || hType > 127) return HAL_kInvalidHandle;
+ // set last 8 bits, then shift to first 8 bits
+ HAL_Handle handle = hType;
+ handle = handle << 8;
+ handle += static_cast<uint8_t>(version);
+ handle = handle << 16;
+ // add index to set last 16 bits
+ handle += index;
+ return handle;
+}
+} // namespace hal
diff --git a/hal/src/main/native/cpp/jni/AccelerometerJNI.cpp b/hal/src/main/native/cpp/jni/AccelerometerJNI.cpp
new file mode 100644
index 0000000..6d0d256
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/AccelerometerJNI.cpp
@@ -0,0 +1,75 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 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 <jni.h>
+
+#include "edu_wpi_first_hal_AccelerometerJNI.h"
+#include "hal/Accelerometer.h"
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_hal_AccelerometerJNI
+ * Method: setAccelerometerActive
+ * Signature: (Z)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_AccelerometerJNI_setAccelerometerActive
+ (JNIEnv*, jclass, jboolean active)
+{
+ HAL_SetAccelerometerActive(active);
+}
+
+/*
+ * Class: edu_wpi_first_hal_AccelerometerJNI
+ * Method: setAccelerometerRange
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_AccelerometerJNI_setAccelerometerRange
+ (JNIEnv*, jclass, jint range)
+{
+ HAL_SetAccelerometerRange((HAL_AccelerometerRange)range);
+}
+
+/*
+ * Class: edu_wpi_first_hal_AccelerometerJNI
+ * Method: getAccelerometerX
+ * Signature: ()D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_AccelerometerJNI_getAccelerometerX
+ (JNIEnv*, jclass)
+{
+ return HAL_GetAccelerometerX();
+}
+
+/*
+ * Class: edu_wpi_first_hal_AccelerometerJNI
+ * Method: getAccelerometerY
+ * Signature: ()D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_AccelerometerJNI_getAccelerometerY
+ (JNIEnv*, jclass)
+{
+ return HAL_GetAccelerometerY();
+}
+
+/*
+ * Class: edu_wpi_first_hal_AccelerometerJNI
+ * Method: getAccelerometerZ
+ * Signature: ()D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_AccelerometerJNI_getAccelerometerZ
+ (JNIEnv*, jclass)
+{
+ return HAL_GetAccelerometerZ();
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/cpp/jni/AddressableLEDJNI.cpp b/hal/src/main/native/cpp/jni/AddressableLEDJNI.cpp
new file mode 100644
index 0000000..3f76e6a
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/AddressableLEDJNI.cpp
@@ -0,0 +1,145 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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 <jni.h>
+
+#include <wpi/jni_util.h>
+
+#include "HALUtil.h"
+#include "edu_wpi_first_hal_AddressableLEDJNI.h"
+#include "hal/AddressableLED.h"
+
+using namespace frc;
+using namespace wpi::java;
+
+static_assert(sizeof(jbyte) * 4 == sizeof(HAL_AddressableLEDData));
+
+extern "C" {
+/*
+ * Class: edu_wpi_first_hal_AddressableLEDJNI
+ * Method: initialize
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_AddressableLEDJNI_initialize
+ (JNIEnv* env, jclass, jint handle)
+{
+ int32_t status = 0;
+ auto ret = HAL_InitializeAddressableLED(
+ static_cast<HAL_DigitalHandle>(handle), &status);
+ CheckStatus(env, status);
+ return ret;
+}
+
+/*
+ * Class: edu_wpi_first_hal_AddressableLEDJNI
+ * Method: free
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_AddressableLEDJNI_free
+ (JNIEnv* env, jclass, jint handle)
+{
+ HAL_FreeAddressableLED(static_cast<HAL_AddressableLEDHandle>(handle));
+}
+
+/*
+ * Class: edu_wpi_first_hal_AddressableLEDJNI
+ * Method: setLength
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_AddressableLEDJNI_setLength
+ (JNIEnv* env, jclass, jint handle, jint length)
+{
+ int32_t status = 0;
+ HAL_SetAddressableLEDLength(static_cast<HAL_AddressableLEDHandle>(handle),
+ length, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_AddressableLEDJNI
+ * Method: setData
+ * Signature: (I[B)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_AddressableLEDJNI_setData
+ (JNIEnv* env, jclass, jint handle, jbyteArray arr)
+{
+ int32_t status = 0;
+ JByteArrayRef jArrRef{env, arr};
+ auto arrRef = jArrRef.array();
+ HAL_WriteAddressableLEDData(
+ static_cast<HAL_AddressableLEDHandle>(handle),
+ reinterpret_cast<const HAL_AddressableLEDData*>(arrRef.data()),
+ arrRef.size() / 4, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_AddressableLEDJNI
+ * Method: setBitTiming
+ * Signature: (IIIII)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_AddressableLEDJNI_setBitTiming
+ (JNIEnv* env, jclass, jint handle, jint lowTime0, jint highTime0,
+ jint lowTime1, jint highTime1)
+{
+ int32_t status = 0;
+ HAL_SetAddressableLEDBitTiming(static_cast<HAL_AddressableLEDHandle>(handle),
+ lowTime0, highTime0, lowTime1, highTime1,
+ &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_AddressableLEDJNI
+ * Method: setSyncTime
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_AddressableLEDJNI_setSyncTime
+ (JNIEnv* env, jclass, jint handle, jint syncTime)
+{
+ int32_t status = 0;
+ HAL_SetAddressableLEDSyncTime(static_cast<HAL_AddressableLEDHandle>(handle),
+ syncTime, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_AddressableLEDJNI
+ * Method: start
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_AddressableLEDJNI_start
+ (JNIEnv* env, jclass, jint handle)
+{
+ int32_t status = 0;
+ HAL_StartAddressableLEDOutput(static_cast<HAL_AddressableLEDHandle>(handle),
+ &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_AddressableLEDJNI
+ * Method: stop
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_AddressableLEDJNI_stop
+ (JNIEnv* env, jclass, jint handle)
+{
+ int32_t status = 0;
+ HAL_StopAddressableLEDOutput(static_cast<HAL_AddressableLEDHandle>(handle),
+ &status);
+ CheckStatus(env, status);
+}
+} // extern "C"
diff --git a/hal/src/main/native/cpp/jni/AnalogGyroJNI.cpp b/hal/src/main/native/cpp/jni/AnalogGyroJNI.cpp
new file mode 100644
index 0000000..35b7414
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/AnalogGyroJNI.cpp
@@ -0,0 +1,194 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 <jni.h>
+
+#include <cassert>
+
+#include "HALUtil.h"
+#include "edu_wpi_first_hal_AnalogGyroJNI.h"
+#include "hal/AnalogGyro.h"
+#include "hal/handles/HandlesInternal.h"
+
+using namespace frc;
+
+extern "C" {
+/*
+ * Class: edu_wpi_first_hal_AnalogGyroJNI
+ * Method: initializeAnalogGyro
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_AnalogGyroJNI_initializeAnalogGyro
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ HAL_GyroHandle handle =
+ HAL_InitializeAnalogGyro((HAL_AnalogInputHandle)id, &status);
+ // Analog input does range checking, so we don't need to do so.
+ CheckStatusForceThrow(env, status);
+ return (jint)handle;
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogGyroJNI
+ * Method: setupAnalogGyro
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_AnalogGyroJNI_setupAnalogGyro
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ HAL_SetupAnalogGyro((HAL_GyroHandle)id, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogGyroJNI
+ * Method: freeAnalogGyro
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_AnalogGyroJNI_freeAnalogGyro
+ (JNIEnv* env, jclass, jint id)
+{
+ HAL_FreeAnalogGyro((HAL_GyroHandle)id);
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogGyroJNI
+ * Method: setAnalogGyroParameters
+ * Signature: (IDDI)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_AnalogGyroJNI_setAnalogGyroParameters
+ (JNIEnv* env, jclass, jint id, jdouble vPDPS, jdouble offset, jint center)
+{
+ int32_t status = 0;
+ HAL_SetAnalogGyroParameters((HAL_GyroHandle)id, vPDPS, offset, center,
+ &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogGyroJNI
+ * Method: setAnalogGyroVoltsPerDegreePerSecond
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_AnalogGyroJNI_setAnalogGyroVoltsPerDegreePerSecond
+ (JNIEnv* env, jclass, jint id, jdouble vPDPS)
+{
+ int32_t status = 0;
+ HAL_SetAnalogGyroVoltsPerDegreePerSecond((HAL_GyroHandle)id, vPDPS, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogGyroJNI
+ * Method: resetAnalogGyro
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_AnalogGyroJNI_resetAnalogGyro
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ HAL_ResetAnalogGyro((HAL_GyroHandle)id, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogGyroJNI
+ * Method: calibrateAnalogGyro
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_AnalogGyroJNI_calibrateAnalogGyro
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ HAL_CalibrateAnalogGyro((HAL_GyroHandle)id, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogGyroJNI
+ * Method: setAnalogGyroDeadband
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_AnalogGyroJNI_setAnalogGyroDeadband
+ (JNIEnv* env, jclass, jint id, jdouble deadband)
+{
+ int32_t status = 0;
+ HAL_SetAnalogGyroDeadband((HAL_GyroHandle)id, deadband, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogGyroJNI
+ * Method: getAnalogGyroAngle
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_AnalogGyroJNI_getAnalogGyroAngle
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ jdouble value = HAL_GetAnalogGyroAngle((HAL_GyroHandle)id, &status);
+ CheckStatus(env, status);
+ return value;
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogGyroJNI
+ * Method: getAnalogGyroRate
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_AnalogGyroJNI_getAnalogGyroRate
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ jdouble value = HAL_GetAnalogGyroRate((HAL_GyroHandle)id, &status);
+ CheckStatus(env, status);
+ return value;
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogGyroJNI
+ * Method: getAnalogGyroOffset
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_AnalogGyroJNI_getAnalogGyroOffset
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ jdouble value = HAL_GetAnalogGyroOffset((HAL_GyroHandle)id, &status);
+ CheckStatus(env, status);
+ return value;
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogGyroJNI
+ * Method: getAnalogGyroCenter
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_AnalogGyroJNI_getAnalogGyroCenter
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ jint value = HAL_GetAnalogGyroCenter((HAL_GyroHandle)id, &status);
+ CheckStatus(env, status);
+ return value;
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/cpp/jni/AnalogJNI.cpp b/hal/src/main/native/cpp/jni/AnalogJNI.cpp
new file mode 100644
index 0000000..5571d55
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/AnalogJNI.cpp
@@ -0,0 +1,669 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 <jni.h>
+
+#include <cassert>
+
+#include "HALUtil.h"
+#include "edu_wpi_first_hal_AnalogJNI.h"
+#include "hal/AnalogAccumulator.h"
+#include "hal/AnalogInput.h"
+#include "hal/AnalogOutput.h"
+#include "hal/AnalogTrigger.h"
+#include "hal/Ports.h"
+#include "hal/handles/HandlesInternal.h"
+
+using namespace frc;
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_hal_AnalogJNI
+ * Method: initializeAnalogInputPort
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_initializeAnalogInputPort
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ auto analog = HAL_InitializeAnalogInputPort((HAL_PortHandle)id, &status);
+ CheckStatusRange(env, status, 0, HAL_GetNumAnalogInputs(),
+ hal::getPortHandleChannel((HAL_PortHandle)id));
+ return (jint)analog;
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogJNI
+ * Method: freeAnalogInputPort
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_freeAnalogInputPort
+ (JNIEnv* env, jclass, jint id)
+{
+ HAL_FreeAnalogInputPort((HAL_AnalogInputHandle)id);
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogJNI
+ * Method: initializeAnalogOutputPort
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_initializeAnalogOutputPort
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ HAL_AnalogOutputHandle analog =
+ HAL_InitializeAnalogOutputPort((HAL_PortHandle)id, &status);
+ CheckStatusRange(env, status, 0, HAL_GetNumAnalogOutputs(),
+ hal::getPortHandleChannel((HAL_PortHandle)id));
+ return (jlong)analog;
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogJNI
+ * Method: freeAnalogOutputPort
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_freeAnalogOutputPort
+ (JNIEnv* env, jclass, jint id)
+{
+ HAL_FreeAnalogOutputPort((HAL_AnalogOutputHandle)id);
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogJNI
+ * Method: checkAnalogModule
+ * Signature: (B)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_checkAnalogModule
+ (JNIEnv*, jclass, jbyte value)
+{
+ jboolean returnValue = HAL_CheckAnalogModule(value);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogJNI
+ * Method: checkAnalogInputChannel
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_checkAnalogInputChannel
+ (JNIEnv*, jclass, jint value)
+{
+ jboolean returnValue = HAL_CheckAnalogInputChannel(value);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogJNI
+ * Method: checkAnalogOutputChannel
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_checkAnalogOutputChannel
+ (JNIEnv*, jclass, jint value)
+{
+ jboolean returnValue = HAL_CheckAnalogOutputChannel(value);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogJNI
+ * Method: setAnalogInputSimDevice
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_setAnalogInputSimDevice
+ (JNIEnv* env, jclass, jint handle, jint device)
+{
+ HAL_SetAnalogInputSimDevice((HAL_AnalogInputHandle)handle,
+ (HAL_SimDeviceHandle)device);
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogJNI
+ * Method: setAnalogOutput
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_setAnalogOutput
+ (JNIEnv* env, jclass, jint id, jdouble voltage)
+{
+ int32_t status = 0;
+ HAL_SetAnalogOutput((HAL_AnalogOutputHandle)id, voltage, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogJNI
+ * Method: getAnalogOutput
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_getAnalogOutput
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ double val = HAL_GetAnalogOutput((HAL_AnalogOutputHandle)id, &status);
+ CheckStatus(env, status);
+ return val;
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogJNI
+ * Method: setAnalogSampleRate
+ * Signature: (D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_setAnalogSampleRate
+ (JNIEnv* env, jclass, jdouble value)
+{
+ int32_t status = 0;
+ HAL_SetAnalogSampleRate(value, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogJNI
+ * Method: getAnalogSampleRate
+ * Signature: ()D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_getAnalogSampleRate
+ (JNIEnv* env, jclass)
+{
+ int32_t status = 0;
+ double returnValue = HAL_GetAnalogSampleRate(&status);
+ CheckStatus(env, status);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogJNI
+ * Method: setAnalogAverageBits
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_setAnalogAverageBits
+ (JNIEnv* env, jclass, jint id, jint value)
+{
+ int32_t status = 0;
+ HAL_SetAnalogAverageBits((HAL_AnalogInputHandle)id, value, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogJNI
+ * Method: getAnalogAverageBits
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_getAnalogAverageBits
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ jint returnValue =
+ HAL_GetAnalogAverageBits((HAL_AnalogInputHandle)id, &status);
+ CheckStatus(env, status);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogJNI
+ * Method: setAnalogOversampleBits
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_setAnalogOversampleBits
+ (JNIEnv* env, jclass, jint id, jint value)
+{
+ int32_t status = 0;
+ HAL_SetAnalogOversampleBits((HAL_AnalogInputHandle)id, value, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogJNI
+ * Method: getAnalogOversampleBits
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_getAnalogOversampleBits
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ jint returnValue =
+ HAL_GetAnalogOversampleBits((HAL_AnalogInputHandle)id, &status);
+ CheckStatus(env, status);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogJNI
+ * Method: getAnalogValue
+ * Signature: (I)S
+ */
+JNIEXPORT jshort JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_getAnalogValue
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ jshort returnValue = HAL_GetAnalogValue((HAL_AnalogInputHandle)id, &status);
+ CheckStatus(env, status);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogJNI
+ * Method: getAnalogAverageValue
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_getAnalogAverageValue
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ jint returnValue =
+ HAL_GetAnalogAverageValue((HAL_AnalogInputHandle)id, &status);
+ CheckStatus(env, status);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogJNI
+ * Method: getAnalogVoltsToValue
+ * Signature: (ID)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_getAnalogVoltsToValue
+ (JNIEnv* env, jclass, jint id, jdouble voltageValue)
+{
+ int32_t status = 0;
+ jint returnValue = HAL_GetAnalogVoltsToValue((HAL_AnalogInputHandle)id,
+ voltageValue, &status);
+ CheckStatus(env, status);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogJNI
+ * Method: getAnalogVoltage
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_getAnalogVoltage
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ jdouble returnValue =
+ HAL_GetAnalogVoltage((HAL_AnalogInputHandle)id, &status);
+ CheckStatus(env, status);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogJNI
+ * Method: getAnalogAverageVoltage
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_getAnalogAverageVoltage
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ jdouble returnValue =
+ HAL_GetAnalogAverageVoltage((HAL_AnalogInputHandle)id, &status);
+ CheckStatus(env, status);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogJNI
+ * Method: getAnalogLSBWeight
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_getAnalogLSBWeight
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+
+ jint returnValue = HAL_GetAnalogLSBWeight((HAL_AnalogInputHandle)id, &status);
+ CheckStatus(env, status);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogJNI
+ * Method: getAnalogOffset
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_getAnalogOffset
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+
+ jint returnValue = HAL_GetAnalogOffset((HAL_AnalogInputHandle)id, &status);
+ CheckStatus(env, status);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogJNI
+ * Method: isAccumulatorChannel
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_isAccumulatorChannel
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+
+ jboolean returnValue =
+ HAL_IsAccumulatorChannel((HAL_AnalogInputHandle)id, &status);
+ CheckStatus(env, status);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogJNI
+ * Method: initAccumulator
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_initAccumulator
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ HAL_InitAccumulator((HAL_AnalogInputHandle)id, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogJNI
+ * Method: resetAccumulator
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_resetAccumulator
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ HAL_ResetAccumulator((HAL_AnalogInputHandle)id, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogJNI
+ * Method: setAccumulatorCenter
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_setAccumulatorCenter
+ (JNIEnv* env, jclass, jint id, jint center)
+{
+ int32_t status = 0;
+ HAL_SetAccumulatorCenter((HAL_AnalogInputHandle)id, center, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogJNI
+ * Method: setAccumulatorDeadband
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_setAccumulatorDeadband
+ (JNIEnv* env, jclass, jint id, jint deadband)
+{
+ int32_t status = 0;
+ HAL_SetAccumulatorDeadband((HAL_AnalogInputHandle)id, deadband, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogJNI
+ * Method: getAccumulatorValue
+ * Signature: (I)J
+ */
+JNIEXPORT jlong JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_getAccumulatorValue
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ jlong returnValue =
+ HAL_GetAccumulatorValue((HAL_AnalogInputHandle)id, &status);
+ CheckStatus(env, status);
+
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogJNI
+ * Method: getAccumulatorCount
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_getAccumulatorCount
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ jint returnValue =
+ HAL_GetAccumulatorCount((HAL_AnalogInputHandle)id, &status);
+ CheckStatus(env, status);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogJNI
+ * Method: getAccumulatorOutput
+ * Signature: (ILjava/lang/Object;)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_getAccumulatorOutput
+ (JNIEnv* env, jclass, jint id, jobject accumulatorResult)
+{
+ int32_t status = 0;
+ int64_t value = 0;
+ int64_t count = 0;
+ HAL_GetAccumulatorOutput((HAL_AnalogInputHandle)id, &value, &count, &status);
+ SetAccumulatorResultObject(env, accumulatorResult, value, count);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogJNI
+ * Method: initializeAnalogTrigger
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_initializeAnalogTrigger
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ HAL_AnalogTriggerHandle analogTrigger =
+ HAL_InitializeAnalogTrigger((HAL_AnalogInputHandle)id, &status);
+ CheckStatus(env, status);
+ return (jint)analogTrigger;
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogJNI
+ * Method: initializeAnalogTriggerDutyCycle
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_initializeAnalogTriggerDutyCycle
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ HAL_AnalogTriggerHandle analogTrigger =
+ HAL_InitializeAnalogTriggerDutyCycle((HAL_DutyCycleHandle)id, &status);
+ CheckStatus(env, status);
+ return (jint)analogTrigger;
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogJNI
+ * Method: cleanAnalogTrigger
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_cleanAnalogTrigger
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ HAL_CleanAnalogTrigger((HAL_AnalogTriggerHandle)id, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogJNI
+ * Method: setAnalogTriggerLimitsRaw
+ * Signature: (III)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_setAnalogTriggerLimitsRaw
+ (JNIEnv* env, jclass, jint id, jint lower, jint upper)
+{
+ int32_t status = 0;
+ HAL_SetAnalogTriggerLimitsRaw((HAL_AnalogTriggerHandle)id, lower, upper,
+ &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogJNI
+ * Method: setAnalogTriggerLimitsDutyCycle
+ * Signature: (IDD)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_setAnalogTriggerLimitsDutyCycle
+ (JNIEnv* env, jclass, jint id, jdouble lower, jdouble upper)
+{
+ int32_t status = 0;
+ HAL_SetAnalogTriggerLimitsDutyCycle((HAL_AnalogTriggerHandle)id, lower, upper,
+ &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogJNI
+ * Method: setAnalogTriggerLimitsVoltage
+ * Signature: (IDD)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_setAnalogTriggerLimitsVoltage
+ (JNIEnv* env, jclass, jint id, jdouble lower, jdouble upper)
+{
+ int32_t status = 0;
+ HAL_SetAnalogTriggerLimitsVoltage((HAL_AnalogTriggerHandle)id, lower, upper,
+ &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogJNI
+ * Method: setAnalogTriggerAveraged
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_setAnalogTriggerAveraged
+ (JNIEnv* env, jclass, jint id, jboolean averaged)
+{
+ int32_t status = 0;
+ HAL_SetAnalogTriggerAveraged((HAL_AnalogTriggerHandle)id, averaged, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogJNI
+ * Method: setAnalogTriggerFiltered
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_setAnalogTriggerFiltered
+ (JNIEnv* env, jclass, jint id, jboolean filtered)
+{
+ int32_t status = 0;
+ HAL_SetAnalogTriggerFiltered((HAL_AnalogTriggerHandle)id, filtered, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogJNI
+ * Method: getAnalogTriggerInWindow
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_getAnalogTriggerInWindow
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ jboolean val =
+ HAL_GetAnalogTriggerInWindow((HAL_AnalogTriggerHandle)id, &status);
+ CheckStatus(env, status);
+ return val;
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogJNI
+ * Method: getAnalogTriggerTriggerState
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_getAnalogTriggerTriggerState
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ jboolean val =
+ HAL_GetAnalogTriggerTriggerState((HAL_AnalogTriggerHandle)id, &status);
+ CheckStatus(env, status);
+ return val;
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogJNI
+ * Method: getAnalogTriggerOutput
+ * Signature: (II)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_getAnalogTriggerOutput
+ (JNIEnv* env, jclass, jint id, jint type)
+{
+ int32_t status = 0;
+ jboolean val = HAL_GetAnalogTriggerOutput(
+ (HAL_AnalogTriggerHandle)id, (HAL_AnalogTriggerType)type, &status);
+ CheckStatus(env, status);
+ return val;
+}
+
+/*
+ * Class: edu_wpi_first_hal_AnalogJNI
+ * Method: getAnalogTriggerFPGAIndex
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_getAnalogTriggerFPGAIndex
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ auto val =
+ HAL_GetAnalogTriggerFPGAIndex((HAL_AnalogTriggerHandle)id, &status);
+ CheckStatus(env, status);
+ return val;
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/cpp/jni/CANAPIJNI.cpp b/hal/src/main/native/cpp/jni/CANAPIJNI.cpp
new file mode 100644
index 0000000..7ac7cf6
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/CANAPIJNI.cpp
@@ -0,0 +1,221 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 <jni.h>
+
+#include <cassert>
+
+#include <wpi/SmallString.h>
+#include <wpi/jni_util.h>
+#include <wpi/raw_ostream.h>
+
+#include "HALUtil.h"
+#include "edu_wpi_first_hal_CANAPIJNI.h"
+#include "hal/CAN.h"
+#include "hal/CANAPI.h"
+#include "hal/Errors.h"
+
+using namespace frc;
+using namespace wpi::java;
+
+extern "C" {
+/*
+ * Class: edu_wpi_first_hal_CANAPIJNI
+ * Method: initializeCAN
+ * Signature: (III)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_CANAPIJNI_initializeCAN
+ (JNIEnv* env, jclass, jint manufacturer, jint deviceId, jint deviceType)
+{
+ int32_t status = 0;
+ auto handle =
+ HAL_InitializeCAN(static_cast<HAL_CANManufacturer>(manufacturer),
+ static_cast<int32_t>(deviceId),
+ static_cast<HAL_CANDeviceType>(deviceType), &status);
+
+ CheckStatusForceThrow(env, status);
+ return handle;
+}
+
+/*
+ * Class: edu_wpi_first_hal_CANAPIJNI
+ * Method: cleanCAN
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_CANAPIJNI_cleanCAN
+ (JNIEnv* env, jclass, jint handle)
+{
+ HAL_CleanCAN(static_cast<HAL_CANHandle>(handle));
+}
+
+/*
+ * Class: edu_wpi_first_hal_CANAPIJNI
+ * Method: writeCANPacket
+ * Signature: (I[BI)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_CANAPIJNI_writeCANPacket
+ (JNIEnv* env, jclass, jint handle, jbyteArray data, jint apiId)
+{
+ auto halHandle = static_cast<HAL_CANHandle>(handle);
+ JByteArrayRef arr{env, data};
+ auto arrRef = arr.array();
+ int32_t status = 0;
+ HAL_WriteCANPacket(halHandle, reinterpret_cast<const uint8_t*>(arrRef.data()),
+ arrRef.size(), apiId, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_CANAPIJNI
+ * Method: writeCANPacketRepeating
+ * Signature: (I[BII)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_CANAPIJNI_writeCANPacketRepeating
+ (JNIEnv* env, jclass, jint handle, jbyteArray data, jint apiId,
+ jint timeoutMs)
+{
+ auto halHandle = static_cast<HAL_CANHandle>(handle);
+ JByteArrayRef arr{env, data};
+ auto arrRef = arr.array();
+ int32_t status = 0;
+ HAL_WriteCANPacketRepeating(halHandle,
+ reinterpret_cast<const uint8_t*>(arrRef.data()),
+ arrRef.size(), apiId, timeoutMs, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_CANAPIJNI
+ * Method: writeCANRTRFrame
+ * Signature: (III)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_CANAPIJNI_writeCANRTRFrame
+ (JNIEnv* env, jclass, jint handle, jint length, jint apiId)
+{
+ auto halHandle = static_cast<HAL_CANHandle>(handle);
+ int32_t status = 0;
+ HAL_WriteCANRTRFrame(halHandle, static_cast<int32_t>(length), apiId, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_CANAPIJNI
+ * Method: stopCANPacketRepeating
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_CANAPIJNI_stopCANPacketRepeating
+ (JNIEnv* env, jclass, jint handle, jint apiId)
+{
+ auto halHandle = static_cast<HAL_CANHandle>(handle);
+ int32_t status = 0;
+ HAL_StopCANPacketRepeating(halHandle, apiId, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_CANAPIJNI
+ * Method: readCANPacketNew
+ * Signature: (IILjava/lang/Object;)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_CANAPIJNI_readCANPacketNew
+ (JNIEnv* env, jclass, jint handle, jint apiId, jobject data)
+{
+ auto halHandle = static_cast<HAL_CANHandle>(handle);
+ uint8_t dataTemp[8];
+ int32_t dataLength = 0;
+ uint64_t timestamp = 0;
+ int32_t status = 0;
+ HAL_ReadCANPacketNew(halHandle, apiId, dataTemp, &dataLength, ×tamp,
+ &status);
+ if (status == HAL_ERR_CANSessionMux_MessageNotFound) {
+ return false;
+ }
+ if (!CheckStatus(env, status)) {
+ return false;
+ }
+ if (dataLength > 8) dataLength = 8;
+
+ jbyteArray toSetArray = SetCANDataObject(env, data, dataLength, timestamp);
+ auto javaLen = env->GetArrayLength(toSetArray);
+ if (javaLen < dataLength) dataLength = javaLen;
+ env->SetByteArrayRegion(toSetArray, 0, dataLength,
+ reinterpret_cast<jbyte*>(dataTemp));
+ return true;
+}
+
+/*
+ * Class: edu_wpi_first_hal_CANAPIJNI
+ * Method: readCANPacketLatest
+ * Signature: (IILjava/lang/Object;)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_CANAPIJNI_readCANPacketLatest
+ (JNIEnv* env, jclass, jint handle, jint apiId, jobject data)
+{
+ auto halHandle = static_cast<HAL_CANHandle>(handle);
+ uint8_t dataTemp[8];
+ int32_t dataLength = 0;
+ uint64_t timestamp = 0;
+ int32_t status = 0;
+ HAL_ReadCANPacketLatest(halHandle, apiId, dataTemp, &dataLength, ×tamp,
+ &status);
+ if (status == HAL_ERR_CANSessionMux_MessageNotFound) {
+ return false;
+ }
+ if (!CheckStatus(env, status)) {
+ return false;
+ }
+ if (dataLength > 8) dataLength = 8;
+
+ jbyteArray toSetArray = SetCANDataObject(env, data, dataLength, timestamp);
+ auto javaLen = env->GetArrayLength(toSetArray);
+ if (javaLen < dataLength) dataLength = javaLen;
+ env->SetByteArrayRegion(toSetArray, 0, dataLength,
+ reinterpret_cast<jbyte*>(dataTemp));
+ return true;
+}
+
+/*
+ * Class: edu_wpi_first_hal_CANAPIJNI
+ * Method: readCANPacketTimeout
+ * Signature: (IIILjava/lang/Object;)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_CANAPIJNI_readCANPacketTimeout
+ (JNIEnv* env, jclass, jint handle, jint apiId, jint timeoutMs, jobject data)
+{
+ auto halHandle = static_cast<HAL_CANHandle>(handle);
+ uint8_t dataTemp[8];
+ int32_t dataLength = 0;
+ uint64_t timestamp = 0;
+ int32_t status = 0;
+ HAL_ReadCANPacketTimeout(halHandle, apiId, dataTemp, &dataLength, ×tamp,
+ timeoutMs, &status);
+ if (status == HAL_CAN_TIMEOUT ||
+ status == HAL_ERR_CANSessionMux_MessageNotFound) {
+ return false;
+ }
+ if (!CheckStatus(env, status)) {
+ return false;
+ }
+ if (dataLength > 8) dataLength = 8;
+
+ jbyteArray toSetArray = SetCANDataObject(env, data, dataLength, timestamp);
+ auto javaLen = env->GetArrayLength(toSetArray);
+ if (javaLen < dataLength) dataLength = javaLen;
+ env->SetByteArrayRegion(toSetArray, 0, dataLength,
+ reinterpret_cast<jbyte*>(dataTemp));
+ return true;
+}
+} // extern "C"
diff --git a/hal/src/main/native/cpp/jni/CANJNI.cpp b/hal/src/main/native/cpp/jni/CANJNI.cpp
new file mode 100644
index 0000000..9278c24
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/CANJNI.cpp
@@ -0,0 +1,97 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 <jni.h>
+
+#include <cassert>
+
+#include <wpi/SmallString.h>
+#include <wpi/jni_util.h>
+#include <wpi/raw_ostream.h>
+
+#include "HALUtil.h"
+#include "edu_wpi_first_hal_can_CANJNI.h"
+#include "hal/CAN.h"
+
+using namespace frc;
+using namespace wpi::java;
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_hal_can_CANJNI
+ * Method: FRCNetCommCANSessionMuxSendMessage
+ * Signature: (I[BI)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_can_CANJNI_FRCNetCommCANSessionMuxSendMessage
+ (JNIEnv* env, jclass, jint messageID, jbyteArray data, jint periodMs)
+{
+ JByteArrayRef dataArray{env, data};
+
+ const uint8_t* dataBuffer =
+ reinterpret_cast<const uint8_t*>(dataArray.array().data());
+ uint8_t dataSize = dataArray.array().size();
+
+ int32_t status = 0;
+ HAL_CAN_SendMessage(messageID, dataBuffer, dataSize, periodMs, &status);
+ CheckCANStatus(env, status, messageID);
+}
+
+/*
+ * Class: edu_wpi_first_hal_can_CANJNI
+ * Method: FRCNetCommCANSessionMuxReceiveMessage
+ * Signature: (Ljava/lang/Object;ILjava/lang/Object;)[B
+ */
+JNIEXPORT jbyteArray JNICALL
+Java_edu_wpi_first_hal_can_CANJNI_FRCNetCommCANSessionMuxReceiveMessage
+ (JNIEnv* env, jclass, jobject messageID, jint messageIDMask,
+ jobject timeStamp)
+{
+ uint32_t* messageIDPtr =
+ reinterpret_cast<uint32_t*>(env->GetDirectBufferAddress(messageID));
+ uint32_t* timeStampPtr =
+ reinterpret_cast<uint32_t*>(env->GetDirectBufferAddress(timeStamp));
+
+ uint8_t dataSize = 0;
+ uint8_t buffer[8];
+
+ int32_t status = 0;
+ HAL_CAN_ReceiveMessage(messageIDPtr, messageIDMask, buffer, &dataSize,
+ timeStampPtr, &status);
+
+ if (!CheckCANStatus(env, status, *messageIDPtr)) return nullptr;
+ return MakeJByteArray(env,
+ wpi::StringRef{reinterpret_cast<const char*>(buffer),
+ static_cast<size_t>(dataSize)});
+}
+
+/*
+ * Class: edu_wpi_first_hal_can_CANJNI
+ * Method: GetCANStatus
+ * Signature: (Ljava/lang/Object;)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_can_CANJNI_GetCANStatus
+ (JNIEnv* env, jclass, jobject canStatus)
+{
+ float percentBusUtilization = 0;
+ uint32_t busOffCount = 0;
+ uint32_t txFullCount = 0;
+ uint32_t receiveErrorCount = 0;
+ uint32_t transmitErrorCount = 0;
+ int32_t status = 0;
+ HAL_CAN_GetCANStatus(&percentBusUtilization, &busOffCount, &txFullCount,
+ &receiveErrorCount, &transmitErrorCount, &status);
+
+ if (!CheckStatus(env, status)) return;
+
+ SetCanStatusObject(env, canStatus, percentBusUtilization, busOffCount,
+ txFullCount, receiveErrorCount, transmitErrorCount);
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/cpp/jni/CompressorJNI.cpp b/hal/src/main/native/cpp/jni/CompressorJNI.cpp
new file mode 100644
index 0000000..75a5e0a
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/CompressorJNI.cpp
@@ -0,0 +1,233 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 "HALUtil.h"
+#include "edu_wpi_first_hal_CompressorJNI.h"
+#include "hal/Compressor.h"
+#include "hal/Ports.h"
+#include "hal/Solenoid.h"
+
+using namespace frc;
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_hal_CompressorJNI
+ * Method: initializeCompressor
+ * Signature: (B)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_CompressorJNI_initializeCompressor
+ (JNIEnv* env, jclass, jbyte module)
+{
+ int32_t status = 0;
+ auto handle = HAL_InitializeCompressor(module, &status);
+ CheckStatusRange(env, status, 0, HAL_GetNumPCMModules(), module);
+
+ return (jint)handle;
+}
+
+/*
+ * Class: edu_wpi_first_hal_CompressorJNI
+ * Method: checkCompressorModule
+ * Signature: (B)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_CompressorJNI_checkCompressorModule
+ (JNIEnv* env, jclass, jbyte module)
+{
+ return HAL_CheckCompressorModule(module);
+}
+
+/*
+ * Class: edu_wpi_first_hal_CompressorJNI
+ * Method: getCompressor
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_CompressorJNI_getCompressor
+ (JNIEnv* env, jclass, jint compressorHandle)
+{
+ int32_t status = 0;
+ bool val = HAL_GetCompressor((HAL_CompressorHandle)compressorHandle, &status);
+ CheckStatus(env, status);
+ return val;
+}
+
+/*
+ * Class: edu_wpi_first_hal_CompressorJNI
+ * Method: setCompressorClosedLoopControl
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_CompressorJNI_setCompressorClosedLoopControl
+ (JNIEnv* env, jclass, jint compressorHandle, jboolean value)
+{
+ int32_t status = 0;
+ HAL_SetCompressorClosedLoopControl((HAL_CompressorHandle)compressorHandle,
+ value, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_CompressorJNI
+ * Method: getCompressorClosedLoopControl
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_CompressorJNI_getCompressorClosedLoopControl
+ (JNIEnv* env, jclass, jint compressorHandle)
+{
+ int32_t status = 0;
+ bool val = HAL_GetCompressorClosedLoopControl(
+ (HAL_CompressorHandle)compressorHandle, &status);
+ CheckStatus(env, status);
+ return val;
+}
+
+/*
+ * Class: edu_wpi_first_hal_CompressorJNI
+ * Method: getCompressorPressureSwitch
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_CompressorJNI_getCompressorPressureSwitch
+ (JNIEnv* env, jclass, jint compressorHandle)
+{
+ int32_t status = 0;
+ bool val = HAL_GetCompressorPressureSwitch(
+ (HAL_CompressorHandle)compressorHandle, &status);
+ CheckStatus(env, status);
+ return val;
+}
+
+/*
+ * Class: edu_wpi_first_hal_CompressorJNI
+ * Method: getCompressorCurrent
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_CompressorJNI_getCompressorCurrent
+ (JNIEnv* env, jclass, jint compressorHandle)
+{
+ int32_t status = 0;
+ double val =
+ HAL_GetCompressorCurrent((HAL_CompressorHandle)compressorHandle, &status);
+ CheckStatus(env, status);
+ return val;
+}
+
+/*
+ * Class: edu_wpi_first_hal_CompressorJNI
+ * Method: getCompressorCurrentTooHighFault
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_CompressorJNI_getCompressorCurrentTooHighFault
+ (JNIEnv* env, jclass, jint compressorHandle)
+{
+ int32_t status = 0;
+ bool val = HAL_GetCompressorCurrentTooHighFault(
+ (HAL_CompressorHandle)compressorHandle, &status);
+ CheckStatus(env, status);
+ return val;
+}
+
+/*
+ * Class: edu_wpi_first_hal_CompressorJNI
+ * Method: getCompressorCurrentTooHighStickyFault
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_CompressorJNI_getCompressorCurrentTooHighStickyFault
+ (JNIEnv* env, jclass, jint compressorHandle)
+{
+ int32_t status = 0;
+ bool val = HAL_GetCompressorCurrentTooHighStickyFault(
+ (HAL_CompressorHandle)compressorHandle, &status);
+ CheckStatus(env, status);
+ return val;
+}
+
+/*
+ * Class: edu_wpi_first_hal_CompressorJNI
+ * Method: getCompressorShortedStickyFault
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_CompressorJNI_getCompressorShortedStickyFault
+ (JNIEnv* env, jclass, jint compressorHandle)
+{
+ int32_t status = 0;
+ bool val = HAL_GetCompressorShortedStickyFault(
+ (HAL_CompressorHandle)compressorHandle, &status);
+ CheckStatus(env, status);
+ return val;
+}
+
+/*
+ * Class: edu_wpi_first_hal_CompressorJNI
+ * Method: getCompressorShortedFault
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_CompressorJNI_getCompressorShortedFault
+ (JNIEnv* env, jclass, jint compressorHandle)
+{
+ int32_t status = 0;
+ bool val = HAL_GetCompressorShortedFault(
+ (HAL_CompressorHandle)compressorHandle, &status);
+ CheckStatus(env, status);
+ return val;
+}
+
+/*
+ * Class: edu_wpi_first_hal_CompressorJNI
+ * Method: getCompressorNotConnectedStickyFault
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_CompressorJNI_getCompressorNotConnectedStickyFault
+ (JNIEnv* env, jclass, jint compressorHandle)
+{
+ int32_t status = 0;
+ bool val = HAL_GetCompressorNotConnectedStickyFault(
+ (HAL_CompressorHandle)compressorHandle, &status);
+ CheckStatus(env, status);
+ return val;
+}
+
+/*
+ * Class: edu_wpi_first_hal_CompressorJNI
+ * Method: getCompressorNotConnectedFault
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_CompressorJNI_getCompressorNotConnectedFault
+ (JNIEnv* env, jclass, jint compressorHandle)
+{
+ int32_t status = 0;
+ bool val = HAL_GetCompressorNotConnectedFault(
+ (HAL_CompressorHandle)compressorHandle, &status);
+ CheckStatus(env, status);
+ return val;
+}
+/*
+ * Class: edu_wpi_first_hal_CompressorJNI
+ * Method: clearAllPCMStickyFaults
+ * Signature: (B)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_CompressorJNI_clearAllPCMStickyFaults
+ (JNIEnv* env, jclass, jbyte module)
+{
+ int32_t status = 0;
+ HAL_ClearAllPCMStickyFaults(static_cast<int32_t>(module), &status);
+ CheckStatus(env, status);
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/cpp/jni/ConstantsJNI.cpp b/hal/src/main/native/cpp/jni/ConstantsJNI.cpp
new file mode 100644
index 0000000..fb1ae0b
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/ConstantsJNI.cpp
@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 <jni.h>
+
+#include <cassert>
+
+#include "HALUtil.h"
+#include "edu_wpi_first_hal_ConstantsJNI.h"
+#include "hal/Constants.h"
+
+using namespace frc;
+
+extern "C" {
+/*
+ * Class: edu_wpi_first_hal_ConstantsJNI
+ * Method: getSystemClockTicksPerMicrosecond
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_ConstantsJNI_getSystemClockTicksPerMicrosecond
+ (JNIEnv* env, jclass)
+{
+ jint value = HAL_GetSystemClockTicksPerMicrosecond();
+ return value;
+}
+} // extern "C"
diff --git a/hal/src/main/native/cpp/jni/CounterJNI.cpp b/hal/src/main/native/cpp/jni/CounterJNI.cpp
new file mode 100644
index 0000000..41dedab
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/CounterJNI.cpp
@@ -0,0 +1,370 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 <jni.h>
+
+#include <cassert>
+
+#include "HALUtil.h"
+#include "edu_wpi_first_hal_CounterJNI.h"
+#include "hal/Counter.h"
+#include "hal/Errors.h"
+
+using namespace frc;
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_hal_CounterJNI
+ * Method: initializeCounter
+ * Signature: (ILjava/lang/Object;)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_CounterJNI_initializeCounter
+ (JNIEnv* env, jclass, jint mode, jobject index)
+{
+ jint* indexPtr = reinterpret_cast<jint*>(env->GetDirectBufferAddress(index));
+ int32_t status = 0;
+ auto counter = HAL_InitializeCounter(
+ (HAL_Counter_Mode)mode, reinterpret_cast<int32_t*>(indexPtr), &status);
+ CheckStatusForceThrow(env, status);
+ return (jint)counter;
+}
+
+/*
+ * Class: edu_wpi_first_hal_CounterJNI
+ * Method: freeCounter
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_CounterJNI_freeCounter
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ HAL_FreeCounter((HAL_CounterHandle)id, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_CounterJNI
+ * Method: setCounterAverageSize
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_CounterJNI_setCounterAverageSize
+ (JNIEnv* env, jclass, jint id, jint value)
+{
+ int32_t status = 0;
+ HAL_SetCounterAverageSize((HAL_CounterHandle)id, value, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_CounterJNI
+ * Method: setCounterUpSource
+ * Signature: (III)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_CounterJNI_setCounterUpSource
+ (JNIEnv* env, jclass, jint id, jint digitalSourceHandle,
+ jint analogTriggerType)
+{
+ int32_t status = 0;
+ HAL_SetCounterUpSource((HAL_CounterHandle)id, (HAL_Handle)digitalSourceHandle,
+ (HAL_AnalogTriggerType)analogTriggerType, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_CounterJNI
+ * Method: setCounterUpSourceEdge
+ * Signature: (IZZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_CounterJNI_setCounterUpSourceEdge
+ (JNIEnv* env, jclass, jint id, jboolean valueRise, jboolean valueFall)
+{
+ int32_t status = 0;
+ HAL_SetCounterUpSourceEdge((HAL_CounterHandle)id, valueRise, valueFall,
+ &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_CounterJNI
+ * Method: clearCounterUpSource
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_CounterJNI_clearCounterUpSource
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ HAL_ClearCounterUpSource((HAL_CounterHandle)id, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_CounterJNI
+ * Method: setCounterDownSource
+ * Signature: (III)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_CounterJNI_setCounterDownSource
+ (JNIEnv* env, jclass, jint id, jint digitalSourceHandle,
+ jint analogTriggerType)
+{
+ int32_t status = 0;
+ HAL_SetCounterDownSource((HAL_CounterHandle)id,
+ (HAL_Handle)digitalSourceHandle,
+ (HAL_AnalogTriggerType)analogTriggerType, &status);
+ if (status == PARAMETER_OUT_OF_RANGE) {
+ ThrowIllegalArgumentException(env,
+ "Counter only supports DownSource in "
+ "TwoPulse and ExternalDirection modes.");
+ return;
+ }
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_CounterJNI
+ * Method: setCounterDownSourceEdge
+ * Signature: (IZZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_CounterJNI_setCounterDownSourceEdge
+ (JNIEnv* env, jclass, jint id, jboolean valueRise, jboolean valueFall)
+{
+ int32_t status = 0;
+ HAL_SetCounterDownSourceEdge((HAL_CounterHandle)id, valueRise, valueFall,
+ &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_CounterJNI
+ * Method: clearCounterDownSource
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_CounterJNI_clearCounterDownSource
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ HAL_ClearCounterDownSource((HAL_CounterHandle)id, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_CounterJNI
+ * Method: setCounterUpDownMode
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_CounterJNI_setCounterUpDownMode
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ HAL_SetCounterUpDownMode((HAL_CounterHandle)id, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_CounterJNI
+ * Method: setCounterExternalDirectionMode
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_CounterJNI_setCounterExternalDirectionMode
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ HAL_SetCounterExternalDirectionMode((HAL_CounterHandle)id, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_CounterJNI
+ * Method: setCounterSemiPeriodMode
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_CounterJNI_setCounterSemiPeriodMode
+ (JNIEnv* env, jclass, jint id, jboolean value)
+{
+ int32_t status = 0;
+ HAL_SetCounterSemiPeriodMode((HAL_CounterHandle)id, value, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_CounterJNI
+ * Method: setCounterPulseLengthMode
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_CounterJNI_setCounterPulseLengthMode
+ (JNIEnv* env, jclass, jint id, jdouble value)
+{
+ int32_t status = 0;
+ HAL_SetCounterPulseLengthMode((HAL_CounterHandle)id, value, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_CounterJNI
+ * Method: getCounterSamplesToAverage
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_CounterJNI_getCounterSamplesToAverage
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ jint returnValue =
+ HAL_GetCounterSamplesToAverage((HAL_CounterHandle)id, &status);
+ CheckStatus(env, status);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_CounterJNI
+ * Method: setCounterSamplesToAverage
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_CounterJNI_setCounterSamplesToAverage
+ (JNIEnv* env, jclass, jint id, jint value)
+{
+ int32_t status = 0;
+ HAL_SetCounterSamplesToAverage((HAL_CounterHandle)id, value, &status);
+ if (status == PARAMETER_OUT_OF_RANGE) {
+ ThrowBoundaryException(env, value, 1, 127);
+ return;
+ }
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_CounterJNI
+ * Method: resetCounter
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_CounterJNI_resetCounter
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ HAL_ResetCounter((HAL_CounterHandle)id, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_CounterJNI
+ * Method: getCounter
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_CounterJNI_getCounter
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ jint returnValue = HAL_GetCounter((HAL_CounterHandle)id, &status);
+ CheckStatus(env, status);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_CounterJNI
+ * Method: getCounterPeriod
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_CounterJNI_getCounterPeriod
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ jdouble returnValue = HAL_GetCounterPeriod((HAL_CounterHandle)id, &status);
+ CheckStatus(env, status);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_CounterJNI
+ * Method: setCounterMaxPeriod
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_CounterJNI_setCounterMaxPeriod
+ (JNIEnv* env, jclass, jint id, jdouble value)
+{
+ int32_t status = 0;
+ HAL_SetCounterMaxPeriod((HAL_CounterHandle)id, value, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_CounterJNI
+ * Method: setCounterUpdateWhenEmpty
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_CounterJNI_setCounterUpdateWhenEmpty
+ (JNIEnv* env, jclass, jint id, jboolean value)
+{
+ int32_t status = 0;
+ HAL_SetCounterUpdateWhenEmpty((HAL_CounterHandle)id, value, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_CounterJNI
+ * Method: getCounterStopped
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_CounterJNI_getCounterStopped
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ jboolean returnValue = HAL_GetCounterStopped((HAL_CounterHandle)id, &status);
+ CheckStatus(env, status);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_CounterJNI
+ * Method: getCounterDirection
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_CounterJNI_getCounterDirection
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ jboolean returnValue =
+ HAL_GetCounterDirection((HAL_CounterHandle)id, &status);
+ CheckStatus(env, status);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_CounterJNI
+ * Method: setCounterReverseDirection
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_CounterJNI_setCounterReverseDirection
+ (JNIEnv* env, jclass, jint id, jboolean value)
+{
+ int32_t status = 0;
+ HAL_SetCounterReverseDirection((HAL_CounterHandle)id, value, &status);
+ CheckStatus(env, status);
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/cpp/jni/DIOJNI.cpp b/hal/src/main/native/cpp/jni/DIOJNI.cpp
new file mode 100644
index 0000000..9c44b4c
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/DIOJNI.cpp
@@ -0,0 +1,265 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 <jni.h>
+
+#include <cassert>
+
+#include "HALUtil.h"
+#include "edu_wpi_first_hal_DIOJNI.h"
+#include "hal/DIO.h"
+#include "hal/PWM.h"
+#include "hal/Ports.h"
+#include "hal/handles/HandlesInternal.h"
+
+using namespace frc;
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_hal_DIOJNI
+ * Method: initializeDIOPort
+ * Signature: (IZ)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_DIOJNI_initializeDIOPort
+ (JNIEnv* env, jclass, jint id, jboolean input)
+{
+ int32_t status = 0;
+ auto dio = HAL_InitializeDIOPort((HAL_PortHandle)id,
+ static_cast<uint8_t>(input), &status);
+ CheckStatusRange(env, status, 0, HAL_GetNumDigitalChannels(),
+ hal::getPortHandleChannel((HAL_PortHandle)id));
+ return (jint)dio;
+}
+
+/*
+ * Class: edu_wpi_first_hal_DIOJNI
+ * Method: checkDIOChannel
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_DIOJNI_checkDIOChannel
+ (JNIEnv* env, jclass, jint channel)
+{
+ return HAL_CheckDIOChannel(channel);
+}
+
+/*
+ * Class: edu_wpi_first_hal_DIOJNI
+ * Method: freeDIOPort
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_DIOJNI_freeDIOPort
+ (JNIEnv* env, jclass, jint id)
+{
+ HAL_FreeDIOPort((HAL_DigitalHandle)id);
+}
+
+/*
+ * Class: edu_wpi_first_hal_DIOJNI
+ * Method: setDIOSimDevice
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_DIOJNI_setDIOSimDevice
+ (JNIEnv* env, jclass, jint handle, jint device)
+{
+ HAL_SetDIOSimDevice((HAL_DigitalHandle)handle, (HAL_SimDeviceHandle)device);
+}
+
+/*
+ * Class: edu_wpi_first_hal_DIOJNI
+ * Method: setDIO
+ * Signature: (IS)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_DIOJNI_setDIO
+ (JNIEnv* env, jclass, jint id, jshort value)
+{
+ int32_t status = 0;
+ HAL_SetDIO((HAL_DigitalHandle)id, value, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_DIOJNI
+ * Method: setDIODirection
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_DIOJNI_setDIODirection
+ (JNIEnv* env, jclass, jint id, jboolean input)
+{
+ int32_t status = 0;
+ HAL_SetDIODirection((HAL_DigitalHandle)id, input, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_DIOJNI
+ * Method: getDIO
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_DIOJNI_getDIO
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ jboolean returnValue = HAL_GetDIO((HAL_DigitalHandle)id, &status);
+ CheckStatus(env, status);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_DIOJNI
+ * Method: getDIODirection
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_DIOJNI_getDIODirection
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ jboolean returnValue = HAL_GetDIODirection((HAL_DigitalHandle)id, &status);
+ CheckStatus(env, status);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_DIOJNI
+ * Method: pulse
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_DIOJNI_pulse
+ (JNIEnv* env, jclass, jint id, jdouble value)
+{
+ int32_t status = 0;
+ HAL_Pulse((HAL_DigitalHandle)id, value, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_DIOJNI
+ * Method: isPulsing
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_DIOJNI_isPulsing
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ jboolean returnValue = HAL_IsPulsing((HAL_DigitalHandle)id, &status);
+ CheckStatus(env, status);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_DIOJNI
+ * Method: isAnyPulsing
+ * Signature: ()Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_DIOJNI_isAnyPulsing
+ (JNIEnv* env, jclass)
+{
+ int32_t status = 0;
+ jboolean returnValue = HAL_IsAnyPulsing(&status);
+ CheckStatus(env, status);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_DIOJNI
+ * Method: getLoopTiming
+ * Signature: ()S
+ */
+JNIEXPORT jshort JNICALL
+Java_edu_wpi_first_hal_DIOJNI_getLoopTiming
+ (JNIEnv* env, jclass)
+{
+ int32_t status = 0;
+ jshort returnValue = HAL_GetPWMLoopTiming(&status);
+ CheckStatus(env, status);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_DIOJNI
+ * Method: allocateDigitalPWM
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_DIOJNI_allocateDigitalPWM
+ (JNIEnv* env, jclass)
+{
+ int32_t status = 0;
+ auto pwm = HAL_AllocateDigitalPWM(&status);
+ CheckStatus(env, status);
+ return (jint)pwm;
+}
+
+/*
+ * Class: edu_wpi_first_hal_DIOJNI
+ * Method: freeDigitalPWM
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_DIOJNI_freeDigitalPWM
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ HAL_FreeDigitalPWM((HAL_DigitalPWMHandle)id, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_DIOJNI
+ * Method: setDigitalPWMRate
+ * Signature: (D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_DIOJNI_setDigitalPWMRate
+ (JNIEnv* env, jclass, jdouble value)
+{
+ int32_t status = 0;
+ HAL_SetDigitalPWMRate(value, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_DIOJNI
+ * Method: setDigitalPWMDutyCycle
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_DIOJNI_setDigitalPWMDutyCycle
+ (JNIEnv* env, jclass, jint id, jdouble value)
+{
+ int32_t status = 0;
+ HAL_SetDigitalPWMDutyCycle((HAL_DigitalPWMHandle)id, value, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_DIOJNI
+ * Method: setDigitalPWMOutputChannel
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_DIOJNI_setDigitalPWMOutputChannel
+ (JNIEnv* env, jclass, jint id, jint value)
+{
+ int32_t status = 0;
+ HAL_SetDigitalPWMOutputChannel((HAL_DigitalPWMHandle)id,
+ static_cast<uint32_t>(value), &status);
+ CheckStatus(env, status);
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/cpp/jni/DigitalGlitchFilterJNI.cpp b/hal/src/main/native/cpp/jni/DigitalGlitchFilterJNI.cpp
new file mode 100644
index 0000000..fbbaadb
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/DigitalGlitchFilterJNI.cpp
@@ -0,0 +1,82 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 <jni.h>
+
+#include "HALUtil.h"
+#include "edu_wpi_first_hal_DigitalGlitchFilterJNI.h"
+#include "hal/DIO.h"
+
+using namespace frc;
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_hal_DigitalGlitchFilterJNI
+ * Method: setFilterSelect
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_DigitalGlitchFilterJNI_setFilterSelect
+ (JNIEnv* env, jclass, jint id, jint filter_index)
+{
+ int32_t status = 0;
+
+ HAL_SetFilterSelect(static_cast<HAL_DigitalHandle>(id), filter_index,
+ &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_DigitalGlitchFilterJNI
+ * Method: getFilterSelect
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_DigitalGlitchFilterJNI_getFilterSelect
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+
+ jint result =
+ HAL_GetFilterSelect(static_cast<HAL_DigitalHandle>(id), &status);
+ CheckStatus(env, status);
+ return result;
+}
+
+/*
+ * Class: edu_wpi_first_hal_DigitalGlitchFilterJNI
+ * Method: setFilterPeriod
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_DigitalGlitchFilterJNI_setFilterPeriod
+ (JNIEnv* env, jclass, jint filter_index, jint fpga_cycles)
+{
+ int32_t status = 0;
+
+ HAL_SetFilterPeriod(filter_index, fpga_cycles, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_DigitalGlitchFilterJNI
+ * Method: getFilterPeriod
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_DigitalGlitchFilterJNI_getFilterPeriod
+ (JNIEnv* env, jclass, jint filter_index)
+{
+ int32_t status = 0;
+
+ jint result = HAL_GetFilterPeriod(filter_index, &status);
+ CheckStatus(env, status);
+ return result;
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/cpp/jni/DutyCycleJNI.cpp b/hal/src/main/native/cpp/jni/DutyCycleJNI.cpp
new file mode 100644
index 0000000..510ca00
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/DutyCycleJNI.cpp
@@ -0,0 +1,126 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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 <jni.h>
+
+#include "HALUtil.h"
+#include "edu_wpi_first_hal_DutyCycleJNI.h"
+#include "hal/DutyCycle.h"
+
+using namespace frc;
+
+extern "C" {
+/*
+ * Class: edu_wpi_first_hal_DutyCycleJNI
+ * Method: initialize
+ * Signature: (II)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_DutyCycleJNI_initialize
+ (JNIEnv* env, jclass, jint digitalSourceHandle, jint analogTriggerType)
+{
+ int32_t status = 0;
+ auto handle = HAL_InitializeDutyCycle(
+ static_cast<HAL_Handle>(digitalSourceHandle),
+ static_cast<HAL_AnalogTriggerType>(analogTriggerType), &status);
+ CheckStatus(env, status);
+ return handle;
+}
+
+/*
+ * Class: edu_wpi_first_hal_DutyCycleJNI
+ * Method: free
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_DutyCycleJNI_free
+ (JNIEnv*, jclass, jint handle)
+{
+ HAL_FreeDutyCycle(static_cast<HAL_DutyCycleHandle>(handle));
+}
+
+/*
+ * Class: edu_wpi_first_hal_DutyCycleJNI
+ * Method: getFrequency
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_DutyCycleJNI_getFrequency
+ (JNIEnv* env, jclass, jint handle)
+{
+ int32_t status = 0;
+ auto retVal = HAL_GetDutyCycleFrequency(
+ static_cast<HAL_DutyCycleHandle>(handle), &status);
+ CheckStatus(env, status);
+ return retVal;
+}
+
+/*
+ * Class: edu_wpi_first_hal_DutyCycleJNI
+ * Method: getOutput
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_DutyCycleJNI_getOutput
+ (JNIEnv* env, jclass, jint handle)
+{
+ int32_t status = 0;
+ auto retVal =
+ HAL_GetDutyCycleOutput(static_cast<HAL_DutyCycleHandle>(handle), &status);
+ CheckStatus(env, status);
+ return retVal;
+}
+
+/*
+ * Class: edu_wpi_first_hal_DutyCycleJNI
+ * Method: getOutputRaw
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_DutyCycleJNI_getOutputRaw
+ (JNIEnv* env, jclass, jint handle)
+{
+ int32_t status = 0;
+ auto retVal = HAL_GetDutyCycleOutputRaw(
+ static_cast<HAL_DutyCycleHandle>(handle), &status);
+ CheckStatus(env, status);
+ return retVal;
+}
+
+/*
+ * Class: edu_wpi_first_hal_DutyCycleJNI
+ * Method: getOutputScaleFactor
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_DutyCycleJNI_getOutputScaleFactor
+ (JNIEnv* env, jclass, jint handle)
+{
+ int32_t status = 0;
+ auto retVal = HAL_GetDutyCycleOutputScaleFactor(
+ static_cast<HAL_DutyCycleHandle>(handle), &status);
+ CheckStatus(env, status);
+ return retVal;
+}
+
+/*
+ * Class: edu_wpi_first_hal_DutyCycleJNI
+ * Method: getFPGAIndex
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_DutyCycleJNI_getFPGAIndex
+ (JNIEnv* env, jclass, jint handle)
+{
+ int32_t status = 0;
+ auto retVal = HAL_GetDutyCycleFPGAIndex(
+ static_cast<HAL_DutyCycleHandle>(handle), &status);
+ CheckStatus(env, status);
+ return retVal;
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/cpp/jni/EncoderJNI.cpp b/hal/src/main/native/cpp/jni/EncoderJNI.cpp
new file mode 100644
index 0000000..e5aa7e8
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/EncoderJNI.cpp
@@ -0,0 +1,392 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 <jni.h>
+
+#include <cassert>
+
+#include "HALUtil.h"
+#include "edu_wpi_first_hal_EncoderJNI.h"
+#include "hal/Encoder.h"
+#include "hal/Errors.h"
+
+using namespace frc;
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_hal_EncoderJNI
+ * Method: initializeEncoder
+ * Signature: (IIIIZI)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_EncoderJNI_initializeEncoder
+ (JNIEnv* env, jclass, jint digitalSourceHandleA, jint analogTriggerTypeA,
+ jint digitalSourceHandleB, jint analogTriggerTypeB,
+ jboolean reverseDirection, jint encodingType)
+{
+ int32_t status = 0;
+ auto encoder = HAL_InitializeEncoder(
+ (HAL_Handle)digitalSourceHandleA,
+ (HAL_AnalogTriggerType)analogTriggerTypeA,
+ (HAL_Handle)digitalSourceHandleB,
+ (HAL_AnalogTriggerType)analogTriggerTypeB, reverseDirection,
+ (HAL_EncoderEncodingType)encodingType, &status);
+ CheckStatusForceThrow(env, status);
+ return (jint)encoder;
+}
+
+/*
+ * Class: edu_wpi_first_hal_EncoderJNI
+ * Method: freeEncoder
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_EncoderJNI_freeEncoder
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ HAL_FreeEncoder((HAL_EncoderHandle)id, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_EncoderJNI
+ * Method: setEncoderSimDevice
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_EncoderJNI_setEncoderSimDevice
+ (JNIEnv* env, jclass, jint handle, jint device)
+{
+ HAL_SetEncoderSimDevice((HAL_EncoderHandle)handle,
+ (HAL_SimDeviceHandle)device);
+}
+
+/*
+ * Class: edu_wpi_first_hal_EncoderJNI
+ * Method: getEncoder
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_EncoderJNI_getEncoder
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ jint returnValue = HAL_GetEncoder((HAL_EncoderHandle)id, &status);
+ CheckStatus(env, status);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_EncoderJNI
+ * Method: getEncoderRaw
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_EncoderJNI_getEncoderRaw
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ jint returnValue = HAL_GetEncoderRaw((HAL_EncoderHandle)id, &status);
+ CheckStatus(env, status);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_EncoderJNI
+ * Method: getEncodingScaleFactor
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_EncoderJNI_getEncodingScaleFactor
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ jint returnValue =
+ HAL_GetEncoderEncodingScale((HAL_EncoderHandle)id, &status);
+ CheckStatus(env, status);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_EncoderJNI
+ * Method: resetEncoder
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_EncoderJNI_resetEncoder
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ HAL_ResetEncoder((HAL_EncoderHandle)id, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_EncoderJNI
+ * Method: getEncoderPeriod
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_EncoderJNI_getEncoderPeriod
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ double returnValue = HAL_GetEncoderPeriod((HAL_EncoderHandle)id, &status);
+ CheckStatus(env, status);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_EncoderJNI
+ * Method: setEncoderMaxPeriod
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_EncoderJNI_setEncoderMaxPeriod
+ (JNIEnv* env, jclass, jint id, jdouble value)
+{
+ int32_t status = 0;
+ HAL_SetEncoderMaxPeriod((HAL_EncoderHandle)id, value, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_EncoderJNI
+ * Method: getEncoderStopped
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_EncoderJNI_getEncoderStopped
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ jboolean returnValue = HAL_GetEncoderStopped((HAL_EncoderHandle)id, &status);
+ CheckStatus(env, status);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_EncoderJNI
+ * Method: getEncoderDirection
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_EncoderJNI_getEncoderDirection
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ jboolean returnValue =
+ HAL_GetEncoderDirection((HAL_EncoderHandle)id, &status);
+ CheckStatus(env, status);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_EncoderJNI
+ * Method: getEncoderDistance
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_EncoderJNI_getEncoderDistance
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ jdouble returnValue = HAL_GetEncoderDistance((HAL_EncoderHandle)id, &status);
+ CheckStatus(env, status);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_EncoderJNI
+ * Method: getEncoderRate
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_EncoderJNI_getEncoderRate
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ jdouble returnValue = HAL_GetEncoderRate((HAL_EncoderHandle)id, &status);
+ CheckStatus(env, status);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_EncoderJNI
+ * Method: setEncoderMinRate
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_EncoderJNI_setEncoderMinRate
+ (JNIEnv* env, jclass, jint id, jdouble value)
+{
+ int32_t status = 0;
+ HAL_SetEncoderMinRate((HAL_EncoderHandle)id, value, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_EncoderJNI
+ * Method: setEncoderDistancePerPulse
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_EncoderJNI_setEncoderDistancePerPulse
+ (JNIEnv* env, jclass, jint id, jdouble value)
+{
+ int32_t status = 0;
+ HAL_SetEncoderDistancePerPulse((HAL_EncoderHandle)id, value, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_EncoderJNI
+ * Method: setEncoderReverseDirection
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_EncoderJNI_setEncoderReverseDirection
+ (JNIEnv* env, jclass, jint id, jboolean value)
+{
+ int32_t status = 0;
+ HAL_SetEncoderReverseDirection((HAL_EncoderHandle)id, value, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_EncoderJNI
+ * Method: setEncoderSamplesToAverage
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_EncoderJNI_setEncoderSamplesToAverage
+ (JNIEnv* env, jclass, jint id, jint value)
+{
+ int32_t status = 0;
+ HAL_SetEncoderSamplesToAverage((HAL_EncoderHandle)id, value, &status);
+ if (status == PARAMETER_OUT_OF_RANGE) {
+ ThrowBoundaryException(env, value, 1, 127);
+ return;
+ }
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_EncoderJNI
+ * Method: getEncoderSamplesToAverage
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_EncoderJNI_getEncoderSamplesToAverage
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ jint returnValue =
+ HAL_GetEncoderSamplesToAverage((HAL_EncoderHandle)id, &status);
+ CheckStatus(env, status);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_EncoderJNI
+ * Method: setEncoderIndexSource
+ * Signature: (IIII)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_EncoderJNI_setEncoderIndexSource
+ (JNIEnv* env, jclass, jint id, jint digitalSourceHandle,
+ jint analogTriggerType, jint type)
+{
+ int32_t status = 0;
+ HAL_SetEncoderIndexSource((HAL_EncoderHandle)id,
+ (HAL_Handle)digitalSourceHandle,
+ (HAL_AnalogTriggerType)analogTriggerType,
+ (HAL_EncoderIndexingType)type, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_EncoderJNI
+ * Method: getEncoderFPGAIndex
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_EncoderJNI_getEncoderFPGAIndex
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ jint returnValue = HAL_GetEncoderFPGAIndex((HAL_EncoderHandle)id, &status);
+ CheckStatus(env, status);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_EncoderJNI
+ * Method: getEncoderEncodingScale
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_EncoderJNI_getEncoderEncodingScale
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ jint returnValue =
+ HAL_GetEncoderEncodingScale((HAL_EncoderHandle)id, &status);
+ CheckStatus(env, status);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_EncoderJNI
+ * Method: getEncoderDecodingScaleFactor
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_EncoderJNI_getEncoderDecodingScaleFactor
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ jdouble returnValue =
+ HAL_GetEncoderDecodingScaleFactor((HAL_EncoderHandle)id, &status);
+ CheckStatus(env, status);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_EncoderJNI
+ * Method: getEncoderDistancePerPulse
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_EncoderJNI_getEncoderDistancePerPulse
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ jdouble returnValue =
+ HAL_GetEncoderDistancePerPulse((HAL_EncoderHandle)id, &status);
+ CheckStatus(env, status);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_EncoderJNI
+ * Method: getEncoderEncodingType
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_EncoderJNI_getEncoderEncodingType
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ jint returnValue = HAL_GetEncoderEncodingType((HAL_EncoderHandle)id, &status);
+ CheckStatus(env, status);
+ return returnValue;
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/cpp/jni/HAL.cpp b/hal/src/main/native/cpp/jni/HAL.cpp
new file mode 100644
index 0000000..393b0b4
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/HAL.cpp
@@ -0,0 +1,477 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 "hal/HAL.h"
+
+#include <jni.h>
+
+#include <cassert>
+#include <cstring>
+
+#include <wpi/jni_util.h>
+
+#include "HALUtil.h"
+#include "edu_wpi_first_hal_HAL.h"
+#include "hal/DriverStation.h"
+#include "hal/Main.h"
+
+using namespace frc;
+using namespace wpi::java;
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_hal_HAL
+ * Method: initialize
+ * Signature: (II)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_HAL_initialize
+ (JNIEnv*, jclass, jint timeout, jint mode)
+{
+ return HAL_Initialize(timeout, mode);
+}
+
+/*
+ * Class: edu_wpi_first_hal_HAL
+ * Method: hasMain
+ * Signature: ()Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_HAL_hasMain
+ (JNIEnv*, jclass)
+{
+ return HAL_HasMain();
+}
+
+/*
+ * Class: edu_wpi_first_hal_HAL
+ * Method: runMain
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_HAL_runMain
+ (JNIEnv*, jclass)
+{
+ HAL_RunMain();
+}
+
+/*
+ * Class: edu_wpi_first_hal_HAL
+ * Method: exitMain
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_HAL_exitMain
+ (JNIEnv*, jclass)
+{
+ HAL_ExitMain();
+}
+
+/*
+ * Class: edu_wpi_first_hal_HAL
+ * Method: observeUserProgramStarting
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_HAL_observeUserProgramStarting
+ (JNIEnv*, jclass)
+{
+ HAL_ObserveUserProgramStarting();
+}
+
+/*
+ * Class: edu_wpi_first_hal_HAL
+ * Method: observeUserProgramDisabled
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_HAL_observeUserProgramDisabled
+ (JNIEnv*, jclass)
+{
+ HAL_ObserveUserProgramDisabled();
+}
+
+/*
+ * Class: edu_wpi_first_hal_HAL
+ * Method: observeUserProgramAutonomous
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_HAL_observeUserProgramAutonomous
+ (JNIEnv*, jclass)
+{
+ HAL_ObserveUserProgramAutonomous();
+}
+
+/*
+ * Class: edu_wpi_first_hal_HAL
+ * Method: observeUserProgramTeleop
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_HAL_observeUserProgramTeleop
+ (JNIEnv*, jclass)
+{
+ HAL_ObserveUserProgramTeleop();
+}
+
+/*
+ * Class: edu_wpi_first_hal_HAL
+ * Method: observeUserProgramTest
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_HAL_observeUserProgramTest
+ (JNIEnv*, jclass)
+{
+ HAL_ObserveUserProgramTest();
+}
+
+/*
+ * Class: edu_wpi_first_hal_HAL
+ * Method: report
+ * Signature: (IIILjava/lang/String;)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_HAL_report
+ (JNIEnv* paramEnv, jclass, jint paramResource, jint paramInstanceNumber,
+ jint paramContext, jstring paramFeature)
+{
+ JStringRef featureStr{paramEnv, paramFeature};
+ jint returnValue = HAL_Report(paramResource, paramInstanceNumber,
+ paramContext, featureStr.c_str());
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_HAL
+ * Method: nativeGetControlWord
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_HAL_nativeGetControlWord
+ (JNIEnv*, jclass)
+{
+ static_assert(sizeof(HAL_ControlWord) == sizeof(jint),
+ "Java int must match the size of control word");
+ HAL_ControlWord controlWord;
+ std::memset(&controlWord, 0, sizeof(HAL_ControlWord));
+ HAL_GetControlWord(&controlWord);
+ jint retVal = 0;
+ std::memcpy(&retVal, &controlWord, sizeof(HAL_ControlWord));
+ return retVal;
+}
+
+/*
+ * Class: edu_wpi_first_hal_HAL
+ * Method: nativeGetAllianceStation
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_HAL_nativeGetAllianceStation
+ (JNIEnv*, jclass)
+{
+ int32_t status = 0;
+ auto allianceStation = HAL_GetAllianceStation(&status);
+ return static_cast<jint>(allianceStation);
+}
+
+/*
+ * Class: edu_wpi_first_hal_HAL
+ * Method: getJoystickAxes
+ * Signature: (B[F)S
+ */
+JNIEXPORT jshort JNICALL
+Java_edu_wpi_first_hal_HAL_getJoystickAxes
+ (JNIEnv* env, jclass, jbyte joystickNum, jfloatArray axesArray)
+{
+ HAL_JoystickAxes axes;
+ HAL_GetJoystickAxes(joystickNum, &axes);
+
+ jsize javaSize = env->GetArrayLength(axesArray);
+ if (axes.count > javaSize) {
+ wpi::SmallString<128> errStr;
+ wpi::raw_svector_ostream oss{errStr};
+ oss << "Native array size larger then passed in java array size "
+ << "Native Size: " << static_cast<int>(axes.count)
+ << " Java Size: " << static_cast<int>(javaSize);
+
+ ThrowIllegalArgumentException(env, errStr.str());
+ return 0;
+ }
+
+ env->SetFloatArrayRegion(axesArray, 0, axes.count, axes.axes);
+
+ return axes.count;
+}
+
+/*
+ * Class: edu_wpi_first_hal_HAL
+ * Method: getJoystickPOVs
+ * Signature: (B[S)S
+ */
+JNIEXPORT jshort JNICALL
+Java_edu_wpi_first_hal_HAL_getJoystickPOVs
+ (JNIEnv* env, jclass, jbyte joystickNum, jshortArray povsArray)
+{
+ HAL_JoystickPOVs povs;
+ HAL_GetJoystickPOVs(joystickNum, &povs);
+
+ jsize javaSize = env->GetArrayLength(povsArray);
+ if (povs.count > javaSize) {
+ wpi::SmallString<128> errStr;
+ wpi::raw_svector_ostream oss{errStr};
+ oss << "Native array size larger then passed in java array size "
+ << "Native Size: " << static_cast<int>(povs.count)
+ << " Java Size: " << static_cast<int>(javaSize);
+
+ ThrowIllegalArgumentException(env, errStr.str());
+ return 0;
+ }
+
+ env->SetShortArrayRegion(povsArray, 0, povs.count, povs.povs);
+
+ return povs.count;
+}
+
+/*
+ * Class: edu_wpi_first_hal_HAL
+ * Method: getJoystickButtons
+ * Signature: (BLjava/lang/Object;)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_HAL_getJoystickButtons
+ (JNIEnv* env, jclass, jbyte joystickNum, jobject count)
+{
+ HAL_JoystickButtons joystickButtons;
+ HAL_GetJoystickButtons(joystickNum, &joystickButtons);
+ jbyte* countPtr =
+ reinterpret_cast<jbyte*>(env->GetDirectBufferAddress(count));
+ *countPtr = joystickButtons.count;
+ return joystickButtons.buttons;
+}
+
+/*
+ * Class: edu_wpi_first_hal_HAL
+ * Method: setJoystickOutputs
+ * Signature: (BISS)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_HAL_setJoystickOutputs
+ (JNIEnv*, jclass, jbyte port, jint outputs, jshort leftRumble,
+ jshort rightRumble)
+{
+ return HAL_SetJoystickOutputs(port, outputs, leftRumble, rightRumble);
+}
+
+/*
+ * Class: edu_wpi_first_hal_HAL
+ * Method: getJoystickIsXbox
+ * Signature: (B)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_HAL_getJoystickIsXbox
+ (JNIEnv*, jclass, jbyte port)
+{
+ return HAL_GetJoystickIsXbox(port);
+}
+
+/*
+ * Class: edu_wpi_first_hal_HAL
+ * Method: getJoystickType
+ * Signature: (B)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_HAL_getJoystickType
+ (JNIEnv*, jclass, jbyte port)
+{
+ return HAL_GetJoystickType(port);
+}
+
+/*
+ * Class: edu_wpi_first_hal_HAL
+ * Method: getJoystickName
+ * Signature: (B)Ljava/lang/String;
+ */
+JNIEXPORT jstring JNICALL
+Java_edu_wpi_first_hal_HAL_getJoystickName
+ (JNIEnv* env, jclass, jbyte port)
+{
+ char* joystickName = HAL_GetJoystickName(port);
+ jstring str = MakeJString(env, joystickName);
+ HAL_FreeJoystickName(joystickName);
+ return str;
+}
+
+/*
+ * Class: edu_wpi_first_hal_HAL
+ * Method: getJoystickAxisType
+ * Signature: (BB)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_HAL_getJoystickAxisType
+ (JNIEnv*, jclass, jbyte joystickNum, jbyte axis)
+{
+ return HAL_GetJoystickAxisType(joystickNum, axis);
+}
+
+/*
+ * Class: edu_wpi_first_hal_HAL
+ * Method: isNewControlData
+ * Signature: ()Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_HAL_isNewControlData
+ (JNIEnv*, jclass)
+{
+ return static_cast<jboolean>(HAL_IsNewControlData());
+}
+
+/*
+ * Class: edu_wpi_first_hal_HAL
+ * Method: waitForDSData
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_HAL_waitForDSData
+ (JNIEnv* env, jclass)
+{
+ HAL_WaitForDSData();
+}
+
+/*
+ * Class: edu_wpi_first_hal_HAL
+ * Method: releaseDSMutex
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_HAL_releaseDSMutex
+ (JNIEnv* env, jclass)
+{
+ HAL_ReleaseDSMutex();
+}
+
+/*
+ * Class: edu_wpi_first_hal_HAL
+ * Method: waitForDSDataTimeout
+ * Signature: (D)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_HAL_waitForDSDataTimeout
+ (JNIEnv*, jclass, jdouble timeout)
+{
+ return static_cast<jboolean>(HAL_WaitForDSDataTimeout(timeout));
+}
+
+/*
+ * Class: edu_wpi_first_hal_HAL
+ * Method: getMatchTime
+ * Signature: ()D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_HAL_getMatchTime
+ (JNIEnv* env, jclass)
+{
+ int32_t status = 0;
+ return HAL_GetMatchTime(&status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_HAL
+ * Method: getSystemActive
+ * Signature: ()Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_HAL_getSystemActive
+ (JNIEnv* env, jclass)
+{
+ int32_t status = 0;
+ bool val = HAL_GetSystemActive(&status);
+ CheckStatus(env, status);
+ return val;
+}
+
+/*
+ * Class: edu_wpi_first_hal_HAL
+ * Method: getBrownedOut
+ * Signature: ()Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_HAL_getBrownedOut
+ (JNIEnv* env, jclass)
+{
+ int32_t status = 0;
+ bool val = HAL_GetBrownedOut(&status);
+ CheckStatus(env, status);
+ return val;
+}
+
+/*
+ * Class: edu_wpi_first_hal_HAL
+ * Method: getMatchInfo
+ * Signature: (Ljava/lang/Object;)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_HAL_getMatchInfo
+ (JNIEnv* env, jclass, jobject info)
+{
+ HAL_MatchInfo matchInfo;
+ auto status = HAL_GetMatchInfo(&matchInfo);
+ if (status == 0) {
+ SetMatchInfoObject(env, info, matchInfo);
+ }
+ return status;
+}
+
+/*
+ * Class: edu_wpi_first_hal_HAL
+ * Method: sendError
+ * Signature: (ZIZLjava/lang/String;Ljava/lang/String;Ljava/lang/String;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_HAL_sendError
+ (JNIEnv* env, jclass, jboolean isError, jint errorCode, jboolean isLVCode,
+ jstring details, jstring location, jstring callStack, jboolean printMsg)
+{
+ JStringRef detailsStr{env, details};
+ JStringRef locationStr{env, location};
+ JStringRef callStackStr{env, callStack};
+
+ jint returnValue =
+ HAL_SendError(isError, errorCode, isLVCode, detailsStr.c_str(),
+ locationStr.c_str(), callStackStr.c_str(), printMsg);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_HAL
+ * Method: getPortWithModule
+ * Signature: (BB)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_HAL_getPortWithModule
+ (JNIEnv* env, jclass, jbyte module, jbyte channel)
+{
+ HAL_PortHandle port = HAL_GetPortWithModule(module, channel);
+ return (jint)port;
+}
+
+/*
+ * Class: edu_wpi_first_hal_HAL
+ * Method: getPort
+ * Signature: (B)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_HAL_getPort
+ (JNIEnv* env, jclass, jbyte channel)
+{
+ HAL_PortHandle port = HAL_GetPort(channel);
+ return (jint)port;
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/cpp/jni/HALUtil.cpp b/hal/src/main/native/cpp/jni/HALUtil.cpp
new file mode 100644
index 0000000..26b7919
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/HALUtil.cpp
@@ -0,0 +1,469 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 "HALUtil.h"
+
+#include <jni.h>
+
+#include <cassert>
+#include <cerrno>
+#include <cstdio>
+#include <cstring>
+#include <string>
+
+#include <wpi/SmallString.h>
+#include <wpi/jni_util.h>
+#include <wpi/raw_ostream.h>
+
+#include "edu_wpi_first_hal_HALUtil.h"
+#include "hal/CAN.h"
+#include "hal/DriverStation.h"
+#include "hal/Errors.h"
+#include "hal/HAL.h"
+
+using namespace wpi::java;
+
+#define kRioStatusOffset -63000
+#define kRioStatusSuccess 0
+#define kRIOStatusBufferInvalidSize (kRioStatusOffset - 80)
+#define kRIOStatusOperationTimedOut -52007
+#define kRIOStatusFeatureNotSupported (kRioStatusOffset - 193)
+#define kRIOStatusResourceNotInitialized -52010
+
+static JavaVM* jvm = nullptr;
+static JException illegalArgExCls;
+static JException boundaryExCls;
+static JException allocationExCls;
+static JException halHandleExCls;
+static JException canInvalidBufferExCls;
+static JException canMessageNotFoundExCls;
+static JException canMessageNotAllowedExCls;
+static JException canNotInitializedExCls;
+static JException uncleanStatusExCls;
+static JClass pwmConfigDataResultCls;
+static JClass canStatusCls;
+static JClass matchInfoDataCls;
+static JClass accumulatorResultCls;
+static JClass canDataCls;
+static JClass halValueCls;
+
+static const JClassInit classes[] = {
+ {"edu/wpi/first/hal/PWMConfigDataResult", &pwmConfigDataResultCls},
+ {"edu/wpi/first/hal/can/CANStatus", &canStatusCls},
+ {"edu/wpi/first/hal/MatchInfoData", &matchInfoDataCls},
+ {"edu/wpi/first/hal/AccumulatorResult", &accumulatorResultCls},
+ {"edu/wpi/first/hal/CANData", &canDataCls},
+ {"edu/wpi/first/hal/HALValue", &halValueCls}};
+
+static const JExceptionInit exceptions[] = {
+ {"java/lang/IllegalArgumentException", &illegalArgExCls},
+ {"edu/wpi/first/hal/util/BoundaryException", &boundaryExCls},
+ {"edu/wpi/first/hal/util/AllocationException", &allocationExCls},
+ {"edu/wpi/first/hal/util/HalHandleException", &halHandleExCls},
+ {"edu/wpi/first/hal/can/CANInvalidBufferException", &canInvalidBufferExCls},
+ {"edu/wpi/first/hal/can/CANMessageNotFoundException",
+ &canMessageNotFoundExCls},
+ {"edu/wpi/first/hal/can/CANMessageNotAllowedException",
+ &canMessageNotAllowedExCls},
+ {"edu/wpi/first/hal/can/CANNotInitializedException",
+ &canNotInitializedExCls},
+ {"edu/wpi/first/hal/util/UncleanStatusException", &uncleanStatusExCls}};
+
+namespace frc {
+
+void ThrowUncleanStatusException(JNIEnv* env, wpi::StringRef msg,
+ int32_t status) {
+ static jmethodID func =
+ env->GetMethodID(uncleanStatusExCls, "<init>", "(ILjava/lang/String;)V");
+
+ jobject exception =
+ env->NewObject(uncleanStatusExCls, func, static_cast<jint>(status),
+ MakeJString(env, msg));
+ env->Throw(static_cast<jthrowable>(exception));
+}
+
+void ThrowAllocationException(JNIEnv* env, int32_t minRange, int32_t maxRange,
+ int32_t requestedValue, int32_t status) {
+ const char* message = HAL_GetErrorMessage(status);
+ wpi::SmallString<1024> buf;
+ wpi::raw_svector_ostream oss(buf);
+ oss << " Code: " << status << ". " << message
+ << ", Minimum Value: " << minRange << ", Maximum Value: " << maxRange
+ << ", Requested Value: " << requestedValue;
+ env->ThrowNew(allocationExCls, buf.c_str());
+ allocationExCls.Throw(env, buf.c_str());
+}
+
+void ThrowHalHandleException(JNIEnv* env, int32_t status) {
+ const char* message = HAL_GetErrorMessage(status);
+ wpi::SmallString<1024> buf;
+ wpi::raw_svector_ostream oss(buf);
+ oss << " Code: " << status << ". " << message;
+ halHandleExCls.Throw(env, buf.c_str());
+}
+
+void ReportError(JNIEnv* env, int32_t status, bool doThrow) {
+ if (status == 0) return;
+ if (status == HAL_HANDLE_ERROR) {
+ ThrowHalHandleException(env, status);
+ }
+ const char* message = HAL_GetErrorMessage(status);
+ if (doThrow && status < 0) {
+ wpi::SmallString<1024> buf;
+ wpi::raw_svector_ostream oss(buf);
+ oss << " Code: " << status << ". " << message;
+ ThrowUncleanStatusException(env, buf.c_str(), status);
+ } else {
+ std::string func;
+ auto stack = GetJavaStackTrace(env, &func, "edu.wpi.first");
+ HAL_SendError(1, status, 0, message, func.c_str(), stack.c_str(), 1);
+ }
+}
+
+void ThrowError(JNIEnv* env, int32_t status, int32_t minRange, int32_t maxRange,
+ int32_t requestedValue) {
+ if (status == 0) return;
+ if (status == NO_AVAILABLE_RESOURCES || status == RESOURCE_IS_ALLOCATED ||
+ status == RESOURCE_OUT_OF_RANGE) {
+ ThrowAllocationException(env, minRange, maxRange, requestedValue, status);
+ }
+ if (status == HAL_HANDLE_ERROR) {
+ ThrowHalHandleException(env, status);
+ }
+ const char* message = HAL_GetErrorMessage(status);
+ wpi::SmallString<1024> buf;
+ wpi::raw_svector_ostream oss(buf);
+ oss << " Code: " << status << ". " << message;
+ ThrowUncleanStatusException(env, buf.c_str(), status);
+}
+
+void ReportCANError(JNIEnv* env, int32_t status, int message_id) {
+ if (status >= 0) return;
+ switch (status) {
+ case kRioStatusSuccess:
+ // Everything is ok... don't throw.
+ break;
+ case HAL_ERR_CANSessionMux_InvalidBuffer:
+ case kRIOStatusBufferInvalidSize: {
+ static jmethodID invalidBufConstruct = nullptr;
+ if (!invalidBufConstruct)
+ invalidBufConstruct =
+ env->GetMethodID(canInvalidBufferExCls, "<init>", "()V");
+ jobject exception =
+ env->NewObject(canInvalidBufferExCls, invalidBufConstruct);
+ env->Throw(static_cast<jthrowable>(exception));
+ break;
+ }
+ case HAL_ERR_CANSessionMux_MessageNotFound:
+ case kRIOStatusOperationTimedOut: {
+ static jmethodID messageNotFoundConstruct = nullptr;
+ if (!messageNotFoundConstruct)
+ messageNotFoundConstruct =
+ env->GetMethodID(canMessageNotFoundExCls, "<init>", "()V");
+ jobject exception =
+ env->NewObject(canMessageNotFoundExCls, messageNotFoundConstruct);
+ env->Throw(static_cast<jthrowable>(exception));
+ break;
+ }
+ case HAL_ERR_CANSessionMux_NotAllowed:
+ case kRIOStatusFeatureNotSupported: {
+ wpi::SmallString<100> buf;
+ wpi::raw_svector_ostream oss(buf);
+ oss << "MessageID = " << message_id;
+ canMessageNotAllowedExCls.Throw(env, buf.c_str());
+ break;
+ }
+ case HAL_ERR_CANSessionMux_NotInitialized:
+ case kRIOStatusResourceNotInitialized: {
+ static jmethodID notInitConstruct = nullptr;
+ if (!notInitConstruct)
+ notInitConstruct =
+ env->GetMethodID(canNotInitializedExCls, "<init>", "()V");
+ jobject exception =
+ env->NewObject(canNotInitializedExCls, notInitConstruct);
+ env->Throw(static_cast<jthrowable>(exception));
+ break;
+ }
+ default: {
+ wpi::SmallString<100> buf;
+ wpi::raw_svector_ostream oss(buf);
+ oss << "Fatal status code detected: " << status;
+ uncleanStatusExCls.Throw(env, buf.c_str());
+ break;
+ }
+ }
+}
+
+void ThrowIllegalArgumentException(JNIEnv* env, wpi::StringRef msg) {
+ illegalArgExCls.Throw(env, msg);
+}
+
+void ThrowBoundaryException(JNIEnv* env, double value, double lower,
+ double upper) {
+ static jmethodID getMessage = nullptr;
+ if (!getMessage)
+ getMessage = env->GetStaticMethodID(boundaryExCls, "getMessage",
+ "(DDD)Ljava/lang/String;");
+
+ static jmethodID constructor = nullptr;
+ if (!constructor)
+ constructor =
+ env->GetMethodID(boundaryExCls, "<init>", "(Ljava/lang/String;)V");
+
+ jobject msg = env->CallStaticObjectMethod(
+ boundaryExCls, getMessage, static_cast<jdouble>(value),
+ static_cast<jdouble>(lower), static_cast<jdouble>(upper));
+ jobject ex = env->NewObject(boundaryExCls, constructor, msg);
+ env->Throw(static_cast<jthrowable>(ex));
+}
+
+jobject CreatePWMConfigDataResult(JNIEnv* env, int32_t maxPwm,
+ int32_t deadbandMaxPwm, int32_t centerPwm,
+ int32_t deadbandMinPwm, int32_t minPwm) {
+ static jmethodID constructor =
+ env->GetMethodID(pwmConfigDataResultCls, "<init>", "(IIIII)V");
+ return env->NewObject(pwmConfigDataResultCls, constructor, (jint)maxPwm,
+ (jint)deadbandMaxPwm, (jint)centerPwm,
+ (jint)deadbandMinPwm, (jint)minPwm);
+}
+
+void SetCanStatusObject(JNIEnv* env, jobject canStatus,
+ float percentBusUtilization, uint32_t busOffCount,
+ uint32_t txFullCount, uint32_t receiveErrorCount,
+ uint32_t transmitErrorCount) {
+ static jmethodID func =
+ env->GetMethodID(canStatusCls, "setStatus", "(DIIII)V");
+ env->CallVoidMethod(canStatus, func, (jdouble)percentBusUtilization,
+ (jint)busOffCount, (jint)txFullCount,
+ (jint)receiveErrorCount, (jint)transmitErrorCount);
+}
+
+void SetMatchInfoObject(JNIEnv* env, jobject matchStatus,
+ const HAL_MatchInfo& matchInfo) {
+ static jmethodID func =
+ env->GetMethodID(matchInfoDataCls, "setData",
+ "(Ljava/lang/String;Ljava/lang/String;III)V");
+
+ env->CallVoidMethod(
+ matchStatus, func, MakeJString(env, matchInfo.eventName),
+ MakeJString(env, wpi::StringRef{reinterpret_cast<const char*>(
+ matchInfo.gameSpecificMessage),
+ matchInfo.gameSpecificMessageSize}),
+ (jint)matchInfo.matchNumber, (jint)matchInfo.replayNumber,
+ (jint)matchInfo.matchType);
+}
+
+void SetAccumulatorResultObject(JNIEnv* env, jobject accumulatorResult,
+ int64_t value, int64_t count) {
+ static jmethodID func =
+ env->GetMethodID(accumulatorResultCls, "set", "(JJ)V");
+
+ env->CallVoidMethod(accumulatorResult, func, (jlong)value, (jlong)count);
+}
+
+jbyteArray SetCANDataObject(JNIEnv* env, jobject canData, int32_t length,
+ uint64_t timestamp) {
+ static jmethodID func = env->GetMethodID(canDataCls, "setData", "(IJ)[B");
+
+ jbyteArray retVal = static_cast<jbyteArray>(
+ env->CallObjectMethod(canData, func, (jint)length, (jlong)timestamp));
+ return retVal;
+}
+
+jobject CreateHALValue(JNIEnv* env, const HAL_Value& value) {
+ static jmethodID fromNative = env->GetStaticMethodID(
+ halValueCls, "fromNative", "(IJD)Ledu/wpi/first/hal/HALValue;");
+ jlong value1 = 0;
+ jdouble value2 = 0.0;
+ switch (value.type) {
+ case HAL_BOOLEAN:
+ value1 = value.data.v_boolean;
+ break;
+ case HAL_DOUBLE:
+ value2 = value.data.v_double;
+ break;
+ case HAL_ENUM:
+ value1 = value.data.v_enum;
+ break;
+ case HAL_INT:
+ value1 = value.data.v_int;
+ break;
+ case HAL_LONG:
+ value1 = value.data.v_long;
+ break;
+ default:
+ break;
+ }
+ return env->CallStaticObjectMethod(halValueCls, fromNative, (jint)value.type,
+ value1, value2);
+}
+
+JavaVM* GetJVM() { return jvm; }
+
+} // namespace frc
+
+namespace sim {
+jint SimOnLoad(JavaVM* vm, void* reserved);
+void SimOnUnload(JavaVM* vm, void* reserved);
+} // namespace sim
+
+using namespace frc;
+
+extern "C" {
+
+//
+// indicate JNI version support desired and load classes
+//
+JNIEXPORT jint JNICALL JNI_OnLoad(JavaVM* vm, void* reserved) {
+ jvm = vm;
+
+ JNIEnv* env;
+ if (vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6) != JNI_OK)
+ return JNI_ERR;
+
+ for (auto& c : classes) {
+ *c.cls = JClass(env, c.name);
+ if (!*c.cls) return JNI_ERR;
+ }
+
+ for (auto& c : exceptions) {
+ *c.cls = JException(env, c.name);
+ if (!*c.cls) return JNI_ERR;
+ }
+
+ return sim::SimOnLoad(vm, reserved);
+}
+
+JNIEXPORT void JNICALL JNI_OnUnload(JavaVM* vm, void* reserved) {
+ sim::SimOnUnload(vm, reserved);
+
+ JNIEnv* env;
+ if (vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6) != JNI_OK)
+ return;
+ // Delete global references
+
+ for (auto& c : classes) {
+ c.cls->free(env);
+ }
+ for (auto& c : exceptions) {
+ c.cls->free(env);
+ }
+ jvm = nullptr;
+}
+
+/*
+ * Class: edu_wpi_first_hal_HALUtil
+ * Method: getFPGAVersion
+ * Signature: ()S
+ */
+JNIEXPORT jshort JNICALL
+Java_edu_wpi_first_hal_HALUtil_getFPGAVersion
+ (JNIEnv* env, jclass)
+{
+ int32_t status = 0;
+ jshort returnValue = HAL_GetFPGAVersion(&status);
+ CheckStatus(env, status);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_HALUtil
+ * Method: getFPGARevision
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_HALUtil_getFPGARevision
+ (JNIEnv* env, jclass)
+{
+ int32_t status = 0;
+ jint returnValue = HAL_GetFPGARevision(&status);
+ CheckStatus(env, status);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_HALUtil
+ * Method: getFPGATime
+ * Signature: ()J
+ */
+JNIEXPORT jlong JNICALL
+Java_edu_wpi_first_hal_HALUtil_getFPGATime
+ (JNIEnv* env, jclass)
+{
+ int32_t status = 0;
+ jlong returnValue = HAL_GetFPGATime(&status);
+ CheckStatus(env, status);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_HALUtil
+ * Method: getHALRuntimeType
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_HALUtil_getHALRuntimeType
+ (JNIEnv* env, jclass)
+{
+ jint returnValue = HAL_GetRuntimeType();
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_HALUtil
+ * Method: getFPGAButton
+ * Signature: ()Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_HALUtil_getFPGAButton
+ (JNIEnv* env, jclass)
+{
+ int32_t status = 0;
+ jboolean returnValue = HAL_GetFPGAButton(&status);
+ CheckStatus(env, status);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_HALUtil
+ * Method: getHALErrorMessage
+ * Signature: (I)Ljava/lang/String;
+ */
+JNIEXPORT jstring JNICALL
+Java_edu_wpi_first_hal_HALUtil_getHALErrorMessage
+ (JNIEnv* paramEnv, jclass, jint paramId)
+{
+ const char* msg = HAL_GetErrorMessage(paramId);
+ return MakeJString(paramEnv, msg);
+}
+
+/*
+ * Class: edu_wpi_first_hal_HALUtil
+ * Method: getHALErrno
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_HALUtil_getHALErrno
+ (JNIEnv*, jclass)
+{
+ return errno;
+}
+
+/*
+ * Class: edu_wpi_first_hal_HALUtil
+ * Method: getHALstrerror
+ * Signature: (I)Ljava/lang/String;
+ */
+JNIEXPORT jstring JNICALL
+Java_edu_wpi_first_hal_HALUtil_getHALstrerror
+ (JNIEnv* env, jclass, jint errorCode)
+{
+ const char* msg = std::strerror(errno);
+ return MakeJString(env, msg);
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/cpp/jni/HALUtil.h b/hal/src/main/native/cpp/jni/HALUtil.h
new file mode 100644
index 0000000..c035f75
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/HALUtil.h
@@ -0,0 +1,77 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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. */
+/*----------------------------------------------------------------------------*/
+
+#ifndef HAL_HAL_SRC_MAIN_NATIVE_CPP_JNI_HALUTIL_H_
+#define HAL_HAL_SRC_MAIN_NATIVE_CPP_JNI_HALUTIL_H_
+
+#include <jni.h>
+#include <stdint.h>
+
+#include <wpi/StringRef.h>
+
+struct HAL_MatchInfo;
+struct HAL_Value;
+
+namespace frc {
+
+void ReportError(JNIEnv* env, int32_t status, bool doThrow = true);
+
+void ThrowError(JNIEnv* env, int32_t status, int32_t minRange, int32_t maxRange,
+ int32_t requestedValue);
+
+inline bool CheckStatus(JNIEnv* env, int32_t status, bool doThrow = true) {
+ if (status != 0) ReportError(env, status, doThrow);
+ return status == 0;
+}
+
+inline bool CheckStatusRange(JNIEnv* env, int32_t status, int32_t minRange,
+ int32_t maxRange, int32_t requestedValue) {
+ if (status != 0) ThrowError(env, status, minRange, maxRange, requestedValue);
+ return status == 0;
+}
+
+inline bool CheckStatusForceThrow(JNIEnv* env, int32_t status) {
+ if (status != 0) ThrowError(env, status, 0, 0, 0);
+ return status == 0;
+}
+
+void ReportCANError(JNIEnv* env, int32_t status, int32_t message_id);
+
+inline bool CheckCANStatus(JNIEnv* env, int32_t status, int32_t message_id) {
+ if (status != 0) ReportCANError(env, status, message_id);
+ return status == 0;
+}
+
+void ThrowIllegalArgumentException(JNIEnv* env, wpi::StringRef msg);
+void ThrowBoundaryException(JNIEnv* env, double value, double lower,
+ double upper);
+
+jobject CreatePWMConfigDataResult(JNIEnv* env, int32_t maxPwm,
+ int32_t deadbandMaxPwm, int32_t centerPwm,
+ int32_t deadbandMinPwm, int32_t minPwm);
+
+void SetCanStatusObject(JNIEnv* env, jobject canStatus,
+ float percentBusUtilization, uint32_t busOffCount,
+ uint32_t txFullCount, uint32_t receiveErrorCount,
+ uint32_t transmitErrorCount);
+
+void SetMatchInfoObject(JNIEnv* env, jobject matchStatus,
+ const HAL_MatchInfo& matchInfo);
+
+void SetAccumulatorResultObject(JNIEnv* env, jobject accumulatorResult,
+ int64_t value, int64_t count);
+
+jbyteArray SetCANDataObject(JNIEnv* env, jobject canData, int32_t length,
+ uint64_t timestamp);
+
+jobject CreateHALValue(JNIEnv* env, const HAL_Value& value);
+
+JavaVM* GetJVM();
+
+} // namespace frc
+
+#endif // HAL_HAL_SRC_MAIN_NATIVE_CPP_JNI_HALUTIL_H_
diff --git a/hal/src/main/native/cpp/jni/I2CJNI.cpp b/hal/src/main/native/cpp/jni/I2CJNI.cpp
new file mode 100644
index 0000000..7812bdb
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/I2CJNI.cpp
@@ -0,0 +1,169 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 <jni.h>
+
+#include <cassert>
+
+#include <wpi/jni_util.h>
+
+#include "HALUtil.h"
+#include "edu_wpi_first_hal_I2CJNI.h"
+#include "hal/I2C.h"
+
+using namespace frc;
+using namespace wpi::java;
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_hal_I2CJNI
+ * Method: i2CInitialize
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_I2CJNI_i2CInitialize
+ (JNIEnv* env, jclass, jint port)
+{
+ int32_t status = 0;
+ HAL_InitializeI2C(static_cast<HAL_I2CPort>(port), &status);
+ CheckStatusForceThrow(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_I2CJNI
+ * Method: i2CTransaction
+ * Signature: (IBLjava/lang/Object;BLjava/lang/Object;B)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_I2CJNI_i2CTransaction
+ (JNIEnv* env, jclass, jint port, jbyte address, jobject dataToSend,
+ jbyte sendSize, jobject dataReceived, jbyte receiveSize)
+{
+ uint8_t* dataToSendPtr = nullptr;
+ if (dataToSend != 0) {
+ dataToSendPtr =
+ reinterpret_cast<uint8_t*>(env->GetDirectBufferAddress(dataToSend));
+ }
+ uint8_t* dataReceivedPtr =
+ reinterpret_cast<uint8_t*>(env->GetDirectBufferAddress(dataReceived));
+ jint returnValue =
+ HAL_TransactionI2C(static_cast<HAL_I2CPort>(port), address, dataToSendPtr,
+ sendSize, dataReceivedPtr, receiveSize);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_I2CJNI
+ * Method: i2CTransactionB
+ * Signature: (IB[BB[BB)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_I2CJNI_i2CTransactionB
+ (JNIEnv* env, jclass, jint port, jbyte address, jbyteArray dataToSend,
+ jbyte sendSize, jbyteArray dataReceived, jbyte receiveSize)
+{
+ wpi::SmallVector<uint8_t, 128> recvBuf;
+ recvBuf.resize(receiveSize);
+ jint returnValue =
+ HAL_TransactionI2C(static_cast<HAL_I2CPort>(port), address,
+ reinterpret_cast<const uint8_t*>(
+ JByteArrayRef(env, dataToSend).array().data()),
+ sendSize, recvBuf.data(), receiveSize);
+ env->SetByteArrayRegion(dataReceived, 0, receiveSize,
+ reinterpret_cast<const jbyte*>(recvBuf.data()));
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_I2CJNI
+ * Method: i2CWrite
+ * Signature: (IBLjava/lang/Object;B)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_I2CJNI_i2CWrite
+ (JNIEnv* env, jclass, jint port, jbyte address, jobject dataToSend,
+ jbyte sendSize)
+{
+ uint8_t* dataToSendPtr = nullptr;
+
+ if (dataToSend != 0) {
+ dataToSendPtr =
+ reinterpret_cast<uint8_t*>(env->GetDirectBufferAddress(dataToSend));
+ }
+ jint returnValue = HAL_WriteI2C(static_cast<HAL_I2CPort>(port), address,
+ dataToSendPtr, sendSize);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_I2CJNI
+ * Method: i2CWriteB
+ * Signature: (IB[BB)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_I2CJNI_i2CWriteB
+ (JNIEnv* env, jclass, jint port, jbyte address, jbyteArray dataToSend,
+ jbyte sendSize)
+{
+ jint returnValue =
+ HAL_WriteI2C(static_cast<HAL_I2CPort>(port), address,
+ reinterpret_cast<const uint8_t*>(
+ JByteArrayRef(env, dataToSend).array().data()),
+ sendSize);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_I2CJNI
+ * Method: i2CRead
+ * Signature: (IBLjava/lang/Object;B)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_I2CJNI_i2CRead
+ (JNIEnv* env, jclass, jint port, jbyte address, jobject dataReceived,
+ jbyte receiveSize)
+{
+ uint8_t* dataReceivedPtr =
+ reinterpret_cast<uint8_t*>(env->GetDirectBufferAddress(dataReceived));
+ jint returnValue = HAL_ReadI2C(static_cast<HAL_I2CPort>(port), address,
+ dataReceivedPtr, receiveSize);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_I2CJNI
+ * Method: i2CReadB
+ * Signature: (IB[BB)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_I2CJNI_i2CReadB
+ (JNIEnv* env, jclass, jint port, jbyte address, jbyteArray dataReceived,
+ jbyte receiveSize)
+{
+ wpi::SmallVector<uint8_t, 128> recvBuf;
+ recvBuf.resize(receiveSize);
+ jint returnValue = HAL_ReadI2C(static_cast<HAL_I2CPort>(port), address,
+ recvBuf.data(), receiveSize);
+ env->SetByteArrayRegion(dataReceived, 0, receiveSize,
+ reinterpret_cast<const jbyte*>(recvBuf.data()));
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_I2CJNI
+ * Method: i2CClose
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_I2CJNI_i2CClose
+ (JNIEnv*, jclass, jint port)
+{
+ HAL_CloseI2C(static_cast<HAL_I2CPort>(port));
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/cpp/jni/InterruptJNI.cpp b/hal/src/main/native/cpp/jni/InterruptJNI.cpp
new file mode 100644
index 0000000..e3a783d
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/InterruptJNI.cpp
@@ -0,0 +1,297 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 <jni.h>
+
+#include <atomic>
+#include <cassert>
+#include <thread>
+
+#include <wpi/SafeThread.h>
+#include <wpi/mutex.h>
+
+#include "HALUtil.h"
+#include "edu_wpi_first_hal_InterruptJNI.h"
+#include "hal/Interrupts.h"
+
+using namespace frc;
+
+// Thread where callbacks are actually performed.
+//
+// JNI's AttachCurrentThread() creates a Java Thread object on every
+// invocation, which is both time inefficient and causes issues with Eclipse
+// (which tries to keep a thread list up-to-date and thus gets swamped).
+//
+// Instead, this class attaches just once. When a hardware notification
+// occurs, a condition variable wakes up this thread and this thread actually
+// makes the call into Java.
+//
+// We don't want to use a FIFO here. If the user code takes too long to
+// process, we will just ignore the redundant wakeup.
+class InterruptThreadJNI : public wpi::SafeThread {
+ public:
+ void Main();
+
+ bool m_notify = false;
+ uint32_t m_mask = 0;
+ jobject m_func = nullptr;
+ jmethodID m_mid;
+ jobject m_param = nullptr;
+};
+
+class InterruptJNI : public wpi::SafeThreadOwner<InterruptThreadJNI> {
+ public:
+ void SetFunc(JNIEnv* env, jobject func, jmethodID mid, jobject param);
+
+ void Notify(uint32_t mask) {
+ auto thr = GetThread();
+ if (!thr) return;
+ thr->m_notify = true;
+ thr->m_mask = mask;
+ thr->m_cond.notify_one();
+ }
+};
+
+void InterruptJNI::SetFunc(JNIEnv* env, jobject func, jmethodID mid,
+ jobject param) {
+ auto thr = GetThread();
+ if (!thr) return;
+ // free global references
+ if (thr->m_func) env->DeleteGlobalRef(thr->m_func);
+ if (thr->m_param) env->DeleteGlobalRef(thr->m_param);
+ // create global references
+ thr->m_func = env->NewGlobalRef(func);
+ thr->m_param = param ? env->NewGlobalRef(param) : nullptr;
+ thr->m_mid = mid;
+}
+
+void InterruptThreadJNI::Main() {
+ JNIEnv* env;
+ JavaVMAttachArgs args;
+ args.version = JNI_VERSION_1_2;
+ args.name = const_cast<char*>("Interrupt");
+ args.group = nullptr;
+ jint rs = GetJVM()->AttachCurrentThreadAsDaemon(
+ reinterpret_cast<void**>(&env), &args);
+ if (rs != JNI_OK) return;
+
+ std::unique_lock lock(m_mutex);
+ while (m_active) {
+ m_cond.wait(lock, [&] { return !m_active || m_notify; });
+ if (!m_active) break;
+ m_notify = false;
+ if (!m_func) continue;
+ jobject func = m_func;
+ jmethodID mid = m_mid;
+ uint32_t mask = m_mask;
+ jobject param = m_param;
+ lock.unlock(); // don't hold mutex during callback execution
+ env->CallVoidMethod(func, mid, static_cast<jint>(mask), param);
+ if (env->ExceptionCheck()) {
+ env->ExceptionDescribe();
+ env->ExceptionClear();
+ }
+ lock.lock();
+ }
+
+ // free global references
+ if (m_func) env->DeleteGlobalRef(m_func);
+ if (m_param) env->DeleteGlobalRef(m_param);
+
+ GetJVM()->DetachCurrentThread();
+}
+
+void interruptHandler(uint32_t mask, void* param) {
+ static_cast<InterruptJNI*>(param)->Notify(mask);
+}
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_hal_InterruptJNI
+ * Method: initializeInterrupts
+ * Signature: (Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_InterruptJNI_initializeInterrupts
+ (JNIEnv* env, jclass, jboolean watcher)
+{
+ int32_t status = 0;
+ HAL_InterruptHandle interrupt = HAL_InitializeInterrupts(watcher, &status);
+
+ CheckStatusForceThrow(env, status);
+ return (jint)interrupt;
+}
+
+/*
+ * Class: edu_wpi_first_hal_InterruptJNI
+ * Method: cleanInterrupts
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_InterruptJNI_cleanInterrupts
+ (JNIEnv* env, jclass, jint interruptHandle)
+{
+ int32_t status = 0;
+ auto param =
+ HAL_CleanInterrupts((HAL_InterruptHandle)interruptHandle, &status);
+ if (param) {
+ delete static_cast<InterruptJNI*>(param);
+ }
+
+ // ignore status, as an invalid handle just needs to be ignored.
+}
+
+/*
+ * Class: edu_wpi_first_hal_InterruptJNI
+ * Method: waitForInterrupt
+ * Signature: (IDZ)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_InterruptJNI_waitForInterrupt
+ (JNIEnv* env, jclass, jint interruptHandle, jdouble timeout,
+ jboolean ignorePrevious)
+{
+ int32_t status = 0;
+ int32_t result = HAL_WaitForInterrupt((HAL_InterruptHandle)interruptHandle,
+ timeout, ignorePrevious, &status);
+
+ CheckStatus(env, status);
+ return result;
+}
+
+/*
+ * Class: edu_wpi_first_hal_InterruptJNI
+ * Method: enableInterrupts
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_InterruptJNI_enableInterrupts
+ (JNIEnv* env, jclass, jint interruptHandle)
+{
+ int32_t status = 0;
+ HAL_EnableInterrupts((HAL_InterruptHandle)interruptHandle, &status);
+
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_InterruptJNI
+ * Method: disableInterrupts
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_InterruptJNI_disableInterrupts
+ (JNIEnv* env, jclass, jint interruptHandle)
+{
+ int32_t status = 0;
+ HAL_DisableInterrupts((HAL_InterruptHandle)interruptHandle, &status);
+
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_InterruptJNI
+ * Method: readInterruptRisingTimestamp
+ * Signature: (I)J
+ */
+JNIEXPORT jlong JNICALL
+Java_edu_wpi_first_hal_InterruptJNI_readInterruptRisingTimestamp
+ (JNIEnv* env, jclass, jint interruptHandle)
+{
+ int32_t status = 0;
+ jlong timeStamp = HAL_ReadInterruptRisingTimestamp(
+ (HAL_InterruptHandle)interruptHandle, &status);
+
+ CheckStatus(env, status);
+ return timeStamp;
+}
+
+/*
+ * Class: edu_wpi_first_hal_InterruptJNI
+ * Method: readInterruptFallingTimestamp
+ * Signature: (I)J
+ */
+JNIEXPORT jlong JNICALL
+Java_edu_wpi_first_hal_InterruptJNI_readInterruptFallingTimestamp
+ (JNIEnv* env, jclass, jint interruptHandle)
+{
+ int32_t status = 0;
+ jlong timeStamp = HAL_ReadInterruptFallingTimestamp(
+ (HAL_InterruptHandle)interruptHandle, &status);
+
+ CheckStatus(env, status);
+ return timeStamp;
+}
+
+/*
+ * Class: edu_wpi_first_hal_InterruptJNI
+ * Method: requestInterrupts
+ * Signature: (III)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_InterruptJNI_requestInterrupts
+ (JNIEnv* env, jclass, jint interruptHandle, jint digitalSourceHandle,
+ jint analogTriggerType)
+{
+ int32_t status = 0;
+ HAL_RequestInterrupts((HAL_InterruptHandle)interruptHandle,
+ (HAL_Handle)digitalSourceHandle,
+ (HAL_AnalogTriggerType)analogTriggerType, &status);
+
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_InterruptJNI
+ * Method: attachInterruptHandler
+ * Signature: (ILjava/lang/Object;Ljava/lang/Object;)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_InterruptJNI_attachInterruptHandler
+ (JNIEnv* env, jclass, jint interruptHandle, jobject handler, jobject param)
+{
+ jclass cls = env->GetObjectClass(handler);
+ if (cls == 0) {
+ assert(false);
+ return;
+ }
+ jmethodID mid = env->GetMethodID(cls, "apply", "(ILjava/lang/Object;)V");
+ if (mid == 0) {
+ assert(false);
+ return;
+ }
+
+ InterruptJNI* intr = new InterruptJNI;
+ intr->Start();
+ intr->SetFunc(env, handler, mid, param);
+
+ int32_t status = 0;
+ HAL_AttachInterruptHandler((HAL_InterruptHandle)interruptHandle,
+ interruptHandler, intr, &status);
+
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_InterruptJNI
+ * Method: setInterruptUpSourceEdge
+ * Signature: (IZZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_InterruptJNI_setInterruptUpSourceEdge
+ (JNIEnv* env, jclass, jint interruptHandle, jboolean risingEdge,
+ jboolean fallingEdge)
+{
+ int32_t status = 0;
+ HAL_SetInterruptUpSourceEdge((HAL_InterruptHandle)interruptHandle, risingEdge,
+ fallingEdge, &status);
+
+ CheckStatus(env, status);
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/cpp/jni/NotifierJNI.cpp b/hal/src/main/native/cpp/jni/NotifierJNI.cpp
new file mode 100644
index 0000000..874d750
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/NotifierJNI.cpp
@@ -0,0 +1,132 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 <jni.h>
+
+#include <cassert>
+#include <cstdio>
+
+#include <wpi/jni_util.h>
+
+#include "HALUtil.h"
+#include "edu_wpi_first_hal_NotifierJNI.h"
+#include "hal/Notifier.h"
+
+using namespace frc;
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_hal_NotifierJNI
+ * Method: initializeNotifier
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_NotifierJNI_initializeNotifier
+ (JNIEnv* env, jclass)
+{
+ int32_t status = 0;
+ HAL_NotifierHandle notifierHandle = HAL_InitializeNotifier(&status);
+
+ if (notifierHandle <= 0 || !CheckStatusForceThrow(env, status)) {
+ return 0; // something went wrong in HAL
+ }
+
+ return (jint)notifierHandle;
+}
+
+/*
+ * Class: edu_wpi_first_hal_NotifierJNI
+ * Method: setNotifierName
+ * Signature: (ILjava/lang/String;)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_NotifierJNI_setNotifierName
+ (JNIEnv* env, jclass cls, jint notifierHandle, jstring name)
+{
+ int32_t status = 0;
+ HAL_SetNotifierName((HAL_NotifierHandle)notifierHandle,
+ wpi::java::JStringRef{env, name}.c_str(), &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_NotifierJNI
+ * Method: stopNotifier
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_NotifierJNI_stopNotifier
+ (JNIEnv* env, jclass cls, jint notifierHandle)
+{
+ int32_t status = 0;
+ HAL_StopNotifier((HAL_NotifierHandle)notifierHandle, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_NotifierJNI
+ * Method: cleanNotifier
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_NotifierJNI_cleanNotifier
+ (JNIEnv* env, jclass, jint notifierHandle)
+{
+ int32_t status = 0;
+ HAL_CleanNotifier((HAL_NotifierHandle)notifierHandle, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_NotifierJNI
+ * Method: updateNotifierAlarm
+ * Signature: (IJ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_NotifierJNI_updateNotifierAlarm
+ (JNIEnv* env, jclass cls, jint notifierHandle, jlong triggerTime)
+{
+ int32_t status = 0;
+ HAL_UpdateNotifierAlarm((HAL_NotifierHandle)notifierHandle,
+ static_cast<uint64_t>(triggerTime), &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_NotifierJNI
+ * Method: cancelNotifierAlarm
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_NotifierJNI_cancelNotifierAlarm
+ (JNIEnv* env, jclass cls, jint notifierHandle)
+{
+ int32_t status = 0;
+ HAL_CancelNotifierAlarm((HAL_NotifierHandle)notifierHandle, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_NotifierJNI
+ * Method: waitForNotifierAlarm
+ * Signature: (I)J
+ */
+JNIEXPORT jlong JNICALL
+Java_edu_wpi_first_hal_NotifierJNI_waitForNotifierAlarm
+ (JNIEnv* env, jclass cls, jint notifierHandle)
+{
+ int32_t status = 0;
+ uint64_t time =
+ HAL_WaitForNotifierAlarm((HAL_NotifierHandle)notifierHandle, &status);
+
+ CheckStatus(env, status);
+
+ return (jlong)time;
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/cpp/jni/PDPJNI.cpp b/hal/src/main/native/cpp/jni/PDPJNI.cpp
new file mode 100644
index 0000000..f649a8d
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/PDPJNI.cpp
@@ -0,0 +1,193 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 "HALUtil.h"
+#include "edu_wpi_first_hal_PDPJNI.h"
+#include "hal/PDP.h"
+#include "hal/Ports.h"
+
+using namespace frc;
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_hal_PDPJNI
+ * Method: initializePDP
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_PDPJNI_initializePDP
+ (JNIEnv* env, jclass, jint module)
+{
+ int32_t status = 0;
+ auto handle = HAL_InitializePDP(module, &status);
+ CheckStatusRange(env, status, 0, HAL_GetNumPDPModules(), module);
+ return static_cast<jint>(handle);
+}
+
+/*
+ * Class: edu_wpi_first_hal_PDPJNI
+ * Method: checkPDPChannel
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_PDPJNI_checkPDPChannel
+ (JNIEnv* env, jclass, jint channel)
+{
+ return HAL_CheckPDPChannel(channel);
+}
+
+/*
+ * Class: edu_wpi_first_hal_PDPJNI
+ * Method: checkPDPModule
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_PDPJNI_checkPDPModule
+ (JNIEnv* env, jclass, jint module)
+{
+ return HAL_CheckPDPModule(module);
+}
+
+/*
+ * Class: edu_wpi_first_hal_PDPJNI
+ * Method: getPDPTemperature
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_PDPJNI_getPDPTemperature
+ (JNIEnv* env, jclass, jint handle)
+{
+ int32_t status = 0;
+ double temperature = HAL_GetPDPTemperature(handle, &status);
+ CheckStatus(env, status, false);
+ return temperature;
+}
+
+/*
+ * Class: edu_wpi_first_hal_PDPJNI
+ * Method: getPDPVoltage
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_PDPJNI_getPDPVoltage
+ (JNIEnv* env, jclass, jint handle)
+{
+ int32_t status = 0;
+ double voltage = HAL_GetPDPVoltage(handle, &status);
+ CheckStatus(env, status, false);
+ return voltage;
+}
+
+/*
+ * Class: edu_wpi_first_hal_PDPJNI
+ * Method: getPDPChannelCurrent
+ * Signature: (BI)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_PDPJNI_getPDPChannelCurrent
+ (JNIEnv* env, jclass, jbyte channel, jint handle)
+{
+ int32_t status = 0;
+ double current = HAL_GetPDPChannelCurrent(handle, channel, &status);
+ CheckStatus(env, status, false);
+ return current;
+}
+
+/*
+ * Class: edu_wpi_first_hal_PDPJNI
+ * Method: getPDPAllCurrents
+ * Signature: (I[D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_PDPJNI_getPDPAllCurrents
+ (JNIEnv* env, jclass, jint handle, jdoubleArray jarr)
+{
+ double storage[16];
+ int32_t status = 0;
+ HAL_GetPDPAllChannelCurrents(handle, storage, &status);
+ if (!CheckStatus(env, status, false)) {
+ return;
+ }
+
+ env->SetDoubleArrayRegion(jarr, 0, 16, storage);
+}
+
+/*
+ * Class: edu_wpi_first_hal_PDPJNI
+ * Method: getPDPTotalCurrent
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_PDPJNI_getPDPTotalCurrent
+ (JNIEnv* env, jclass, jint handle)
+{
+ int32_t status = 0;
+ double current = HAL_GetPDPTotalCurrent(handle, &status);
+ CheckStatus(env, status, false);
+ return current;
+}
+
+/*
+ * Class: edu_wpi_first_hal_PDPJNI
+ * Method: getPDPTotalPower
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_PDPJNI_getPDPTotalPower
+ (JNIEnv* env, jclass, jint handle)
+{
+ int32_t status = 0;
+ double power = HAL_GetPDPTotalPower(handle, &status);
+ CheckStatus(env, status, false);
+ return power;
+}
+
+/*
+ * Class: edu_wpi_first_hal_PDPJNI
+ * Method: getPDPTotalEnergy
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_PDPJNI_getPDPTotalEnergy
+ (JNIEnv* env, jclass, jint handle)
+{
+ int32_t status = 0;
+ double energy = HAL_GetPDPTotalEnergy(handle, &status);
+ CheckStatus(env, status, false);
+ return energy;
+}
+
+/*
+ * Class: edu_wpi_first_hal_PDPJNI
+ * Method: resetPDPTotalEnergy
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_PDPJNI_resetPDPTotalEnergy
+ (JNIEnv* env, jclass, jint handle)
+{
+ int32_t status = 0;
+ HAL_ResetPDPTotalEnergy(handle, &status);
+ CheckStatus(env, status, false);
+}
+
+/*
+ * Class: edu_wpi_first_hal_PDPJNI
+ * Method: clearPDPStickyFaults
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_PDPJNI_clearPDPStickyFaults
+ (JNIEnv* env, jclass, jint handle)
+{
+ int32_t status = 0;
+ HAL_ClearPDPStickyFaults(handle, &status);
+ CheckStatus(env, status, false);
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/cpp/jni/PWMJNI.cpp b/hal/src/main/native/cpp/jni/PWMJNI.cpp
new file mode 100644
index 0000000..80293c4
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/PWMJNI.cpp
@@ -0,0 +1,277 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 <jni.h>
+
+#include <cassert>
+
+#include "HALUtil.h"
+#include "edu_wpi_first_hal_PWMJNI.h"
+#include "hal/DIO.h"
+#include "hal/PWM.h"
+#include "hal/Ports.h"
+#include "hal/handles/HandlesInternal.h"
+
+using namespace frc;
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_hal_PWMJNI
+ * Method: initializePWMPort
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_PWMJNI_initializePWMPort
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ auto pwm = HAL_InitializePWMPort((HAL_PortHandle)id, &status);
+ CheckStatusRange(env, status, 0, HAL_GetNumPWMChannels(),
+ hal::getPortHandleChannel((HAL_PortHandle)id));
+ return (jint)pwm;
+}
+
+/*
+ * Class: edu_wpi_first_hal_PWMJNI
+ * Method: checkPWMChannel
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_PWMJNI_checkPWMChannel
+ (JNIEnv* env, jclass, jint channel)
+{
+ return HAL_CheckPWMChannel(channel);
+}
+
+/*
+ * Class: edu_wpi_first_hal_PWMJNI
+ * Method: freePWMPort
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_PWMJNI_freePWMPort
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ HAL_FreePWMPort((HAL_DigitalHandle)id, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_PWMJNI
+ * Method: setPWMConfigRaw
+ * Signature: (IIIIII)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_PWMJNI_setPWMConfigRaw
+ (JNIEnv* env, jclass, jint id, jint maxPwm, jint deadbandMaxPwm,
+ jint centerPwm, jint deadbandMinPwm, jint minPwm)
+{
+ int32_t status = 0;
+ HAL_SetPWMConfigRaw((HAL_DigitalHandle)id, maxPwm, deadbandMaxPwm, centerPwm,
+ deadbandMinPwm, minPwm, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_PWMJNI
+ * Method: setPWMConfig
+ * Signature: (IDDDDD)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_PWMJNI_setPWMConfig
+ (JNIEnv* env, jclass, jint id, jdouble maxPwm, jdouble deadbandMaxPwm,
+ jdouble centerPwm, jdouble deadbandMinPwm, jdouble minPwm)
+{
+ int32_t status = 0;
+ HAL_SetPWMConfig((HAL_DigitalHandle)id, maxPwm, deadbandMaxPwm, centerPwm,
+ deadbandMinPwm, minPwm, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_PWMJNI
+ * Method: getPWMConfigRaw
+ * Signature: (I)Ljava/lang/Object;
+ */
+JNIEXPORT jobject JNICALL
+Java_edu_wpi_first_hal_PWMJNI_getPWMConfigRaw
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ int32_t maxPwm = 0;
+ int32_t deadbandMaxPwm = 0;
+ int32_t centerPwm = 0;
+ int32_t deadbandMinPwm = 0;
+ int32_t minPwm = 0;
+ HAL_GetPWMConfigRaw((HAL_DigitalHandle)id, &maxPwm, &deadbandMaxPwm,
+ ¢erPwm, &deadbandMinPwm, &minPwm, &status);
+ CheckStatus(env, status);
+ return CreatePWMConfigDataResult(env, maxPwm, deadbandMaxPwm, centerPwm,
+ deadbandMinPwm, minPwm);
+}
+
+/*
+ * Class: edu_wpi_first_hal_PWMJNI
+ * Method: setPWMEliminateDeadband
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_PWMJNI_setPWMEliminateDeadband
+ (JNIEnv* env, jclass, jint id, jboolean value)
+{
+ int32_t status = 0;
+ HAL_SetPWMEliminateDeadband((HAL_DigitalHandle)id, value, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_PWMJNI
+ * Method: getPWMEliminateDeadband
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_PWMJNI_getPWMEliminateDeadband
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ auto val = HAL_GetPWMEliminateDeadband((HAL_DigitalHandle)id, &status);
+ CheckStatus(env, status);
+ return (jboolean)val;
+}
+
+/*
+ * Class: edu_wpi_first_hal_PWMJNI
+ * Method: setPWMRaw
+ * Signature: (IS)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_PWMJNI_setPWMRaw
+ (JNIEnv* env, jclass, jint id, jshort value)
+{
+ int32_t status = 0;
+ HAL_SetPWMRaw((HAL_DigitalHandle)id, value, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_PWMJNI
+ * Method: setPWMSpeed
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_PWMJNI_setPWMSpeed
+ (JNIEnv* env, jclass, jint id, jdouble value)
+{
+ int32_t status = 0;
+ HAL_SetPWMSpeed((HAL_DigitalHandle)id, value, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_PWMJNI
+ * Method: setPWMPosition
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_PWMJNI_setPWMPosition
+ (JNIEnv* env, jclass, jint id, jdouble value)
+{
+ int32_t status = 0;
+ HAL_SetPWMPosition((HAL_DigitalHandle)id, value, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_PWMJNI
+ * Method: getPWMRaw
+ * Signature: (I)S
+ */
+JNIEXPORT jshort JNICALL
+Java_edu_wpi_first_hal_PWMJNI_getPWMRaw
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ jshort returnValue = HAL_GetPWMRaw((HAL_DigitalHandle)id, &status);
+ CheckStatus(env, status);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_PWMJNI
+ * Method: getPWMSpeed
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_PWMJNI_getPWMSpeed
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ jdouble returnValue = HAL_GetPWMSpeed((HAL_DigitalHandle)id, &status);
+ CheckStatus(env, status);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_PWMJNI
+ * Method: getPWMPosition
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_PWMJNI_getPWMPosition
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ jdouble returnValue = HAL_GetPWMPosition((HAL_DigitalHandle)id, &status);
+ CheckStatus(env, status);
+ return returnValue;
+}
+
+/*
+ * Class: edu_wpi_first_hal_PWMJNI
+ * Method: setPWMDisabled
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_PWMJNI_setPWMDisabled
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ HAL_SetPWMDisabled((HAL_DigitalHandle)id, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_PWMJNI
+ * Method: latchPWMZero
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_PWMJNI_latchPWMZero
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ HAL_LatchPWMZero((HAL_DigitalHandle)id, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_PWMJNI
+ * Method: setPWMPeriodScale
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_PWMJNI_setPWMPeriodScale
+ (JNIEnv* env, jclass, jint id, jint value)
+{
+ int32_t status = 0;
+ HAL_SetPWMPeriodScale((HAL_DigitalHandle)id, value, &status);
+ CheckStatus(env, status);
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/cpp/jni/PortsJNI.cpp b/hal/src/main/native/cpp/jni/PortsJNI.cpp
new file mode 100644
index 0000000..1adb7fb
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/PortsJNI.cpp
@@ -0,0 +1,252 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 <jni.h>
+
+#include <cassert>
+
+#include "HALUtil.h"
+#include "edu_wpi_first_hal_PortsJNI.h"
+#include "hal/Ports.h"
+
+using namespace frc;
+
+extern "C" {
+/*
+ * Class: edu_wpi_first_hal_PortsJNI
+ * Method: getNumAccumulators
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_PortsJNI_getNumAccumulators
+ (JNIEnv* env, jclass)
+{
+ jint value = HAL_GetNumAccumulators();
+ return value;
+}
+
+/*
+ * Class: edu_wpi_first_hal_PortsJNI
+ * Method: getNumAnalogTriggers
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_PortsJNI_getNumAnalogTriggers
+ (JNIEnv* env, jclass)
+{
+ jint value = HAL_GetNumAnalogTriggers();
+ return value;
+}
+
+/*
+ * Class: edu_wpi_first_hal_PortsJNI
+ * Method: getNumAnalogInputs
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_PortsJNI_getNumAnalogInputs
+ (JNIEnv* env, jclass)
+{
+ jint value = HAL_GetNumAnalogInputs();
+ return value;
+}
+
+/*
+ * Class: edu_wpi_first_hal_PortsJNI
+ * Method: getNumAnalogOutputs
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_PortsJNI_getNumAnalogOutputs
+ (JNIEnv* env, jclass)
+{
+ jint value = HAL_GetNumAnalogOutputs();
+ return value;
+}
+
+/*
+ * Class: edu_wpi_first_hal_PortsJNI
+ * Method: getNumCounters
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_PortsJNI_getNumCounters
+ (JNIEnv* env, jclass)
+{
+ jint value = HAL_GetNumCounters();
+ return value;
+}
+
+/*
+ * Class: edu_wpi_first_hal_PortsJNI
+ * Method: getNumDigitalHeaders
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_PortsJNI_getNumDigitalHeaders
+ (JNIEnv* env, jclass)
+{
+ jint value = HAL_GetNumDigitalHeaders();
+ return value;
+}
+
+/*
+ * Class: edu_wpi_first_hal_PortsJNI
+ * Method: getNumPWMHeaders
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_PortsJNI_getNumPWMHeaders
+ (JNIEnv* env, jclass)
+{
+ jint value = HAL_GetNumPWMHeaders();
+ return value;
+}
+
+/*
+ * Class: edu_wpi_first_hal_PortsJNI
+ * Method: getNumDigitalChannels
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_PortsJNI_getNumDigitalChannels
+ (JNIEnv* env, jclass)
+{
+ jint value = HAL_GetNumDigitalChannels();
+ return value;
+}
+
+/*
+ * Class: edu_wpi_first_hal_PortsJNI
+ * Method: getNumPWMChannels
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_PortsJNI_getNumPWMChannels
+ (JNIEnv* env, jclass)
+{
+ jint value = HAL_GetNumPWMChannels();
+ return value;
+}
+
+/*
+ * Class: edu_wpi_first_hal_PortsJNI
+ * Method: getNumDigitalPWMOutputs
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_PortsJNI_getNumDigitalPWMOutputs
+ (JNIEnv* env, jclass)
+{
+ jint value = HAL_GetNumDigitalPWMOutputs();
+ return value;
+}
+
+/*
+ * Class: edu_wpi_first_hal_PortsJNI
+ * Method: getNumEncoders
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_PortsJNI_getNumEncoders
+ (JNIEnv* env, jclass)
+{
+ jint value = HAL_GetNumEncoders();
+ return value;
+}
+
+/*
+ * Class: edu_wpi_first_hal_PortsJNI
+ * Method: getNumInterrupts
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_PortsJNI_getNumInterrupts
+ (JNIEnv* env, jclass)
+{
+ jint value = HAL_GetNumInterrupts();
+ return value;
+}
+
+/*
+ * Class: edu_wpi_first_hal_PortsJNI
+ * Method: getNumRelayChannels
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_PortsJNI_getNumRelayChannels
+ (JNIEnv* env, jclass)
+{
+ jint value = HAL_GetNumRelayChannels();
+ return value;
+}
+
+/*
+ * Class: edu_wpi_first_hal_PortsJNI
+ * Method: getNumRelayHeaders
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_PortsJNI_getNumRelayHeaders
+ (JNIEnv* env, jclass)
+{
+ jint value = HAL_GetNumRelayHeaders();
+ return value;
+}
+
+/*
+ * Class: edu_wpi_first_hal_PortsJNI
+ * Method: getNumPCMModules
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_PortsJNI_getNumPCMModules
+ (JNIEnv* env, jclass)
+{
+ jint value = HAL_GetNumPCMModules();
+ return value;
+}
+
+/*
+ * Class: edu_wpi_first_hal_PortsJNI
+ * Method: getNumSolenoidChannels
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_PortsJNI_getNumSolenoidChannels
+ (JNIEnv* env, jclass)
+{
+ jint value = HAL_GetNumSolenoidChannels();
+ return value;
+}
+
+/*
+ * Class: edu_wpi_first_hal_PortsJNI
+ * Method: getNumPDPModules
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_PortsJNI_getNumPDPModules
+ (JNIEnv* env, jclass)
+{
+ jint value = HAL_GetNumPDPModules();
+ return value;
+}
+
+/*
+ * Class: edu_wpi_first_hal_PortsJNI
+ * Method: getNumPDPChannels
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_PortsJNI_getNumPDPChannels
+ (JNIEnv* env, jclass)
+{
+ jint value = HAL_GetNumPDPChannels();
+ return value;
+}
+} // extern "C"
diff --git a/hal/src/main/native/cpp/jni/PowerJNI.cpp b/hal/src/main/native/cpp/jni/PowerJNI.cpp
new file mode 100644
index 0000000..9184e9e
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/PowerJNI.cpp
@@ -0,0 +1,228 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 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 <jni.h>
+
+#include "HALUtil.h"
+#include "edu_wpi_first_hal_PowerJNI.h"
+#include "hal/Power.h"
+
+using namespace frc;
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_hal_PowerJNI
+ * Method: getVinVoltage
+ * Signature: ()D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_PowerJNI_getVinVoltage
+ (JNIEnv* env, jclass)
+{
+ int32_t status = 0;
+ double val = HAL_GetVinVoltage(&status);
+ CheckStatus(env, status);
+ return val;
+}
+
+/*
+ * Class: edu_wpi_first_hal_PowerJNI
+ * Method: getVinCurrent
+ * Signature: ()D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_PowerJNI_getVinCurrent
+ (JNIEnv* env, jclass)
+{
+ int32_t status = 0;
+ double val = HAL_GetVinCurrent(&status);
+ CheckStatus(env, status);
+ return val;
+}
+
+/*
+ * Class: edu_wpi_first_hal_PowerJNI
+ * Method: getUserVoltage6V
+ * Signature: ()D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_PowerJNI_getUserVoltage6V
+ (JNIEnv* env, jclass)
+{
+ int32_t status = 0;
+ double val = HAL_GetUserVoltage6V(&status);
+ CheckStatus(env, status);
+ return val;
+}
+
+/*
+ * Class: edu_wpi_first_hal_PowerJNI
+ * Method: getUserCurrent6V
+ * Signature: ()D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_PowerJNI_getUserCurrent6V
+ (JNIEnv* env, jclass)
+{
+ int32_t status = 0;
+ double val = HAL_GetUserCurrent6V(&status);
+ CheckStatus(env, status);
+ return val;
+}
+
+/*
+ * Class: edu_wpi_first_hal_PowerJNI
+ * Method: getUserActive6V
+ * Signature: ()Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_PowerJNI_getUserActive6V
+ (JNIEnv* env, jclass)
+{
+ int32_t status = 0;
+ bool val = HAL_GetUserActive6V(&status);
+ CheckStatus(env, status);
+ return val;
+}
+
+/*
+ * Class: edu_wpi_first_hal_PowerJNI
+ * Method: getUserCurrentFaults6V
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_PowerJNI_getUserCurrentFaults6V
+ (JNIEnv* env, jclass)
+{
+ int32_t status = 0;
+ int32_t val = HAL_GetUserCurrentFaults6V(&status);
+ CheckStatus(env, status);
+ return val;
+}
+
+/*
+ * Class: edu_wpi_first_hal_PowerJNI
+ * Method: getUserVoltage5V
+ * Signature: ()D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_PowerJNI_getUserVoltage5V
+ (JNIEnv* env, jclass)
+{
+ int32_t status = 0;
+ double val = HAL_GetUserVoltage5V(&status);
+ CheckStatus(env, status);
+ return val;
+}
+
+/*
+ * Class: edu_wpi_first_hal_PowerJNI
+ * Method: getUserCurrent5V
+ * Signature: ()D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_PowerJNI_getUserCurrent5V
+ (JNIEnv* env, jclass)
+{
+ int32_t status = 0;
+ double val = HAL_GetUserCurrent5V(&status);
+ CheckStatus(env, status);
+ return val;
+}
+
+/*
+ * Class: edu_wpi_first_hal_PowerJNI
+ * Method: getUserActive5V
+ * Signature: ()Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_PowerJNI_getUserActive5V
+ (JNIEnv* env, jclass)
+{
+ int32_t status = 0;
+ bool val = HAL_GetUserActive5V(&status);
+ CheckStatus(env, status);
+ return val;
+}
+
+/*
+ * Class: edu_wpi_first_hal_PowerJNI
+ * Method: getUserCurrentFaults5V
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_PowerJNI_getUserCurrentFaults5V
+ (JNIEnv* env, jclass)
+{
+ int32_t status = 0;
+ int32_t val = HAL_GetUserCurrentFaults5V(&status);
+ CheckStatus(env, status);
+ return val;
+}
+
+/*
+ * Class: edu_wpi_first_hal_PowerJNI
+ * Method: getUserVoltage3V3
+ * Signature: ()D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_PowerJNI_getUserVoltage3V3
+ (JNIEnv* env, jclass)
+{
+ int32_t status = 0;
+ double val = HAL_GetUserVoltage3V3(&status);
+ CheckStatus(env, status);
+ return val;
+}
+
+/*
+ * Class: edu_wpi_first_hal_PowerJNI
+ * Method: getUserCurrent3V3
+ * Signature: ()D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_PowerJNI_getUserCurrent3V3
+ (JNIEnv* env, jclass)
+{
+ int32_t status = 0;
+ double val = HAL_GetUserCurrent3V3(&status);
+ CheckStatus(env, status);
+ return val;
+}
+
+/*
+ * Class: edu_wpi_first_hal_PowerJNI
+ * Method: getUserActive3V3
+ * Signature: ()Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_PowerJNI_getUserActive3V3
+ (JNIEnv* env, jclass)
+{
+ int32_t status = 0;
+ bool val = HAL_GetUserActive3V3(&status);
+ CheckStatus(env, status);
+ return val;
+}
+
+/*
+ * Class: edu_wpi_first_hal_PowerJNI
+ * Method: getUserCurrentFaults3V3
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_PowerJNI_getUserCurrentFaults3V3
+ (JNIEnv* env, jclass)
+{
+ int32_t status = 0;
+ int32_t val = HAL_GetUserCurrentFaults3V3(&status);
+ CheckStatus(env, status);
+ return val;
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/cpp/jni/RelayJNI.cpp b/hal/src/main/native/cpp/jni/RelayJNI.cpp
new file mode 100644
index 0000000..c058435
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/RelayJNI.cpp
@@ -0,0 +1,92 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 <jni.h>
+
+#include <cassert>
+
+#include "HALUtil.h"
+#include "edu_wpi_first_hal_RelayJNI.h"
+#include "hal/Ports.h"
+#include "hal/Relay.h"
+#include "hal/handles/HandlesInternal.h"
+
+using namespace frc;
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_hal_RelayJNI
+ * Method: initializeRelayPort
+ * Signature: (IZ)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_RelayJNI_initializeRelayPort
+ (JNIEnv* env, jclass, jint id, jboolean fwd)
+{
+ int32_t status = 0;
+ HAL_RelayHandle handle = HAL_InitializeRelayPort(
+ (HAL_PortHandle)id, static_cast<uint8_t>(fwd), &status);
+ CheckStatusRange(env, status, 0, HAL_GetNumRelayChannels(),
+ hal::getPortHandleChannel((HAL_PortHandle)id));
+ return (jint)handle;
+}
+
+/*
+ * Class: edu_wpi_first_hal_RelayJNI
+ * Method: freeRelayPort
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_RelayJNI_freeRelayPort
+ (JNIEnv* env, jclass, jint id)
+{
+ HAL_FreeRelayPort((HAL_RelayHandle)id);
+}
+
+/*
+ * Class: edu_wpi_first_hal_RelayJNI
+ * Method: checkRelayChannel
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_RelayJNI_checkRelayChannel
+ (JNIEnv* env, jclass, jint channel)
+{
+ return (jboolean)HAL_CheckRelayChannel(static_cast<uint8_t>(channel));
+}
+
+/*
+ * Class: edu_wpi_first_hal_RelayJNI
+ * Method: setRelay
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_RelayJNI_setRelay
+ (JNIEnv* env, jclass, jint id, jboolean value)
+{
+ int32_t status = 0;
+ HAL_SetRelay((HAL_RelayHandle)id, value, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_RelayJNI
+ * Method: getRelay
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_RelayJNI_getRelay
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ jboolean returnValue = HAL_GetRelay((HAL_RelayHandle)id, &status);
+ CheckStatus(env, status);
+ return returnValue;
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/cpp/jni/SPIJNI.cpp b/hal/src/main/native/cpp/jni/SPIJNI.cpp
new file mode 100644
index 0000000..7962e21
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/SPIJNI.cpp
@@ -0,0 +1,413 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 <jni.h>
+
+#include <cassert>
+
+#include <wpi/jni_util.h>
+
+#include "HALUtil.h"
+#include "edu_wpi_first_hal_SPIJNI.h"
+#include "hal/SPI.h"
+
+using namespace frc;
+using namespace wpi::java;
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_hal_SPIJNI
+ * Method: spiInitialize
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_SPIJNI_spiInitialize
+ (JNIEnv* env, jclass, jint port)
+{
+ int32_t status = 0;
+ HAL_InitializeSPI(static_cast<HAL_SPIPort>(port), &status);
+ CheckStatusForceThrow(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_SPIJNI
+ * Method: spiTransaction
+ * Signature: (ILjava/lang/Object;Ljava/lang/Object;B)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_SPIJNI_spiTransaction
+ (JNIEnv* env, jclass, jint port, jobject dataToSend, jobject dataReceived,
+ jbyte size)
+{
+ uint8_t* dataToSendPtr = nullptr;
+ if (dataToSend != 0) {
+ dataToSendPtr =
+ reinterpret_cast<uint8_t*>(env->GetDirectBufferAddress(dataToSend));
+ }
+ uint8_t* dataReceivedPtr =
+ reinterpret_cast<uint8_t*>(env->GetDirectBufferAddress(dataReceived));
+ jint retVal = HAL_TransactionSPI(static_cast<HAL_SPIPort>(port),
+ dataToSendPtr, dataReceivedPtr, size);
+ return retVal;
+}
+
+/*
+ * Class: edu_wpi_first_hal_SPIJNI
+ * Method: spiTransactionB
+ * Signature: (I[B[BB)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_SPIJNI_spiTransactionB
+ (JNIEnv* env, jclass, jint port, jbyteArray dataToSend,
+ jbyteArray dataReceived, jbyte size)
+{
+ wpi::SmallVector<uint8_t, 128> recvBuf;
+ recvBuf.resize(size);
+ jint retVal =
+ HAL_TransactionSPI(static_cast<HAL_SPIPort>(port),
+ reinterpret_cast<const uint8_t*>(
+ JByteArrayRef(env, dataToSend).array().data()),
+ recvBuf.data(), size);
+ env->SetByteArrayRegion(dataReceived, 0, size,
+ reinterpret_cast<const jbyte*>(recvBuf.data()));
+ return retVal;
+}
+
+/*
+ * Class: edu_wpi_first_hal_SPIJNI
+ * Method: spiWrite
+ * Signature: (ILjava/lang/Object;B)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_SPIJNI_spiWrite
+ (JNIEnv* env, jclass, jint port, jobject dataToSend, jbyte size)
+{
+ uint8_t* dataToSendPtr = nullptr;
+ if (dataToSend != 0) {
+ dataToSendPtr =
+ reinterpret_cast<uint8_t*>(env->GetDirectBufferAddress(dataToSend));
+ }
+ jint retVal =
+ HAL_WriteSPI(static_cast<HAL_SPIPort>(port), dataToSendPtr, size);
+ return retVal;
+}
+
+/*
+ * Class: edu_wpi_first_hal_SPIJNI
+ * Method: spiWriteB
+ * Signature: (I[BB)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_SPIJNI_spiWriteB
+ (JNIEnv* env, jclass, jint port, jbyteArray dataToSend, jbyte size)
+{
+ jint retVal = HAL_WriteSPI(static_cast<HAL_SPIPort>(port),
+ reinterpret_cast<const uint8_t*>(
+ JByteArrayRef(env, dataToSend).array().data()),
+ size);
+ return retVal;
+}
+
+/*
+ * Class: edu_wpi_first_hal_SPIJNI
+ * Method: spiRead
+ * Signature: (IZLjava/lang/Object;B)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_SPIJNI_spiRead
+ (JNIEnv* env, jclass, jint port, jboolean initiate, jobject dataReceived,
+ jbyte size)
+{
+ uint8_t* dataReceivedPtr =
+ reinterpret_cast<uint8_t*>(env->GetDirectBufferAddress(dataReceived));
+ jint retVal;
+ if (initiate) {
+ wpi::SmallVector<uint8_t, 128> sendBuf;
+ sendBuf.resize(size);
+ retVal = HAL_TransactionSPI(static_cast<HAL_SPIPort>(port), sendBuf.data(),
+ dataReceivedPtr, size);
+ } else {
+ retVal = HAL_ReadSPI(static_cast<HAL_SPIPort>(port),
+ reinterpret_cast<uint8_t*>(dataReceivedPtr), size);
+ }
+ return retVal;
+}
+
+/*
+ * Class: edu_wpi_first_hal_SPIJNI
+ * Method: spiReadB
+ * Signature: (IZ[BB)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_SPIJNI_spiReadB
+ (JNIEnv* env, jclass, jint port, jboolean initiate, jbyteArray dataReceived,
+ jbyte size)
+{
+ jint retVal;
+ wpi::SmallVector<uint8_t, 128> recvBuf;
+ recvBuf.resize(size);
+ if (initiate) {
+ wpi::SmallVector<uint8_t, 128> sendBuf;
+ sendBuf.resize(size);
+ retVal = HAL_TransactionSPI(static_cast<HAL_SPIPort>(port), sendBuf.data(),
+ recvBuf.data(), size);
+ } else {
+ retVal = HAL_ReadSPI(static_cast<HAL_SPIPort>(port), recvBuf.data(), size);
+ }
+ env->SetByteArrayRegion(dataReceived, 0, size,
+ reinterpret_cast<const jbyte*>(recvBuf.data()));
+ return retVal;
+}
+
+/*
+ * Class: edu_wpi_first_hal_SPIJNI
+ * Method: spiClose
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_SPIJNI_spiClose
+ (JNIEnv*, jclass, jint port)
+{
+ HAL_CloseSPI(static_cast<HAL_SPIPort>(port));
+}
+
+/*
+ * Class: edu_wpi_first_hal_SPIJNI
+ * Method: spiSetSpeed
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_SPIJNI_spiSetSpeed
+ (JNIEnv*, jclass, jint port, jint speed)
+{
+ HAL_SetSPISpeed(static_cast<HAL_SPIPort>(port), speed);
+}
+
+/*
+ * Class: edu_wpi_first_hal_SPIJNI
+ * Method: spiSetOpts
+ * Signature: (IIII)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_SPIJNI_spiSetOpts
+ (JNIEnv*, jclass, jint port, jint msb_first, jint sample_on_trailing,
+ jint clk_idle_high)
+{
+ HAL_SetSPIOpts(static_cast<HAL_SPIPort>(port), msb_first, sample_on_trailing,
+ clk_idle_high);
+}
+
+/*
+ * Class: edu_wpi_first_hal_SPIJNI
+ * Method: spiSetChipSelectActiveHigh
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_SPIJNI_spiSetChipSelectActiveHigh
+ (JNIEnv* env, jclass, jint port)
+{
+ int32_t status = 0;
+ HAL_SetSPIChipSelectActiveHigh(static_cast<HAL_SPIPort>(port), &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_SPIJNI
+ * Method: spiSetChipSelectActiveLow
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_SPIJNI_spiSetChipSelectActiveLow
+ (JNIEnv* env, jclass, jint port)
+{
+ int32_t status = 0;
+ HAL_SetSPIChipSelectActiveLow(static_cast<HAL_SPIPort>(port), &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_SPIJNI
+ * Method: spiInitAuto
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_SPIJNI_spiInitAuto
+ (JNIEnv* env, jclass, jint port, jint bufferSize)
+{
+ int32_t status = 0;
+ HAL_InitSPIAuto(static_cast<HAL_SPIPort>(port), bufferSize, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_SPIJNI
+ * Method: spiFreeAuto
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_SPIJNI_spiFreeAuto
+ (JNIEnv* env, jclass, jint port)
+{
+ int32_t status = 0;
+ HAL_FreeSPIAuto(static_cast<HAL_SPIPort>(port), &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_SPIJNI
+ * Method: spiStartAutoRate
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_SPIJNI_spiStartAutoRate
+ (JNIEnv* env, jclass, jint port, jdouble period)
+{
+ int32_t status = 0;
+ HAL_StartSPIAutoRate(static_cast<HAL_SPIPort>(port), period, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_SPIJNI
+ * Method: spiStartAutoTrigger
+ * Signature: (IIIZZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_SPIJNI_spiStartAutoTrigger
+ (JNIEnv* env, jclass, jint port, jint digitalSourceHandle,
+ jint analogTriggerType, jboolean triggerRising, jboolean triggerFalling)
+{
+ int32_t status = 0;
+ HAL_StartSPIAutoTrigger(static_cast<HAL_SPIPort>(port), digitalSourceHandle,
+ static_cast<HAL_AnalogTriggerType>(analogTriggerType),
+ triggerRising, triggerFalling, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_SPIJNI
+ * Method: spiStopAuto
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_SPIJNI_spiStopAuto
+ (JNIEnv* env, jclass, jint port)
+{
+ int32_t status = 0;
+ HAL_StopSPIAuto(static_cast<HAL_SPIPort>(port), &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_SPIJNI
+ * Method: spiSetAutoTransmitData
+ * Signature: (I[BI)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_SPIJNI_spiSetAutoTransmitData
+ (JNIEnv* env, jclass, jint port, jbyteArray dataToSend, jint zeroSize)
+{
+ JByteArrayRef jarr(env, dataToSend);
+ int32_t status = 0;
+ HAL_SetSPIAutoTransmitData(
+ static_cast<HAL_SPIPort>(port),
+ reinterpret_cast<const uint8_t*>(jarr.array().data()),
+ jarr.array().size(), zeroSize, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_SPIJNI
+ * Method: spiForceAutoRead
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_SPIJNI_spiForceAutoRead
+ (JNIEnv* env, jclass, jint port)
+{
+ int32_t status = 0;
+ HAL_ForceSPIAutoRead(static_cast<HAL_SPIPort>(port), &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_SPIJNI
+ * Method: spiReadAutoReceivedData
+ * Signature: (ILjava/lang/Object;ID)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_SPIJNI_spiReadAutoReceivedData__ILjava_nio_ByteBuffer_2ID
+ (JNIEnv* env, jclass, jint port, jobject buffer, jint numToRead,
+ jdouble timeout)
+{
+ uint32_t* recvBuf =
+ reinterpret_cast<uint32_t*>(env->GetDirectBufferAddress(buffer));
+ int32_t status = 0;
+ jint retval = HAL_ReadSPIAutoReceivedData(
+ static_cast<HAL_SPIPort>(port), recvBuf, numToRead, timeout, &status);
+ CheckStatus(env, status);
+ return retval;
+}
+
+/*
+ * Class: edu_wpi_first_hal_SPIJNI
+ * Method: spiReadAutoReceivedData
+ * Signature: (I[IID)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_SPIJNI_spiReadAutoReceivedData__I_3IID
+ (JNIEnv* env, jclass, jint port, jintArray buffer, jint numToRead,
+ jdouble timeout)
+{
+ wpi::SmallVector<uint32_t, 128> recvBuf;
+ recvBuf.resize(numToRead);
+ int32_t status = 0;
+ jint retval =
+ HAL_ReadSPIAutoReceivedData(static_cast<HAL_SPIPort>(port),
+ recvBuf.data(), numToRead, timeout, &status);
+ if (!CheckStatus(env, status)) return retval;
+ if (numToRead > 0) {
+ env->SetIntArrayRegion(buffer, 0, numToRead,
+ reinterpret_cast<const jint*>(recvBuf.data()));
+ }
+ return retval;
+}
+
+/*
+ * Class: edu_wpi_first_hal_SPIJNI
+ * Method: spiGetAutoDroppedCount
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_SPIJNI_spiGetAutoDroppedCount
+ (JNIEnv* env, jclass, jint port)
+{
+ int32_t status = 0;
+ auto retval =
+ HAL_GetSPIAutoDroppedCount(static_cast<HAL_SPIPort>(port), &status);
+ CheckStatus(env, status);
+ return retval;
+}
+
+/*
+ * Class: edu_wpi_first_hal_SPIJNI
+ * Method: spiConfigureAutoStall
+ * Signature: (IIII)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_SPIJNI_spiConfigureAutoStall
+ (JNIEnv* env, jclass, jint port, jint csToSclkTicks, jint stallTicks,
+ jint pow2BytesPerRead)
+{
+ int32_t status = 0;
+ HAL_ConfigureSPIAutoStall(static_cast<HAL_SPIPort>(port), csToSclkTicks,
+ stallTicks, pow2BytesPerRead, &status);
+ CheckStatus(env, status);
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/cpp/jni/SerialPortJNI.cpp b/hal/src/main/native/cpp/jni/SerialPortJNI.cpp
new file mode 100644
index 0000000..9fe9d92
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/SerialPortJNI.cpp
@@ -0,0 +1,318 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 <jni.h>
+
+#include <cassert>
+
+#include <wpi/jni_util.h>
+
+#include "HALUtil.h"
+#include "edu_wpi_first_hal_SerialPortJNI.h"
+#include "hal/SerialPort.h"
+
+using namespace frc;
+using namespace wpi::java;
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_hal_SerialPortJNI
+ * Method: serialInitializePort
+ * Signature: (B)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_SerialPortJNI_serialInitializePort
+ (JNIEnv* env, jclass, jbyte port)
+{
+ int32_t status = 0;
+ auto handle =
+ HAL_InitializeSerialPort(static_cast<HAL_SerialPort>(port), &status);
+ CheckStatusForceThrow(env, status);
+ return static_cast<jint>(handle);
+}
+
+/*
+ * Class: edu_wpi_first_hal_SerialPortJNI
+ * Method: serialInitializePortDirect
+ * Signature: (BLjava/lang/String;)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_SerialPortJNI_serialInitializePortDirect
+ (JNIEnv* env, jclass, jbyte port, jstring portName)
+{
+ JStringRef portNameRef{env, portName};
+ int32_t status = 0;
+ auto handle = HAL_InitializeSerialPortDirect(
+ static_cast<HAL_SerialPort>(port), portNameRef.c_str(), &status);
+ CheckStatusForceThrow(env, status);
+ return static_cast<jint>(handle);
+}
+
+/*
+ * Class: edu_wpi_first_hal_SerialPortJNI
+ * Method: serialSetBaudRate
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_SerialPortJNI_serialSetBaudRate
+ (JNIEnv* env, jclass, jint handle, jint rate)
+{
+ int32_t status = 0;
+ HAL_SetSerialBaudRate(static_cast<HAL_SerialPortHandle>(handle), rate,
+ &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_SerialPortJNI
+ * Method: serialSetDataBits
+ * Signature: (IB)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_SerialPortJNI_serialSetDataBits
+ (JNIEnv* env, jclass, jint handle, jbyte bits)
+{
+ int32_t status = 0;
+ HAL_SetSerialDataBits(static_cast<HAL_SerialPortHandle>(handle), bits,
+ &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_SerialPortJNI
+ * Method: serialSetParity
+ * Signature: (IB)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_SerialPortJNI_serialSetParity
+ (JNIEnv* env, jclass, jint handle, jbyte parity)
+{
+ int32_t status = 0;
+ HAL_SetSerialParity(static_cast<HAL_SerialPortHandle>(handle), parity,
+ &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_SerialPortJNI
+ * Method: serialSetStopBits
+ * Signature: (IB)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_SerialPortJNI_serialSetStopBits
+ (JNIEnv* env, jclass, jint handle, jbyte bits)
+{
+ int32_t status = 0;
+ HAL_SetSerialStopBits(static_cast<HAL_SerialPortHandle>(handle), bits,
+ &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_SerialPortJNI
+ * Method: serialSetWriteMode
+ * Signature: (IB)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_SerialPortJNI_serialSetWriteMode
+ (JNIEnv* env, jclass, jint handle, jbyte mode)
+{
+ int32_t status = 0;
+ HAL_SetSerialWriteMode(static_cast<HAL_SerialPortHandle>(handle), mode,
+ &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_SerialPortJNI
+ * Method: serialSetFlowControl
+ * Signature: (IB)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_SerialPortJNI_serialSetFlowControl
+ (JNIEnv* env, jclass, jint handle, jbyte flow)
+{
+ int32_t status = 0;
+ HAL_SetSerialFlowControl(static_cast<HAL_SerialPortHandle>(handle), flow,
+ &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_SerialPortJNI
+ * Method: serialSetTimeout
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_SerialPortJNI_serialSetTimeout
+ (JNIEnv* env, jclass, jint handle, jdouble timeout)
+{
+ int32_t status = 0;
+ HAL_SetSerialTimeout(static_cast<HAL_SerialPortHandle>(handle), timeout,
+ &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_SerialPortJNI
+ * Method: serialEnableTermination
+ * Signature: (IC)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_SerialPortJNI_serialEnableTermination
+ (JNIEnv* env, jclass, jint handle, jchar terminator)
+{
+ int32_t status = 0;
+ HAL_EnableSerialTermination(static_cast<HAL_SerialPortHandle>(handle),
+ terminator, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_SerialPortJNI
+ * Method: serialDisableTermination
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_SerialPortJNI_serialDisableTermination
+ (JNIEnv* env, jclass, jint handle)
+{
+ int32_t status = 0;
+ HAL_DisableSerialTermination(static_cast<HAL_SerialPortHandle>(handle),
+ &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_SerialPortJNI
+ * Method: serialSetReadBufferSize
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_SerialPortJNI_serialSetReadBufferSize
+ (JNIEnv* env, jclass, jint handle, jint size)
+{
+ int32_t status = 0;
+ HAL_SetSerialReadBufferSize(static_cast<HAL_SerialPortHandle>(handle), size,
+ &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_SerialPortJNI
+ * Method: serialSetWriteBufferSize
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_SerialPortJNI_serialSetWriteBufferSize
+ (JNIEnv* env, jclass, jint handle, jint size)
+{
+ int32_t status = 0;
+ HAL_SetSerialWriteBufferSize(static_cast<HAL_SerialPortHandle>(handle), size,
+ &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_SerialPortJNI
+ * Method: serialGetBytesReceived
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_SerialPortJNI_serialGetBytesReceived
+ (JNIEnv* env, jclass, jint handle)
+{
+ int32_t status = 0;
+ jint retVal = HAL_GetSerialBytesReceived(
+ static_cast<HAL_SerialPortHandle>(handle), &status);
+ CheckStatus(env, status);
+ return retVal;
+}
+
+/*
+ * Class: edu_wpi_first_hal_SerialPortJNI
+ * Method: serialRead
+ * Signature: (I[BI)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_SerialPortJNI_serialRead
+ (JNIEnv* env, jclass, jint handle, jbyteArray dataReceived, jint size)
+{
+ wpi::SmallVector<char, 128> recvBuf;
+ recvBuf.resize(size);
+ int32_t status = 0;
+ jint retVal = HAL_ReadSerial(static_cast<HAL_SerialPortHandle>(handle),
+ recvBuf.data(), size, &status);
+ env->SetByteArrayRegion(dataReceived, 0, size,
+ reinterpret_cast<const jbyte*>(recvBuf.data()));
+ CheckStatus(env, status);
+ return retVal;
+}
+
+/*
+ * Class: edu_wpi_first_hal_SerialPortJNI
+ * Method: serialWrite
+ * Signature: (I[BI)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_SerialPortJNI_serialWrite
+ (JNIEnv* env, jclass, jint handle, jbyteArray dataToSend, jint size)
+{
+ int32_t status = 0;
+ jint retVal =
+ HAL_WriteSerial(static_cast<HAL_SerialPortHandle>(handle),
+ reinterpret_cast<const char*>(
+ JByteArrayRef(env, dataToSend).array().data()),
+ size, &status);
+ CheckStatus(env, status);
+ return retVal;
+}
+
+/*
+ * Class: edu_wpi_first_hal_SerialPortJNI
+ * Method: serialFlush
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_SerialPortJNI_serialFlush
+ (JNIEnv* env, jclass, jint handle)
+{
+ int32_t status = 0;
+ HAL_FlushSerial(static_cast<HAL_SerialPortHandle>(handle), &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_SerialPortJNI
+ * Method: serialClear
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_SerialPortJNI_serialClear
+ (JNIEnv* env, jclass, jint handle)
+{
+ int32_t status = 0;
+ HAL_ClearSerial(static_cast<HAL_SerialPortHandle>(handle), &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_SerialPortJNI
+ * Method: serialClose
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_SerialPortJNI_serialClose
+ (JNIEnv* env, jclass, jint handle)
+{
+ int32_t status = 0;
+ HAL_CloseSerial(static_cast<HAL_SerialPortHandle>(handle), &status);
+ CheckStatus(env, status);
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/cpp/jni/SimDeviceJNI.cpp b/hal/src/main/native/cpp/jni/SimDeviceJNI.cpp
new file mode 100644
index 0000000..d9652cc
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/SimDeviceJNI.cpp
@@ -0,0 +1,168 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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 <jni.h>
+
+#include <wpi/jni_util.h>
+
+#include "HALUtil.h"
+#include "edu_wpi_first_hal_SimDeviceJNI.h"
+#include "hal/SimDevice.h"
+
+using namespace wpi::java;
+
+static HAL_Value ValueFromJava(jint type, jlong value1, jdouble value2) {
+ HAL_Value value;
+ value.type = static_cast<HAL_Type>(type);
+ switch (value.type) {
+ case HAL_BOOLEAN:
+ value.data.v_boolean = value1;
+ break;
+ case HAL_DOUBLE:
+ value.data.v_double = value2;
+ break;
+ case HAL_ENUM:
+ value.data.v_enum = value1;
+ break;
+ case HAL_INT:
+ value.data.v_int = value1;
+ break;
+ case HAL_LONG:
+ value.data.v_long = value1;
+ break;
+ default:
+ break;
+ }
+ return value;
+}
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_hal_SimDeviceJNI
+ * Method: createSimDevice
+ * Signature: (Ljava/lang/String;)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_SimDeviceJNI_createSimDevice
+ (JNIEnv* env, jclass, jstring name)
+{
+ return HAL_CreateSimDevice(JStringRef{env, name}.c_str());
+}
+
+/*
+ * Class: edu_wpi_first_hal_SimDeviceJNI
+ * Method: freeSimDevice
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_SimDeviceJNI_freeSimDevice
+ (JNIEnv*, jclass, jint handle)
+{
+ HAL_FreeSimDevice(handle);
+}
+
+/*
+ * Class: edu_wpi_first_hal_SimDeviceJNI
+ * Method: createSimValueNative
+ * Signature: (ILjava/lang/String;ZIJD)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_SimDeviceJNI_createSimValueNative
+ (JNIEnv* env, jclass, jint device, jstring name, jboolean readonly, jint type,
+ jlong value1, jdouble value2)
+{
+ return HAL_CreateSimValue(device, JStringRef{env, name}.c_str(), readonly,
+ ValueFromJava(type, value1, value2));
+}
+
+/*
+ * Class: edu_wpi_first_hal_SimDeviceJNI
+ * Method: createSimValueEnum
+ * Signature: (ILjava/lang/String;Z[Ljava/lang/Object;I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_SimDeviceJNI_createSimValueEnum
+ (JNIEnv* env, jclass, jint device, jstring name, jboolean readonly,
+ jobjectArray options, jint initialValue)
+{
+ size_t len = env->GetArrayLength(options);
+ std::vector<std::string> arr;
+ arr.reserve(len);
+ for (size_t i = 0; i < len; ++i) {
+ JLocal<jstring> elem{
+ env, static_cast<jstring>(env->GetObjectArrayElement(options, i))};
+ if (!elem) return 0;
+ arr.push_back(JStringRef{env, elem}.str());
+ }
+ wpi::SmallVector<const char*, 8> carr;
+ for (auto&& val : arr) carr.push_back(val.c_str());
+ return HAL_CreateSimValueEnum(device, JStringRef{env, name}.c_str(), readonly,
+ len, carr.data(), initialValue);
+}
+
+/*
+ * Class: edu_wpi_first_hal_SimDeviceJNI
+ * Method: getSimValue
+ * Signature: (I)Ljava/lang/Object;
+ */
+JNIEXPORT jobject JNICALL
+Java_edu_wpi_first_hal_SimDeviceJNI_getSimValue
+ (JNIEnv* env, jclass, jint handle)
+{
+ return frc::CreateHALValue(env, HAL_GetSimValue(handle));
+}
+
+/*
+ * Class: edu_wpi_first_hal_SimDeviceJNI
+ * Method: getSimValueDouble
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_SimDeviceJNI_getSimValueDouble
+ (JNIEnv*, jclass, jint handle)
+{
+ return HAL_GetSimValueDouble(handle);
+}
+
+/*
+ * Class: edu_wpi_first_hal_SimDeviceJNI
+ * Method: getSimValueEnum
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_SimDeviceJNI_getSimValueEnum
+ (JNIEnv*, jclass, jint handle)
+{
+ return HAL_GetSimValueEnum(handle);
+}
+
+/*
+ * Class: edu_wpi_first_hal_SimDeviceJNI
+ * Method: getSimValueBoolean
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_SimDeviceJNI_getSimValueBoolean
+ (JNIEnv*, jclass, jint handle)
+{
+ return HAL_GetSimValueBoolean(handle);
+}
+
+/*
+ * Class: edu_wpi_first_hal_SimDeviceJNI
+ * Method: setSimValueNative
+ * Signature: (IIJD)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_SimDeviceJNI_setSimValueNative
+ (JNIEnv*, jclass, jint handle, jint type, jlong value1, jdouble value2)
+{
+ HAL_SetSimValue(handle, ValueFromJava(type, value1, value2));
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/cpp/jni/SolenoidJNI.cpp b/hal/src/main/native/cpp/jni/SolenoidJNI.cpp
new file mode 100644
index 0000000..4e6f139
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/SolenoidJNI.cpp
@@ -0,0 +1,203 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 <jni.h>
+
+#include "HALUtil.h"
+#include "edu_wpi_first_hal_SolenoidJNI.h"
+#include "hal/Ports.h"
+#include "hal/Solenoid.h"
+#include "hal/handles/HandlesInternal.h"
+
+using namespace frc;
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_hal_SolenoidJNI
+ * Method: initializeSolenoidPort
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_SolenoidJNI_initializeSolenoidPort
+ (JNIEnv* env, jclass, jint id)
+{
+ int32_t status = 0;
+ HAL_SolenoidHandle handle =
+ HAL_InitializeSolenoidPort((HAL_PortHandle)id, &status);
+
+ // Use solenoid channels, as we have to pick one.
+ CheckStatusRange(env, status, 0, HAL_GetNumSolenoidChannels(),
+ hal::getPortHandleChannel((HAL_PortHandle)id));
+ return (jint)handle;
+}
+
+/*
+ * Class: edu_wpi_first_hal_SolenoidJNI
+ * Method: checkSolenoidChannel
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_SolenoidJNI_checkSolenoidChannel
+ (JNIEnv* env, jclass, jint channel)
+{
+ return HAL_CheckSolenoidChannel(channel);
+}
+
+/*
+ * Class: edu_wpi_first_hal_SolenoidJNI
+ * Method: checkSolenoidModule
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_SolenoidJNI_checkSolenoidModule
+ (JNIEnv* env, jclass, jint module)
+{
+ return HAL_CheckSolenoidModule(module);
+}
+
+/*
+ * Class: edu_wpi_first_hal_SolenoidJNI
+ * Method: freeSolenoidPort
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_SolenoidJNI_freeSolenoidPort
+ (JNIEnv* env, jclass, jint id)
+{
+ HAL_FreeSolenoidPort((HAL_SolenoidHandle)id);
+}
+
+/*
+ * Class: edu_wpi_first_hal_SolenoidJNI
+ * Method: setSolenoid
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_SolenoidJNI_setSolenoid
+ (JNIEnv* env, jclass, jint solenoid_port, jboolean value)
+{
+ int32_t status = 0;
+ HAL_SetSolenoid((HAL_SolenoidHandle)solenoid_port, value, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_SolenoidJNI
+ * Method: getSolenoid
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_SolenoidJNI_getSolenoid
+ (JNIEnv* env, jclass, jint solenoid_port)
+{
+ int32_t status = 0;
+ jboolean val = HAL_GetSolenoid((HAL_SolenoidHandle)solenoid_port, &status);
+ CheckStatus(env, status);
+ return val;
+}
+
+/*
+ * Class: edu_wpi_first_hal_SolenoidJNI
+ * Method: getAllSolenoids
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_SolenoidJNI_getAllSolenoids
+ (JNIEnv* env, jclass, jint module)
+{
+ int32_t status = 0;
+ jint val = HAL_GetAllSolenoids(module, &status);
+ CheckStatus(env, status);
+ return val;
+}
+
+/*
+ * Class: edu_wpi_first_hal_SolenoidJNI
+ * Method: getPCMSolenoidBlackList
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_SolenoidJNI_getPCMSolenoidBlackList
+ (JNIEnv* env, jclass, jint module)
+{
+ int32_t status = 0;
+ jint val = HAL_GetPCMSolenoidBlackList(module, &status);
+ CheckStatus(env, status);
+ return val;
+}
+/*
+ * Class: edu_wpi_first_hal_SolenoidJNI
+ * Method: getPCMSolenoidVoltageStickyFault
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_SolenoidJNI_getPCMSolenoidVoltageStickyFault
+ (JNIEnv* env, jclass, jint module)
+{
+ int32_t status = 0;
+ bool val = HAL_GetPCMSolenoidVoltageStickyFault(module, &status);
+ CheckStatus(env, status);
+ return val;
+}
+/*
+ * Class: edu_wpi_first_hal_SolenoidJNI
+ * Method: getPCMSolenoidVoltageFault
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_SolenoidJNI_getPCMSolenoidVoltageFault
+ (JNIEnv* env, jclass, jint module)
+{
+ int32_t status = 0;
+ bool val = HAL_GetPCMSolenoidVoltageFault(module, &status);
+ CheckStatus(env, status);
+ return val;
+}
+/*
+ * Class: edu_wpi_first_hal_SolenoidJNI
+ * Method: clearAllPCMStickyFaults
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_SolenoidJNI_clearAllPCMStickyFaults
+ (JNIEnv* env, jclass, jint module)
+{
+ int32_t status = 0;
+ HAL_ClearAllPCMStickyFaults(module, &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_SolenoidJNI
+ * Method: setOneShotDuration
+ * Signature: (IJ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_SolenoidJNI_setOneShotDuration
+ (JNIEnv* env, jclass, jint solenoid_port, jlong durationMS)
+{
+ int32_t status = 0;
+ HAL_SetOneShotDuration((HAL_SolenoidHandle)solenoid_port, durationMS,
+ &status);
+ CheckStatus(env, status);
+}
+
+/*
+ * Class: edu_wpi_first_hal_SolenoidJNI
+ * Method: fireOneShot
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_SolenoidJNI_fireOneShot
+ (JNIEnv* env, jclass, jint solenoid_port)
+{
+ int32_t status = 0;
+ HAL_FireOneShot((HAL_SolenoidHandle)solenoid_port, &status);
+ CheckStatus(env, status);
+}
+} // extern "C"
diff --git a/hal/src/main/native/cpp/jni/ThreadsJNI.cpp b/hal/src/main/native/cpp/jni/ThreadsJNI.cpp
new file mode 100644
index 0000000..d4620e2
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/ThreadsJNI.cpp
@@ -0,0 +1,67 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 <jni.h>
+
+#include <cassert>
+
+#include "HALUtil.h"
+#include "edu_wpi_first_hal_ThreadsJNI.h"
+#include "hal/Threads.h"
+
+using namespace frc;
+
+extern "C" {
+/*
+ * Class: edu_wpi_first_hal_ThreadsJNI
+ * Method: getCurrentThreadPriority
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_ThreadsJNI_getCurrentThreadPriority
+ (JNIEnv* env, jclass)
+{
+ int32_t status = 0;
+ HAL_Bool isRT = false;
+ auto ret = HAL_GetCurrentThreadPriority(&isRT, &status);
+ CheckStatus(env, status);
+ return (jint)ret;
+}
+
+/*
+ * Class: edu_wpi_first_hal_ThreadsJNI
+ * Method: getCurrentThreadIsRealTime
+ * Signature: ()Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_ThreadsJNI_getCurrentThreadIsRealTime
+ (JNIEnv* env, jclass)
+{
+ int32_t status = 0;
+ HAL_Bool isRT = false;
+ HAL_GetCurrentThreadPriority(&isRT, &status);
+ CheckStatus(env, status);
+ return (jboolean)isRT;
+}
+
+/*
+ * Class: edu_wpi_first_hal_ThreadsJNI
+ * Method: setCurrentThreadPriority
+ * Signature: (ZI)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_ThreadsJNI_setCurrentThreadPriority
+ (JNIEnv* env, jclass, jboolean realTime, jint priority)
+{
+ int32_t status = 0;
+ auto ret = HAL_SetCurrentThreadPriority(
+ (HAL_Bool)realTime, static_cast<int32_t>(priority), &status);
+ CheckStatus(env, status);
+ return (jboolean)ret;
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/include/hal/Accelerometer.h b/hal/src/main/native/include/hal/Accelerometer.h
new file mode 100644
index 0000000..9dc488b
--- /dev/null
+++ b/hal/src/main/native/include/hal/Accelerometer.h
@@ -0,0 +1,79 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 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 "hal/Types.h"
+
+/**
+ * @defgroup hal_accelerometer Accelerometer Functions
+ * @ingroup hal_capi
+ * @{
+ */
+
+// clang-format off
+/**
+ * The acceptable accelerometer ranges.
+ */
+HAL_ENUM(HAL_AccelerometerRange) {
+ HAL_AccelerometerRange_k2G = 0,
+ HAL_AccelerometerRange_k4G = 1,
+ HAL_AccelerometerRange_k8G = 2,
+};
+// clang-format on
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+/**
+ * Sets the accelerometer to active or standby mode.
+ *
+ * It must be in standby mode to change any configuration.
+ *
+ * @param active true to set to active, false for standby
+ */
+void HAL_SetAccelerometerActive(HAL_Bool active);
+
+/**
+ * Sets the range of values that can be measured (either 2, 4, or 8 g-forces).
+ *
+ * The accelerometer should be in standby mode when this is called.
+ *
+ * @param range the accelerometer range
+ */
+void HAL_SetAccelerometerRange(HAL_AccelerometerRange range);
+
+/**
+ * Gets the x-axis acceleration.
+ *
+ * This is a floating point value in units of 1 g-force.
+ *
+ * @return the X acceleration
+ */
+double HAL_GetAccelerometerX(void);
+
+/**
+ * Gets the y-axis acceleration.
+ *
+ * This is a floating point value in units of 1 g-force.
+ *
+ * @return the Y acceleration
+ */
+double HAL_GetAccelerometerY(void);
+
+/**
+ * Gets the z-axis acceleration.
+ *
+ * This is a floating point value in units of 1 g-force.
+ *
+ * @return the Z acceleration
+ */
+double HAL_GetAccelerometerZ(void);
+#ifdef __cplusplus
+} // extern "C"
+/** @} */
+#endif
diff --git a/hal/src/main/native/include/hal/AddressableLED.h b/hal/src/main/native/include/hal/AddressableLED.h
new file mode 100644
index 0000000..68383ef
--- /dev/null
+++ b/hal/src/main/native/include/hal/AddressableLED.h
@@ -0,0 +1,61 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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 <stdint.h>
+
+#include "hal/AddressableLEDTypes.h"
+#include "hal/Types.h"
+
+/**
+ * @defgroup hal_addressable Addressable LED Functions
+ * @ingroup hal_capi
+ * @{
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+HAL_AddressableLEDHandle HAL_InitializeAddressableLED(
+ HAL_DigitalHandle outputPort, int32_t* status);
+
+void HAL_FreeAddressableLED(HAL_AddressableLEDHandle handle);
+
+void HAL_SetAddressableLEDOutputPort(HAL_AddressableLEDHandle handle,
+ HAL_DigitalHandle outputPort,
+ int32_t* status);
+
+void HAL_SetAddressableLEDLength(HAL_AddressableLEDHandle handle,
+ int32_t length, int32_t* status);
+
+void HAL_WriteAddressableLEDData(HAL_AddressableLEDHandle handle,
+ const struct HAL_AddressableLEDData* data,
+ int32_t length, int32_t* status);
+
+void HAL_SetAddressableLEDBitTiming(HAL_AddressableLEDHandle handle,
+ int32_t lowTime0NanoSeconds,
+ int32_t highTime0NanoSeconds,
+ int32_t lowTime1NanoSeconds,
+ int32_t highTime1NanoSeconds,
+ int32_t* status);
+
+void HAL_SetAddressableLEDSyncTime(HAL_AddressableLEDHandle handle,
+ int32_t syncTimeMicroSeconds,
+ int32_t* status);
+
+void HAL_StartAddressableLEDOutput(HAL_AddressableLEDHandle handle,
+ int32_t* status);
+
+void HAL_StopAddressableLEDOutput(HAL_AddressableLEDHandle handle,
+ int32_t* status);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
+/** @} */
diff --git a/hal/src/main/native/include/hal/AddressableLEDTypes.h b/hal/src/main/native/include/hal/AddressableLEDTypes.h
new file mode 100644
index 0000000..bdcd742
--- /dev/null
+++ b/hal/src/main/native/include/hal/AddressableLEDTypes.h
@@ -0,0 +1,19 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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 <stdint.h>
+
+#define HAL_kAddressableLEDMaxLength 5460
+
+struct HAL_AddressableLEDData {
+ uint8_t b;
+ uint8_t g;
+ uint8_t r;
+ uint8_t padding;
+};
diff --git a/hal/src/main/native/include/hal/AnalogAccumulator.h b/hal/src/main/native/include/hal/AnalogAccumulator.h
new file mode 100644
index 0000000..9e49e90
--- /dev/null
+++ b/hal/src/main/native/include/hal/AnalogAccumulator.h
@@ -0,0 +1,115 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 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 <stdint.h>
+
+#include "hal/Types.h"
+
+/**
+ * @defgroup hal_analogaccumulator Analog Accumulator Functions
+ * @ingroup hal_capi
+ * @{
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * Is the channel attached to an accumulator.
+ *
+ * @param analogPortHandle Handle to the analog port.
+ * @return The analog channel is attached to an accumulator.
+ */
+HAL_Bool HAL_IsAccumulatorChannel(HAL_AnalogInputHandle analogPortHandle,
+ int32_t* status);
+
+/**
+ * Initialize the accumulator.
+ *
+ * @param analogPortHandle Handle to the analog port.
+ */
+void HAL_InitAccumulator(HAL_AnalogInputHandle analogPortHandle,
+ int32_t* status);
+
+/**
+ * Resets the accumulator to the initial value.
+ *
+ * @param analogPortHandle Handle to the analog port.
+ */
+void HAL_ResetAccumulator(HAL_AnalogInputHandle analogPortHandle,
+ int32_t* status);
+
+/**
+ * Set the center value of the accumulator.
+ *
+ * The center value is subtracted from each A/D value before it is added to the
+ * accumulator. This is used for the center value of devices like gyros and
+ * accelerometers to make integration work and to take the device offset into
+ * account when integrating.
+ *
+ * This center value is based on the output of the oversampled and averaged
+ * source from channel 1. Because of this, any non-zero oversample bits will
+ * affect the size of the value for this field.
+ *
+ * @param analogPortHandle Handle to the analog port.
+ * @param center The center value of the accumulator.
+ */
+void HAL_SetAccumulatorCenter(HAL_AnalogInputHandle analogPortHandle,
+ int32_t center, int32_t* status);
+
+/**
+ * Set the accumulator's deadband.
+ *
+ * @param analogPortHandle Handle to the analog port.
+ * @param deadband The deadband of the accumulator.
+ */
+void HAL_SetAccumulatorDeadband(HAL_AnalogInputHandle analogPortHandle,
+ int32_t deadband, int32_t* status);
+
+/**
+ * Read the accumulated value.
+ *
+ * Read the value that has been accumulating on channel 1.
+ * The accumulator is attached after the oversample and average engine.
+ *
+ * @param analogPortHandle Handle to the analog port.
+ * @return The 64-bit value accumulated since the last Reset().
+ */
+int64_t HAL_GetAccumulatorValue(HAL_AnalogInputHandle analogPortHandle,
+ int32_t* status);
+
+/**
+ * Read the number of accumulated values.
+ *
+ * Read the count of the accumulated values since the accumulator was last
+ * Reset().
+ *
+ * @param analogPortHandle Handle to the analog port.
+ * @return The number of times samples from the channel were accumulated.
+ */
+int64_t HAL_GetAccumulatorCount(HAL_AnalogInputHandle analogPortHandle,
+ int32_t* status);
+
+/**
+ * Read the accumulated value and the number of accumulated values atomically.
+ *
+ * This function reads the value and count from the FPGA atomically.
+ * This can be used for averaging.
+ *
+ * @param analogPortHandle Handle to the analog port.
+ * @param value Pointer to the 64-bit accumulated output.
+ * @param count Pointer to the number of accumulation cycles.
+ */
+void HAL_GetAccumulatorOutput(HAL_AnalogInputHandle analogPortHandle,
+ int64_t* value, int64_t* count, int32_t* status);
+#ifdef __cplusplus
+} // extern "C"
+#endif
+/** @} */
diff --git a/hal/src/main/native/include/hal/AnalogGyro.h b/hal/src/main/native/include/hal/AnalogGyro.h
new file mode 100644
index 0000000..54030bd
--- /dev/null
+++ b/hal/src/main/native/include/hal/AnalogGyro.h
@@ -0,0 +1,138 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 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 <stdint.h>
+
+#include "hal/Types.h"
+
+/**
+ * @defgroup hal_analoggyro Analog Gyro Functions
+ * @ingroup hal_capi
+ * @{
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * Initializes an analog gyro.
+ *
+ * @param handle handle to the analog port
+ * @return the initialized gyro handle
+ */
+HAL_GyroHandle HAL_InitializeAnalogGyro(HAL_AnalogInputHandle handle,
+ int32_t* status);
+
+/**
+ * Sets up an analog gyro with the proper offsets and settings for the KOP
+ * analog gyro.
+ *
+ * @param handle the gyro handle
+ */
+void HAL_SetupAnalogGyro(HAL_GyroHandle handle, int32_t* status);
+
+/**
+ * Frees an analog gyro.
+ *
+ * @param handle the gyro handle
+ */
+void HAL_FreeAnalogGyro(HAL_GyroHandle handle);
+
+/**
+ * Sets the analog gyro parameters to the specified values.
+ *
+ * This is meant to be used if you want to reuse the values from a previous
+ * calibration.
+ *
+ * @param handle the gyro handle
+ * @param voltsPerDegreePerSecond the gyro volts scaling
+ * @param offset the gyro offset
+ * @param center the gyro center
+ */
+void HAL_SetAnalogGyroParameters(HAL_GyroHandle handle,
+ double voltsPerDegreePerSecond, double offset,
+ int32_t center, int32_t* status);
+
+/**
+ * Sets the analog gyro volts per degrees per second scaling.
+ *
+ * @param handle the gyro handle
+ * @param voltsPerDegreePerSecond the gyro volts scaling
+ */
+void HAL_SetAnalogGyroVoltsPerDegreePerSecond(HAL_GyroHandle handle,
+ double voltsPerDegreePerSecond,
+ int32_t* status);
+
+/**
+ * Resets the analog gyro value to 0.
+ *
+ * @param handle the gyro handle
+ */
+void HAL_ResetAnalogGyro(HAL_GyroHandle handle, int32_t* status);
+
+/**
+ * Calibrates the analog gyro.
+ *
+ * This happens by calculating the average value of the gyro over 5 seconds, and
+ * setting that as the center. Note that this call blocks for 5 seconds to
+ * perform this.
+ *
+ * @param handle the gyro handle
+ */
+void HAL_CalibrateAnalogGyro(HAL_GyroHandle handle, int32_t* status);
+
+/**
+ * Sets the deadband of the analog gyro.
+ *
+ * @param handle the gyro handle
+ * @param volts the voltage deadband
+ */
+void HAL_SetAnalogGyroDeadband(HAL_GyroHandle handle, double volts,
+ int32_t* status);
+
+/**
+ * Gets the gyro angle in degrees.
+ *
+ * @param handle the gyro handle
+ * @return the gyro angle in degrees
+ */
+double HAL_GetAnalogGyroAngle(HAL_GyroHandle handle, int32_t* status);
+
+/**
+ * Gets the gyro rate in degrees/second.
+ *
+ * @param handle the gyro handle
+ * @return the gyro rate in degrees/second
+ */
+double HAL_GetAnalogGyroRate(HAL_GyroHandle handle, int32_t* status);
+
+/**
+ * Gets the calibrated gyro offset.
+ *
+ * Can be used to not repeat a calibration but reconstruct the gyro object.
+ *
+ * @param handle the gyro handle
+ * @return the gryo offset
+ */
+double HAL_GetAnalogGyroOffset(HAL_GyroHandle handle, int32_t* status);
+
+/**
+ * Gets the calibrated gyro center.
+ *
+ * Can be used to not repeat a calibration but reconstruct the gyro object.
+ *
+ * @param handle the gyro handle
+ * @return the gyro center
+ */
+int32_t HAL_GetAnalogGyroCenter(HAL_GyroHandle handle, int32_t* status);
+#ifdef __cplusplus
+} // extern "C"
+#endif
+/** @} */
diff --git a/hal/src/main/native/include/hal/AnalogInput.h b/hal/src/main/native/include/hal/AnalogInput.h
new file mode 100644
index 0000000..95ba4e0
--- /dev/null
+++ b/hal/src/main/native/include/hal/AnalogInput.h
@@ -0,0 +1,249 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 <stdint.h>
+
+#include "hal/Types.h"
+
+/**
+ * @defgroup hal_analoginput Analog Input Functions
+ * @ingroup hal_capi
+ * @{
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * Initializes the analog input port using the given port object.
+ *
+ * @param portHandle Handle to the port to initialize.
+ * @return the created analog input handle
+ */
+HAL_AnalogInputHandle HAL_InitializeAnalogInputPort(HAL_PortHandle portHandle,
+ int32_t* status);
+
+/**
+ * Frees an analog input port.
+ *
+ * @param analogPortHandle Handle to the analog port.
+ */
+void HAL_FreeAnalogInputPort(HAL_AnalogInputHandle analogPortHandle);
+
+/**
+ * Checks that the analog module number is valid.
+ *
+ * @param module The analog module number.
+ * @return Analog module is valid and present
+ */
+HAL_Bool HAL_CheckAnalogModule(int32_t module);
+
+/**
+ * Checks that the analog output channel number is value.
+ * Verifies that the analog channel number is one of the legal channel numbers.
+ * Channel numbers are 0-based.
+ *
+ * @param channel The analog output channel number.
+ * @return Analog channel is valid
+ */
+HAL_Bool HAL_CheckAnalogInputChannel(int32_t channel);
+
+/**
+ * Indicates the analog input is used by a simulated device.
+ *
+ * @param handle the analog input handle
+ * @param device simulated device handle
+ */
+void HAL_SetAnalogInputSimDevice(HAL_AnalogInputHandle handle,
+ HAL_SimDeviceHandle device);
+
+/**
+ * Sets the sample rate.
+ *
+ * This is a global setting for the Athena and effects all channels.
+ *
+ * @param samplesPerSecond The number of samples per channel per second.
+ */
+void HAL_SetAnalogSampleRate(double samplesPerSecond, int32_t* status);
+
+/**
+ * Gets the current sample rate.
+ *
+ * This assumes one entry in the scan list.
+ * This is a global setting for the Athena and effects all channels.
+ *
+ * @return Sample rate.
+ */
+double HAL_GetAnalogSampleRate(int32_t* status);
+
+/**
+ * Sets the number of averaging bits.
+ *
+ * This sets the number of averaging bits. The actual number of averaged samples
+ * is 2**bits. Use averaging to improve the stability of your measurement at the
+ * expense of sampling rate. The averaging is done automatically in the FPGA.
+ *
+ * @param analogPortHandle Handle to the analog port to configure.
+ * @param bits Number of bits to average.
+ */
+void HAL_SetAnalogAverageBits(HAL_AnalogInputHandle analogPortHandle,
+ int32_t bits, int32_t* status);
+
+/**
+ * Gets the number of averaging bits.
+ *
+ * This gets the number of averaging bits from the FPGA. The actual number of
+ * averaged samples is 2**bits. The averaging is done automatically in the FPGA.
+ *
+ * @param analogPortHandle Handle to the analog port to use.
+ * @return Bits to average.
+ */
+int32_t HAL_GetAnalogAverageBits(HAL_AnalogInputHandle analogPortHandle,
+ int32_t* status);
+
+/**
+ * Sets the number of oversample bits.
+ *
+ * This sets the number of oversample bits. The actual number of oversampled
+ * values is 2**bits. Use oversampling to improve the resolution of your
+ * measurements at the expense of sampling rate. The oversampling is done
+ * automatically in the FPGA.
+ *
+ * @param analogPortHandle Handle to the analog port to use.
+ * @param bits Number of bits to oversample.
+ */
+void HAL_SetAnalogOversampleBits(HAL_AnalogInputHandle analogPortHandle,
+ int32_t bits, int32_t* status);
+
+/**
+ * Gets the number of oversample bits.
+ *
+ * This gets the number of oversample bits from the FPGA. The actual number of
+ * oversampled values is 2**bits. The oversampling is done automatically in the
+ * FPGA.
+ *
+ * @param analogPortHandle Handle to the analog port to use.
+ * @return Bits to oversample.
+ */
+int32_t HAL_GetAnalogOversampleBits(HAL_AnalogInputHandle analogPortHandle,
+ int32_t* status);
+
+/**
+ * Gets a sample straight from the channel on this module.
+ *
+ * The sample is a 12-bit value representing the 0V to 5V range of the A/D
+ * converter in the module. The units are in A/D converter codes. Use
+ * GetVoltage() to get the analog value in calibrated units.
+ *
+ * @param analogPortHandle Handle to the analog port to use.
+ * @return A sample straight from the channel on this module.
+ */
+int32_t HAL_GetAnalogValue(HAL_AnalogInputHandle analogPortHandle,
+ int32_t* status);
+
+/**
+ * Gets a sample from the output of the oversample and average engine for the
+ * channel.
+ *
+ * The sample is 12-bit + the value configured in SetOversampleBits().
+ * The value configured in SetAverageBits() will cause this value to be averaged
+ * 2**bits number of samples. This is not a sliding window. The sample will not
+ * change until 2**(OversamplBits + AverageBits) samples have been acquired from
+ * the module on this channel. Use GetAverageVoltage() to get the analog value
+ * in calibrated units.
+ *
+ * @param analogPortHandle Handle to the analog port to use.
+ * @return A sample from the oversample and average engine for the channel.
+ */
+int32_t HAL_GetAnalogAverageValue(HAL_AnalogInputHandle analogPortHandle,
+ int32_t* status);
+
+/**
+ * Converts a voltage to a raw value for a specified channel.
+ *
+ * This process depends on the calibration of each channel, so the channel must
+ * be specified.
+ *
+ * @todo This assumes raw values. Oversampling not supported as is.
+ *
+ * @param analogPortHandle Handle to the analog port to use.
+ * @param voltage The voltage to convert.
+ * @return The raw value for the channel.
+ */
+int32_t HAL_GetAnalogVoltsToValue(HAL_AnalogInputHandle analogPortHandle,
+ double voltage, int32_t* status);
+
+/**
+ * Gets a scaled sample straight from the channel on this module.
+ *
+ * The value is scaled to units of Volts using the calibrated scaling data from
+ * GetLSBWeight() and GetOffset().
+ *
+ * @param analogPortHandle Handle to the analog port to use.
+ * @return A scaled sample straight from the channel on this module.
+ */
+double HAL_GetAnalogVoltage(HAL_AnalogInputHandle analogPortHandle,
+ int32_t* status);
+
+/**
+ * Gets a scaled sample from the output of the oversample and average engine for
+ * the channel.
+ *
+ * The value is scaled to units of Volts using the calibrated scaling data from
+ * GetLSBWeight() and GetOffset(). Using oversampling will cause this value to
+ * be higher resolution, but it will update more slowly. Using averaging will
+ * cause this value to be more stable, but it will update more slowly.
+ *
+ * @param analogPortHandle Handle to the analog port to use.
+ * @return A scaled sample from the output of the oversample and average engine
+ * for the channel.
+ */
+double HAL_GetAnalogAverageVoltage(HAL_AnalogInputHandle analogPortHandle,
+ int32_t* status);
+
+/**
+ * Gets the factory scaling least significant bit weight constant.
+ * The least significant bit weight constant for the channel that was calibrated
+ * in manufacturing and stored in an eeprom in the module.
+ *
+ * Volts = ((LSB_Weight * 1e-9) * raw) - (Offset * 1e-9)
+ *
+ * @param analogPortHandle Handle to the analog port to use.
+ * @return Least significant bit weight.
+ */
+int32_t HAL_GetAnalogLSBWeight(HAL_AnalogInputHandle analogPortHandle,
+ int32_t* status);
+
+/**
+ * Gets the factory scaling offset constant.
+ * The offset constant for the channel that was calibrated in manufacturing and
+ * stored in an eeprom in the module.
+ *
+ * Volts = ((LSB_Weight * 1e-9) * raw) - (Offset * 1e-9)
+ *
+ * @param analogPortHandle Handle to the analog port to use.
+ * @return Offset constant.
+ */
+int32_t HAL_GetAnalogOffset(HAL_AnalogInputHandle analogPortHandle,
+ int32_t* status);
+
+/**
+ * Get the analog voltage from a raw value.
+ *
+ * @param analogPortHandle Handle to the analog port the values were read from.
+ * @param rawValue The raw analog value
+ * @return The voltage relating to the value
+ */
+double HAL_GetAnalogValueToVolts(HAL_AnalogInputHandle analogPortHandle,
+ int32_t rawValue, int32_t* status);
+#ifdef __cplusplus
+} // extern "C"
+#endif
+/** @} */
diff --git a/hal/src/main/native/include/hal/AnalogOutput.h b/hal/src/main/native/include/hal/AnalogOutput.h
new file mode 100644
index 0000000..ba0d93b
--- /dev/null
+++ b/hal/src/main/native/include/hal/AnalogOutput.h
@@ -0,0 +1,70 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 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 <stdint.h>
+
+#include "hal/Types.h"
+
+/**
+ * @defgroup hal_analogoutput Analog Output Functions
+ * @ingroup hal_capi
+ * @{
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * Initializes the analog output port using the given port object.
+ *
+ * @param handle handle to the port
+ * @return the created analog output handle
+ */
+HAL_AnalogOutputHandle HAL_InitializeAnalogOutputPort(HAL_PortHandle portHandle,
+ int32_t* status);
+
+/**
+ * Frees an analog output port.
+ *
+ * @param analogOutputHandle the analog output handle
+ */
+void HAL_FreeAnalogOutputPort(HAL_AnalogOutputHandle analogOutputHandle);
+
+/**
+ * Sets an analog output value.
+ *
+ * @param analogOutputHandle the analog output handle
+ * @param voltage the voltage (0-5v) to output
+ */
+void HAL_SetAnalogOutput(HAL_AnalogOutputHandle analogOutputHandle,
+ double voltage, int32_t* status);
+
+/**
+ * Gets the current analog output value.
+ *
+ * @param analogOutputHandle the analog output handle
+ * @return the current output voltage (0-5v)
+ */
+double HAL_GetAnalogOutput(HAL_AnalogOutputHandle analogOutputHandle,
+ int32_t* status);
+
+/**
+ * Checks that the analog output channel number is value.
+ *
+ * Verifies that the analog channel number is one of the legal channel numbers.
+ * Channel numbers are 0-based.
+ *
+ * @return Analog channel is valid
+ */
+HAL_Bool HAL_CheckAnalogOutputChannel(int32_t channel);
+#ifdef __cplusplus
+} // extern "C"
+#endif
+/** @} */
diff --git a/hal/src/main/native/include/hal/AnalogTrigger.h b/hal/src/main/native/include/hal/AnalogTrigger.h
new file mode 100644
index 0000000..85d7680
--- /dev/null
+++ b/hal/src/main/native/include/hal/AnalogTrigger.h
@@ -0,0 +1,168 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 <stdint.h>
+
+#include "hal/Types.h"
+
+/**
+ * @defgroup hal_analogtrigger Analog Trigger Functions
+ * @ingroup hal_capi
+ * @{
+ */
+
+// clang-format off
+/**
+ * The type of analog trigger to trigger on.
+ */
+HAL_ENUM(HAL_AnalogTriggerType) {
+ HAL_Trigger_kInWindow = 0,
+ HAL_Trigger_kState = 1,
+ HAL_Trigger_kRisingPulse = 2,
+ HAL_Trigger_kFallingPulse = 3
+};
+// clang-format on
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * Initializes an analog trigger.
+ *
+ * @param portHandle the analog input to use for triggering
+ * @return the created analog trigger handle
+ */
+HAL_AnalogTriggerHandle HAL_InitializeAnalogTrigger(
+ HAL_AnalogInputHandle portHandle, int32_t* status);
+
+/**
+ * Initializes an analog trigger with a Duty Cycle input
+ *
+ */
+HAL_AnalogTriggerHandle HAL_InitializeAnalogTriggerDutyCycle(
+ HAL_DutyCycleHandle dutyCycleHandle, int32_t* status);
+
+/**
+ * Frees an analog trigger.
+ *
+ * @param analogTriggerHandle the trigger handle
+ */
+void HAL_CleanAnalogTrigger(HAL_AnalogTriggerHandle analogTriggerHandle,
+ int32_t* status);
+
+/**
+ * Sets the raw ADC upper and lower limits of the analog trigger.
+ *
+ * HAL_SetAnalogTriggerLimitsVoltage or HAL_SetAnalogTriggerLimitsDutyCycle
+ * is likely better in most cases.
+ *
+ * @param analogTriggerHandle the trigger handle
+ * @param lower the lower ADC value
+ * @param upper the upper ADC value
+ */
+void HAL_SetAnalogTriggerLimitsRaw(HAL_AnalogTriggerHandle analogTriggerHandle,
+ int32_t lower, int32_t upper,
+ int32_t* status);
+
+/**
+ * Sets the upper and lower limits of the analog trigger.
+ *
+ * The limits are given as floating point voltage values.
+ *
+ * @param analogTriggerHandle the trigger handle
+ * @param lower the lower voltage value
+ * @param upper the upper voltage value
+ */
+void HAL_SetAnalogTriggerLimitsVoltage(
+ HAL_AnalogTriggerHandle analogTriggerHandle, double lower, double upper,
+ int32_t* status);
+
+void HAL_SetAnalogTriggerLimitsDutyCycle(
+ HAL_AnalogTriggerHandle analogTriggerHandle, double lower, double upper,
+ int32_t* status);
+
+/**
+ * Configures the analog trigger to use the averaged vs. raw values.
+ *
+ * If the value is true, then the averaged value is selected for the analog
+ * trigger, otherwise the immediate value is used.
+ *
+ * This is not allowed to be used if filtered mode is set.
+ * This is not allowed to be used with Duty Cycle based inputs.
+ *
+ * @param analogTriggerHandle the trigger handle
+ * @param useAveragedValue true to use averaged values, false for raw
+ */
+void HAL_SetAnalogTriggerAveraged(HAL_AnalogTriggerHandle analogTriggerHandle,
+ HAL_Bool useAveragedValue, int32_t* status);
+
+/**
+ * Configures the analog trigger to use a filtered value.
+ *
+ * The analog trigger will operate with a 3 point average rejection filter. This
+ * is designed to help with 360 degree pot applications for the period where the
+ * pot crosses through zero.
+ *
+ * This is not allowed to be used if averaged mode is set.
+ *
+ * @param analogTriggerHandle the trigger handle
+ * @param useFilteredValue true to use filtered values, false for average or
+ * raw
+ */
+void HAL_SetAnalogTriggerFiltered(HAL_AnalogTriggerHandle analogTriggerHandle,
+ HAL_Bool useFilteredValue, int32_t* status);
+
+/**
+ * Returns the InWindow output of the analog trigger.
+ *
+ * True if the analog input is between the upper and lower limits.
+ *
+ * @param analogTriggerHandle the trigger handle
+ * @return the InWindow output of the analog trigger
+ */
+HAL_Bool HAL_GetAnalogTriggerInWindow(
+ HAL_AnalogTriggerHandle analogTriggerHandle, int32_t* status);
+
+/**
+ * Returns the TriggerState output of the analog trigger.
+ *
+ * True if above upper limit.
+ * False if below lower limit.
+ * If in Hysteresis, maintain previous state.
+ *
+ * @param analogTriggerHandle the trigger handle
+ * @return the TriggerState output of the analog trigger
+ */
+HAL_Bool HAL_GetAnalogTriggerTriggerState(
+ HAL_AnalogTriggerHandle analogTriggerHandle, int32_t* status);
+
+/**
+ * Gets the state of the analog trigger output.
+ *
+ * @param analogTriggerHandle the trigger handle
+ * @param type the type of trigger to trigger on
+ * @return the state of the analog trigger output
+ */
+HAL_Bool HAL_GetAnalogTriggerOutput(HAL_AnalogTriggerHandle analogTriggerHandle,
+ HAL_AnalogTriggerType type,
+ int32_t* status);
+
+/**
+ * Get the FPGA index for the AnlogTrigger.
+ *
+ * @param analogTriggerHandle the trigger handle
+ * @return the FPGA index
+ */
+int32_t HAL_GetAnalogTriggerFPGAIndex(
+ HAL_AnalogTriggerHandle analogTriggerHandle, int32_t* status);
+#ifdef __cplusplus
+} // extern "C"
+#endif
+/** @} */
diff --git a/hal/src/main/native/include/hal/CAN.h b/hal/src/main/native/include/hal/CAN.h
new file mode 100644
index 0000000..8cc9c17
--- /dev/null
+++ b/hal/src/main/native/include/hal/CAN.h
@@ -0,0 +1,124 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 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 <stdint.h>
+
+#include "hal/Types.h"
+
+/**
+ * @defgroup hal_canstream CAN Stream Functions
+ * @ingroup hal_capi
+ * @{
+ */
+
+// These are copies of defines located in CANSessionMux.h prepended with HAL_
+
+#define HAL_CAN_SEND_PERIOD_NO_REPEAT 0
+#define HAL_CAN_SEND_PERIOD_STOP_REPEATING -1
+
+/* Flags in the upper bits of the messageID */
+#define HAL_CAN_IS_FRAME_REMOTE 0x80000000
+#define HAL_CAN_IS_FRAME_11BIT 0x40000000
+
+#define HAL_ERR_CANSessionMux_InvalidBuffer -44086
+#define HAL_ERR_CANSessionMux_MessageNotFound -44087
+#define HAL_WARN_CANSessionMux_NoToken 44087
+#define HAL_ERR_CANSessionMux_NotAllowed -44088
+#define HAL_ERR_CANSessionMux_NotInitialized -44089
+#define HAL_ERR_CANSessionMux_SessionOverrun 44050
+
+/**
+ * Storage for CAN Stream Messages.
+ */
+struct HAL_CANStreamMessage {
+ uint32_t messageID;
+ uint32_t timeStamp;
+ uint8_t data[8];
+ uint8_t dataSize;
+};
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * Sends a CAN message.
+ *
+ * @param messageID the CAN ID to send
+ * @param data the data to send (0-8 bytes)
+ * @param dataSize the size of the data to send (0-8 bytes)
+ * @param periodMs the period to repeat the packet at. Use
+ * HAL_CAN_SEND_PERIOD_NO_REPEAT to not repeat.
+ */
+void HAL_CAN_SendMessage(uint32_t messageID, const uint8_t* data,
+ uint8_t dataSize, int32_t periodMs, int32_t* status);
+
+/**
+ * Receives a CAN message.
+ *
+ * @param messageID store for the received message ID
+ * @param messageIDMask the message ID mask to look for
+ * @param data data output (8 bytes)
+ * @param dataSize data length (0-8 bytes)
+ * @param timeStamp the packet received timestamp (based off of
+ * CLOCK_MONOTONIC)
+ */
+void HAL_CAN_ReceiveMessage(uint32_t* messageID, uint32_t messageIDMask,
+ uint8_t* data, uint8_t* dataSize,
+ uint32_t* timeStamp, int32_t* status);
+
+/**
+ * Opens a CAN stream.
+ *
+ * @param sessionHandle output for the session handle
+ * @param messageID the message ID to read
+ * @param messageIDMask the mssage ID mask
+ * @param maxMessages the maximum number of messages to stream
+ */
+void HAL_CAN_OpenStreamSession(uint32_t* sessionHandle, uint32_t messageID,
+ uint32_t messageIDMask, uint32_t maxMessages,
+ int32_t* status);
+
+/**
+ * Closes a CAN stream.
+ *
+ * @param sessionHandle the session to close
+ */
+void HAL_CAN_CloseStreamSession(uint32_t sessionHandle);
+
+/**
+ * Reads a CAN stream message.
+ *
+ * @param sessionHandle the session handle
+ * @param messages array of messages
+ * @param messagesToRead the max number of messages to read
+ * @param messageRead the number of messages actually read
+ */
+void HAL_CAN_ReadStreamSession(uint32_t sessionHandle,
+ struct HAL_CANStreamMessage* messages,
+ uint32_t messagesToRead, uint32_t* messagesRead,
+ int32_t* status);
+
+/**
+ * Gets CAN status information.
+ *
+ * @param percentBusUtilization the bus utilization
+ * @param busOffCount the number of bus off errors
+ * @param txFullCount the number of tx full errors
+ * @param receiveErrorCount the number of receive errors
+ * @param transmitErrorCount the number of transmit errors
+ */
+void HAL_CAN_GetCANStatus(float* percentBusUtilization, uint32_t* busOffCount,
+ uint32_t* txFullCount, uint32_t* receiveErrorCount,
+ uint32_t* transmitErrorCount, int32_t* status);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
+/** @} */
diff --git a/hal/src/main/native/include/hal/CANAPI.h b/hal/src/main/native/include/hal/CANAPI.h
new file mode 100644
index 0000000..7a6bd88
--- /dev/null
+++ b/hal/src/main/native/include/hal/CANAPI.h
@@ -0,0 +1,154 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 <stdint.h>
+
+#include "hal/CANAPITypes.h"
+#include "hal/Types.h"
+
+/**
+ * @defgroup hal_canapi CAN API Functions
+ * @ingroup hal_capi
+ * @{
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * Initializes a CAN device.
+ *
+ * These follow the FIRST standard CAN layout. Link TBD
+ *
+ * @param manufacturer the can manufacturer
+ * @param deviceId the device ID (0-63)
+ * @param deviceType the device type
+ * @return the created CAN handle
+ */
+HAL_CANHandle HAL_InitializeCAN(HAL_CANManufacturer manufacturer,
+ int32_t deviceId, HAL_CANDeviceType deviceType,
+ int32_t* status);
+
+/**
+ * Frees a CAN device
+ *
+ * @param handle the CAN handle
+ */
+void HAL_CleanCAN(HAL_CANHandle handle);
+
+/**
+ * Writes a packet to the CAN device with a specific ID.
+ *
+ * This ID is 10 bits.
+ *
+ * @param handle the CAN handle
+ * @param data the data to write (0-8 bytes)
+ * @param length the length of data (0-8)
+ * @param apiId the ID to write (0-1023 bits)
+ */
+void HAL_WriteCANPacket(HAL_CANHandle handle, const uint8_t* data,
+ int32_t length, int32_t apiId, int32_t* status);
+
+/**
+ * Writes a repeating packet to the CAN device with a specific ID.
+ *
+ * This ID is 10 bits.
+ *
+ * The RoboRIO will automatically repeat the packet at the specified interval
+ *
+ * @param handle the CAN handle
+ * @param data the data to write (0-8 bytes)
+ * @param length the length of data (0-8)
+ * @param apiId the ID to write (0-1023)
+ * @param repeatMs the period to repeat in ms
+ */
+void HAL_WriteCANPacketRepeating(HAL_CANHandle handle, const uint8_t* data,
+ int32_t length, int32_t apiId,
+ int32_t repeatMs, int32_t* status);
+
+/**
+ * Writes an RTR frame of the specified length to the CAN device with the
+ * specific ID.
+ *
+ * By spec, the length must be equal to the length sent by the other device,
+ * otherwise behavior is unspecified.
+ *
+ * @param handle the CAN handle
+ * @param length the length of data to request (0-8)
+ * @param apiId the ID to write (0-1023)
+ */
+void HAL_WriteCANRTRFrame(HAL_CANHandle handle, int32_t length, int32_t apiId,
+ int32_t* status);
+
+/**
+ * Stops a repeating packet with a specific ID.
+ *
+ * This ID is 10 bits.
+ *
+ * @param handle the CAN handle
+ * @param apiId the ID to stop repeating (0-1023)
+ */
+void HAL_StopCANPacketRepeating(HAL_CANHandle handle, int32_t apiId,
+ int32_t* status);
+
+/**
+ * Reads a new CAN packet.
+ *
+ * This will only return properly once per packet received. Multiple calls
+ * without receiving another packet will return an error code.
+ *
+ * @param handle the CAN handle
+ * @param apiId the ID to read (0-1023)
+ * @param data the packet data (8 bytes)
+ * @param length the received length (0-8 bytes)
+ * @param receivedTimestamp the packet received timestamp (based off of
+ * CLOCK_MONOTONIC)
+ */
+void HAL_ReadCANPacketNew(HAL_CANHandle handle, int32_t apiId, uint8_t* data,
+ int32_t* length, uint64_t* receivedTimestamp,
+ int32_t* status);
+
+/**
+ * Reads a CAN packet. The will continuously return the last packet received,
+ * without accounting for packet age.
+ *
+ * @param handle the CAN handle
+ * @param apiId the ID to read (0-1023)
+ * @param data the packet data (8 bytes)
+ * @param length the received length (0-8 bytes)
+ * @param receivedTimestamp the packet received timestamp (based off of
+ * CLOCK_MONOTONIC)
+ */
+void HAL_ReadCANPacketLatest(HAL_CANHandle handle, int32_t apiId, uint8_t* data,
+ int32_t* length, uint64_t* receivedTimestamp,
+ int32_t* status);
+
+/**
+ * Reads a CAN packet. The will return the last packet received until the
+ * packet is older then the requested timeout. Then it will return an error
+ * code.
+ *
+ * @param handle the CAN handle
+ * @param apiId the ID to read (0-1023)
+ * @param data the packet data (8 bytes)
+ * @param length the received length (0-8 bytes)
+ * @param receivedTimestamp the packet received timestamp (based off of
+ * CLOCK_MONOTONIC)
+ * @param timeoutMs the timeout time for the packet
+ */
+void HAL_ReadCANPacketTimeout(HAL_CANHandle handle, int32_t apiId,
+ uint8_t* data, int32_t* length,
+ uint64_t* receivedTimestamp, int32_t timeoutMs,
+ int32_t* status);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
+/** @} */
diff --git a/hal/src/main/native/include/hal/CANAPITypes.h b/hal/src/main/native/include/hal/CANAPITypes.h
new file mode 100644
index 0000000..a23cee1
--- /dev/null
+++ b/hal/src/main/native/include/hal/CANAPITypes.h
@@ -0,0 +1,62 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 <stdint.h>
+
+#include "hal/Types.h"
+
+/**
+ * @defgroup hal_canapi CAN API Functions
+ * @ingroup hal_capi
+ * @{
+ */
+
+// clang-format off
+/**
+ * The CAN device type.
+ *
+ * Teams should use HAL_CAN_Dev_kMiscellaneous
+ */
+HAL_ENUM(HAL_CANDeviceType) {
+ HAL_CAN_Dev_kBroadcast = 0,
+ HAL_CAN_Dev_kRobotController = 1,
+ HAL_CAN_Dev_kMotorController = 2,
+ HAL_CAN_Dev_kRelayController = 3,
+ HAL_CAN_Dev_kGyroSensor = 4,
+ HAL_CAN_Dev_kAccelerometer = 5,
+ HAL_CAN_Dev_kUltrasonicSensor = 6,
+ HAL_CAN_Dev_kGearToothSensor = 7,
+ HAL_CAN_Dev_kPowerDistribution = 8,
+ HAL_CAN_Dev_kPneumatics = 9,
+ HAL_CAN_Dev_kMiscellaneous = 10,
+ HAL_CAN_Dev_kFirmwareUpdate = 31
+};
+
+/**
+ * The CAN manufacturer ID.
+ *
+ * Teams should use HAL_CAN_Man_kTeamUse.
+ */
+HAL_ENUM(HAL_CANManufacturer) {
+ HAL_CAN_Man_kBroadcast = 0,
+ HAL_CAN_Man_kNI = 1,
+ HAL_CAN_Man_kLM = 2,
+ HAL_CAN_Man_kDEKA = 3,
+ HAL_CAN_Man_kCTRE = 4,
+ HAL_CAN_Man_kREV = 5,
+ HAL_CAN_Man_kGrapple = 6,
+ HAL_CAN_Man_kMS = 7,
+ HAL_CAN_Man_kTeamUse = 8,
+ HAL_CAN_Man_kKauaiLabs = 9,
+ HAL_CAN_Man_kCopperforge = 10,
+ HAL_CAN_Man_kPWF = 11,
+ HAL_CAN_Man_kStudica = 12
+};
+// clang-format on
+/** @} */
diff --git a/hal/src/main/native/include/hal/ChipObject.h b/hal/src/main/native/include/hal/ChipObject.h
new file mode 100644
index 0000000..d891ced
--- /dev/null
+++ b/hal/src/main/native/include/hal/ChipObject.h
@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wpedantic"
+#pragma GCC diagnostic ignored "-Wignored-qualifiers"
+
+#include <stdint.h>
+
+#include <FRC_FPGA_ChipObject/RoboRIO_FRC_ChipObject_Aliases.h>
+#include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/nInterfaceGlobals.h>
+#include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tAI.h>
+#include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tAO.h>
+#include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tAccel.h>
+#include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tAccumulator.h>
+#include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tAlarm.h>
+#include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tAnalogTrigger.h>
+#include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tBIST.h>
+#include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tCounter.h>
+#include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tDIO.h>
+#include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tDMA.h>
+#include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tDutyCycle.h>
+#include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tEncoder.h>
+#include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tGlobal.h>
+#include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tHMB.h>
+#include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tInterrupt.h>
+#include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tLED.h>
+#include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tPWM.h>
+#include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tPower.h>
+#include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tRelay.h>
+#include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tSPI.h>
+#include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tSysWatchdog.h>
+#include <FRC_FPGA_ChipObject/tDMAChannelDescriptor.h>
+#include <FRC_FPGA_ChipObject/tDMAManager.h>
+#include <FRC_FPGA_ChipObject/tInterruptManager.h>
+#include <FRC_FPGA_ChipObject/tSystem.h>
+#include <FRC_FPGA_ChipObject/tSystemInterface.h>
+
+namespace hal {
+using namespace nFPGA;
+using namespace nRoboRIO_FPGANamespace;
+} // namespace hal
+
+#pragma GCC diagnostic pop
diff --git a/hal/src/main/native/include/hal/Compressor.h b/hal/src/main/native/include/hal/Compressor.h
new file mode 100644
index 0000000..cb58e46
--- /dev/null
+++ b/hal/src/main/native/include/hal/Compressor.h
@@ -0,0 +1,143 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 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 <stdint.h>
+
+#include "hal/Types.h"
+
+/**
+ * @defgroup hal_compressor Compressor Functions
+ * @ingroup hal_capi
+ * @{
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * Initializes a compressor on the given PCM module.
+ *
+ * @param module the module number
+ * @return the created handle
+ */
+HAL_CompressorHandle HAL_InitializeCompressor(int32_t module, int32_t* status);
+
+/**
+ * Gets if a compressor module is valid.
+ *
+ * @param module the module number
+ * @return true if the module is valid, otherwise false
+ */
+HAL_Bool HAL_CheckCompressorModule(int32_t module);
+
+/**
+ * Gets the compressor state (on or off).
+ *
+ * @param compressorHandle the compressor handle
+ * @return true if the compressor is on, otherwise false
+ */
+HAL_Bool HAL_GetCompressor(HAL_CompressorHandle compressorHandle,
+ int32_t* status);
+
+/**
+ * Sets the compressor to closed loop mode.
+ *
+ * @param compressorHandle the compressor handle
+ * @param value true for closed loop mode, false for off
+ */
+void HAL_SetCompressorClosedLoopControl(HAL_CompressorHandle compressorHandle,
+ HAL_Bool value, int32_t* status);
+
+/**
+ * Gets if the compressor is in closed loop mode.
+ *
+ * @param compressorHandle the compressor handle
+ * @return true if the compressor is in closed loop mode,
+ * otherwise false
+ */
+HAL_Bool HAL_GetCompressorClosedLoopControl(
+ HAL_CompressorHandle compressorHandle, int32_t* status);
+
+/**
+ * Gets the compressor pressure switch state.
+ *
+ * @param compressorHandle the compressor handle
+ * @return true if the pressure switch is triggered, otherwise
+ * false
+ */
+HAL_Bool HAL_GetCompressorPressureSwitch(HAL_CompressorHandle compressorHandle,
+ int32_t* status);
+
+/**
+ * Gets the compressor current.
+ *
+ * @param compressorHandle the compressor handle
+ * @return the compressor current in amps
+ */
+double HAL_GetCompressorCurrent(HAL_CompressorHandle compressorHandle,
+ int32_t* status);
+
+/**
+ * Gets if the compressor is faulted because of too high of current.
+ *
+ * @param compressorHandle the compressor handle
+ * @return true if falted, otherwise false
+ */
+HAL_Bool HAL_GetCompressorCurrentTooHighFault(
+ HAL_CompressorHandle compressorHandle, int32_t* status);
+
+/**
+ * Gets if a sticky fauly is triggered because of too high of current.
+ *
+ * @param compressorHandle the compressor handle
+ * @return true if falted, otherwise false
+ */
+HAL_Bool HAL_GetCompressorCurrentTooHighStickyFault(
+ HAL_CompressorHandle compressorHandle, int32_t* status);
+
+/**
+ * Gets if a sticky fauly is triggered because of a short.
+ *
+ * @param compressorHandle the compressor handle
+ * @return true if falted, otherwise false
+ */
+HAL_Bool HAL_GetCompressorShortedStickyFault(
+ HAL_CompressorHandle compressorHandle, int32_t* status);
+
+/**
+ * Gets if the compressor is faulted because of a short.
+ *
+ * @param compressorHandle the compressor handle
+ * @return true if shorted, otherwise false
+ */
+HAL_Bool HAL_GetCompressorShortedFault(HAL_CompressorHandle compressorHandle,
+ int32_t* status);
+
+/**
+ * Gets if a sticky fault is triggered of the compressor not connected.
+ *
+ * @param compressorHandle the compressor handle
+ * @return true if falted, otherwise false
+ */
+HAL_Bool HAL_GetCompressorNotConnectedStickyFault(
+ HAL_CompressorHandle compressorHandle, int32_t* status);
+
+/**
+ * Gets if the compressor is not connected.
+ *
+ * @param compressorHandle the compressor handle
+ * @return true if not connected, otherwise false
+ */
+HAL_Bool HAL_GetCompressorNotConnectedFault(
+ HAL_CompressorHandle compressorHandle, int32_t* status);
+#ifdef __cplusplus
+} // extern "C"
+#endif
+/** @} */
diff --git a/hal/src/main/native/include/hal/Constants.h b/hal/src/main/native/include/hal/Constants.h
new file mode 100644
index 0000000..2a4d409
--- /dev/null
+++ b/hal/src/main/native/include/hal/Constants.h
@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 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 <stdint.h>
+
+/**
+ * @defgroup hal_constants Constants Functions
+ * @ingroup hal_capi
+ * @{
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * Gets the number of FPGA system clock ticks per microsecond.
+ *
+ * @return the number of clock ticks per microsecond
+ */
+int32_t HAL_GetSystemClockTicksPerMicrosecond(void);
+#ifdef __cplusplus
+} // extern "C"
+#endif
+/** @} */
diff --git a/hal/src/main/native/include/hal/Counter.h b/hal/src/main/native/include/hal/Counter.h
new file mode 100644
index 0000000..c0dd057
--- /dev/null
+++ b/hal/src/main/native/include/hal/Counter.h
@@ -0,0 +1,307 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 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 <stdint.h>
+
+#include "hal/AnalogTrigger.h"
+#include "hal/Types.h"
+
+/**
+ * @defgroup hal_counter Counter Functions
+ * @ingroup hal_capi
+ * @{
+ */
+
+// clang-format off
+/**
+ * The counter mode.
+ */
+HAL_ENUM(HAL_Counter_Mode) {
+ HAL_Counter_kTwoPulse = 0,
+ HAL_Counter_kSemiperiod = 1,
+ HAL_Counter_kPulseLength = 2,
+ HAL_Counter_kExternalDirection = 3
+};
+// clang-format on
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * Initializes a counter.
+ *
+ * @param mode the counter mode
+ * @param index the compressor index (output)
+ * @return the created handle
+ */
+HAL_CounterHandle HAL_InitializeCounter(HAL_Counter_Mode mode, int32_t* index,
+ int32_t* status);
+
+/**
+ * Frees a counter.
+ *
+ * @param counterHandle the counter handle
+ */
+void HAL_FreeCounter(HAL_CounterHandle counterHandle, int32_t* status);
+
+/**
+ * Sets the average sample size of a counter.
+ *
+ * @param counterHandle the counter handle
+ * @param size the size of samples to average
+ */
+void HAL_SetCounterAverageSize(HAL_CounterHandle counterHandle, int32_t size,
+ int32_t* status);
+
+/**
+ * Sets the source object that causes the counter to count up.
+ *
+ * @param counterHandle the counter handle
+ * @param digitalSourceHandle the digital source handle (either a
+ * HAL_AnalogTriggerHandle of a HAL_DigitalHandle)
+ * @param analogTriggerType the analog trigger type if the source is an analog
+ * trigger
+ */
+void HAL_SetCounterUpSource(HAL_CounterHandle counterHandle,
+ HAL_Handle digitalSourceHandle,
+ HAL_AnalogTriggerType analogTriggerType,
+ int32_t* status);
+
+/**
+ * Sets the up source to either detect rising edges or falling edges.
+ *
+ * Note that both are allowed to be set true at the same time without issues.
+ *
+ * @param counterHandle the counter handle
+ * @param risingEdge true to trigger on rising
+ * @param fallingEdge true to trigger on falling
+ */
+void HAL_SetCounterUpSourceEdge(HAL_CounterHandle counterHandle,
+ HAL_Bool risingEdge, HAL_Bool fallingEdge,
+ int32_t* status);
+
+/**
+ * Disables the up counting source to the counter.
+ *
+ * @param counterHandle the counter handle
+ */
+void HAL_ClearCounterUpSource(HAL_CounterHandle counterHandle, int32_t* status);
+
+/**
+ * Sets the source object that causes the counter to count down.
+ *
+ * @param counterHandle the counter handle
+ * @param digitalSourceHandle the digital source handle (either a
+ * HAL_AnalogTriggerHandle of a HAL_DigitalHandle)
+ * @param analogTriggerType the analog trigger type if the source is an analog
+ * trigger
+ */
+void HAL_SetCounterDownSource(HAL_CounterHandle counterHandle,
+ HAL_Handle digitalSourceHandle,
+ HAL_AnalogTriggerType analogTriggerType,
+ int32_t* status);
+
+/**
+ * Sets the down source to either detect rising edges or falling edges.
+ * Note that both are allowed to be set true at the same time without issues.
+ *
+ * @param counterHandle the counter handle
+ * @param risingEdge true to trigger on rising
+ * @param fallingEdge true to trigger on falling
+ */
+void HAL_SetCounterDownSourceEdge(HAL_CounterHandle counterHandle,
+ HAL_Bool risingEdge, HAL_Bool fallingEdge,
+ int32_t* status);
+
+/**
+ * Disables the down counting source to the counter.
+ *
+ * @param counterHandle the counter handle
+ */
+void HAL_ClearCounterDownSource(HAL_CounterHandle counterHandle,
+ int32_t* status);
+
+/**
+ * Sets standard up / down counting mode on this counter.
+ *
+ * Up and down counts are sourced independently from two inputs.
+ *
+ * @param counterHandle the counter handle
+ */
+void HAL_SetCounterUpDownMode(HAL_CounterHandle counterHandle, int32_t* status);
+
+/**
+ * Sets directional counting mode on this counter.
+ *
+ * The direction is determined by the B input, with counting happening with the
+ * A input.
+ *
+ * @param counterHandle the counter handle
+ */
+void HAL_SetCounterExternalDirectionMode(HAL_CounterHandle counterHandle,
+ int32_t* status);
+
+/**
+ * Sets Semi-period mode on this counter.
+ *
+ * The counter counts up based on the time the input is triggered. High or Low
+ * depends on the highSemiPeriod parameter.
+ *
+ * @param counterHandle the counter handle
+ * @param highSemiPeriod true for counting when the input is high, false for low
+ */
+void HAL_SetCounterSemiPeriodMode(HAL_CounterHandle counterHandle,
+ HAL_Bool highSemiPeriod, int32_t* status);
+
+/**
+ * Configures the counter to count in up or down based on the length of the
+ * input pulse.
+ *
+ * This mode is most useful for direction sensitive gear tooth sensors.
+ *
+ * @param counterHandle the counter handle
+ * @param threshold The pulse length beyond which the counter counts the
+ * opposite direction (seconds)
+ */
+void HAL_SetCounterPulseLengthMode(HAL_CounterHandle counterHandle,
+ double threshold, int32_t* status);
+
+/**
+ * Gets the Samples to Average which specifies the number of samples of the
+ * timer to average when calculating the period. Perform averaging to account
+ * for mechanical imperfections or as oversampling to increase resolution.
+ *
+ * @param counterHandle the counter handle
+ * @return SamplesToAverage The number of samples being averaged (from 1 to 127)
+ */
+int32_t HAL_GetCounterSamplesToAverage(HAL_CounterHandle counterHandle,
+ int32_t* status);
+
+/**
+ * Sets the Samples to Average which specifies the number of samples of the
+ * timer to average when calculating the period. Perform averaging to account
+ * for mechanical imperfections or as oversampling to increase resolution.
+ *
+ * @param counterHandle the counter handle
+ * @param samplesToAverage The number of samples to average from 1 to 127
+ */
+void HAL_SetCounterSamplesToAverage(HAL_CounterHandle counterHandle,
+ int32_t samplesToAverage, int32_t* status);
+
+/**
+ * Resets the Counter to zero.
+ *
+ * Sets the counter value to zero. This does not effect the running state of the
+ * counter, just sets the current value to zero.
+ *
+ * @param counterHandle the counter handle
+ */
+void HAL_ResetCounter(HAL_CounterHandle counterHandle, int32_t* status);
+
+/**
+ * Reads the current counter value.
+ *
+ * Reads the value at this instant. It may still be running, so it reflects the
+ * current value. Next time it is read, it might have a different value.
+ *
+ * @param counterHandle the counter handle
+ * @return the current counter value
+ */
+int32_t HAL_GetCounter(HAL_CounterHandle counterHandle, int32_t* status);
+
+/*
+ * Gets the Period of the most recent count.
+ *
+ * Returns the time interval of the most recent count. This can be used for
+ * velocity calculations to determine shaft speed.
+ *
+ * @param counterHandle the counter handle
+ * @returns the period of the last two pulses in units of seconds
+ */
+double HAL_GetCounterPeriod(HAL_CounterHandle counterHandle, int32_t* status);
+
+/**
+ * Sets the maximum period where the device is still considered "moving".
+ *
+ * Sets the maximum period where the device is considered moving. This value is
+ * used to determine the "stopped" state of the counter using the
+ * HAL_GetCounterStopped method.
+ *
+ * @param counterHandle the counter handle
+ * @param maxPeriod the maximum period where the counted device is
+ * considered moving in seconds
+ */
+void HAL_SetCounterMaxPeriod(HAL_CounterHandle counterHandle, double maxPeriod,
+ int32_t* status);
+
+/**
+ * Selects whether you want to continue updating the event timer output when
+ * there are no samples captured.
+ *
+ * The output of the event timer has a buffer of periods that are averaged and
+ * posted to a register on the FPGA. When the timer detects that the event
+ * source has stopped (based on the MaxPeriod) the buffer of samples to be
+ * averaged is emptied.
+ *
+ * If you enable the update when empty, you will be
+ * notified of the stopped source and the event time will report 0 samples.
+ *
+ * If you disable update when empty, the most recent average will remain on the
+ * output until a new sample is acquired.
+ *
+ * You will never see 0 samples output (except when there have been no events
+ * since an FPGA reset) and you will likely not see the stopped bit become true
+ * (since it is updated at the end of an average and there are no samples to
+ * average).
+ *
+ * @param counterHandle the counter handle
+ * @param enabled true to enable counter updating with no samples
+ */
+void HAL_SetCounterUpdateWhenEmpty(HAL_CounterHandle counterHandle,
+ HAL_Bool enabled, int32_t* status);
+
+/**
+ * Determines if the clock is stopped.
+ *
+ * Determine if the clocked input is stopped based on the MaxPeriod value set
+ * using the SetMaxPeriod method. If the clock exceeds the MaxPeriod, then the
+ * device (and counter) are assumed to be stopped and it returns true.
+ *
+ * @param counterHandle the counter handle
+ * @return true if the most recent counter period exceeds the
+ * MaxPeriod value set by SetMaxPeriod
+ */
+HAL_Bool HAL_GetCounterStopped(HAL_CounterHandle counterHandle,
+ int32_t* status);
+
+/**
+ * Gets the last direction the counter value changed.
+ *
+ * @param counterHandle the counter handle
+ * @return the last direction the counter value changed
+ */
+HAL_Bool HAL_GetCounterDirection(HAL_CounterHandle counterHandle,
+ int32_t* status);
+
+/**
+ * Sets the Counter to return reversed sensing on the direction.
+ *
+ * This allows counters to change the direction they are counting in the case of
+ * 1X and 2X quadrature encoding only. Any other counter mode isn't supported.
+ *
+ * @param counterHandle the counter handle
+ * @param reverseDirection true if the value counted should be negated.
+ */
+void HAL_SetCounterReverseDirection(HAL_CounterHandle counterHandle,
+ HAL_Bool reverseDirection, int32_t* status);
+#ifdef __cplusplus
+} // extern "C"
+#endif
+/** @} */
diff --git a/hal/src/main/native/include/hal/DIO.h b/hal/src/main/native/include/hal/DIO.h
new file mode 100644
index 0000000..6ffc71f
--- /dev/null
+++ b/hal/src/main/native/include/hal/DIO.h
@@ -0,0 +1,208 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 <stdint.h>
+
+#include "hal/Types.h"
+
+/**
+ * @defgroup hal_dio DIO Functions
+ * @ingroup hal_capi
+ * @{
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * Creates a new instance of a digital port.
+ *
+ * @param portHandle the port handle to create from
+ * @param input true for input, false for output
+ * @return the created digital handle
+ */
+HAL_DigitalHandle HAL_InitializeDIOPort(HAL_PortHandle portHandle,
+ HAL_Bool input, int32_t* status);
+
+/**
+ * Checks if a DIO channel is valid.
+ *
+ * @param channel the channel number to check
+ * @return true if the channel is correct, otherwise false
+ */
+HAL_Bool HAL_CheckDIOChannel(int32_t channel);
+
+void HAL_FreeDIOPort(HAL_DigitalHandle dioPortHandle);
+
+/**
+ * Indicates the DIO channel is used by a simulated device.
+ *
+ * @param handle the DIO channel handle
+ * @param device simulated device handle
+ */
+void HAL_SetDIOSimDevice(HAL_DigitalHandle handle, HAL_SimDeviceHandle device);
+
+/**
+ * Allocates a DO PWM Generator.
+ *
+ * @return the allocated digital PWM handle
+ */
+HAL_DigitalPWMHandle HAL_AllocateDigitalPWM(int32_t* status);
+
+/**
+ * Frees the resource associated with a DO PWM generator.
+ *
+ * @param pwmGenerator the digital PWM handle
+ */
+void HAL_FreeDigitalPWM(HAL_DigitalPWMHandle pwmGenerator, int32_t* status);
+
+/**
+ * Changes the frequency of the DO PWM generator.
+ *
+ * The valid range is from 0.6 Hz to 19 kHz.
+ *
+ * The frequency resolution is logarithmic.
+ *
+ * @param rate the frequency to output all digital output PWM signals
+ */
+void HAL_SetDigitalPWMRate(double rate, int32_t* status);
+
+/**
+ * Configures the duty-cycle of the PWM generator.
+ *
+ * @param pwmGenerator the digital PWM handle
+ * @param dutyCycle the percent duty cycle to output [0..1]
+ */
+void HAL_SetDigitalPWMDutyCycle(HAL_DigitalPWMHandle pwmGenerator,
+ double dutyCycle, int32_t* status);
+
+/**
+ * Configures which DO channel the PWM signal is output on.
+ *
+ * @param pwmGenerator the digital PWM handle
+ * @param channel the channel to output on
+ */
+void HAL_SetDigitalPWMOutputChannel(HAL_DigitalPWMHandle pwmGenerator,
+ int32_t channel, int32_t* status);
+
+/**
+ * Writes a digital value to a DIO channel.
+ *
+ * @param dioPortHandle the digital port handle
+ * @param value the state to set the digital channel (if it is
+ * configured as an output)
+ */
+void HAL_SetDIO(HAL_DigitalHandle dioPortHandle, HAL_Bool value,
+ int32_t* status);
+
+/**
+ * Sets the direction of a DIO channel.
+ *
+ * @param dioPortHandle the digital port handle
+ * @param input true to set input, false for output
+ */
+void HAL_SetDIODirection(HAL_DigitalHandle dioPortHandle, HAL_Bool input,
+ int32_t* status);
+
+/**
+ * Reads a digital value from a DIO channel.
+ *
+ * @param dioPortHandle the digital port handle
+ * @return the state of the specified channel
+ */
+HAL_Bool HAL_GetDIO(HAL_DigitalHandle dioPortHandle, int32_t* status);
+
+/**
+ * Reads the direction of a DIO channel.
+ *
+ * @param dioPortHandle the digital port handle
+ * @return true for input, false for output
+ */
+HAL_Bool HAL_GetDIODirection(HAL_DigitalHandle dioPortHandle, int32_t* status);
+
+/**
+ * Generates a single digital pulse.
+ *
+ * Write a pulse to the specified digital output channel. There can only be a
+ * single pulse going at any time.
+ *
+ * @param dioPortHandle the digital port handle
+ * @param pulseLength the active length of the pulse (in seconds)
+ */
+void HAL_Pulse(HAL_DigitalHandle dioPortHandle, double pulseLength,
+ int32_t* status);
+
+/**
+ * Checks a DIO line to see if it is currently generating a pulse.
+ *
+ * @return true if a pulse is in progress, otherwise false
+ */
+HAL_Bool HAL_IsPulsing(HAL_DigitalHandle dioPortHandle, int32_t* status);
+
+/**
+ * Checks if any DIO line is currently generating a pulse.
+ *
+ * @return true if a pulse on some line is in progress
+ */
+HAL_Bool HAL_IsAnyPulsing(int32_t* status);
+
+/**
+ * Writes the filter index from the FPGA.
+ *
+ * Set the filter index used to filter out short pulses.
+ *
+ * @param dioPortHandle the digital port handle
+ * @param filterIndex the filter index (Must be in the range 0 - 3, where 0
+ * means "none" and 1 - 3 means filter # filterIndex - 1)
+ */
+void HAL_SetFilterSelect(HAL_DigitalHandle dioPortHandle, int32_t filterIndex,
+ int32_t* status);
+
+/**
+ * Reads the filter index from the FPGA.
+ *
+ * Gets the filter index used to filter out short pulses.
+ *
+ * @param dioPortHandle the digital port handle
+ * @return filterIndex the filter index (Must be in the range 0 - 3,
+ * where 0 means "none" and 1 - 3 means filter # filterIndex - 1)
+ */
+int32_t HAL_GetFilterSelect(HAL_DigitalHandle dioPortHandle, int32_t* status);
+
+/**
+ * Sets the filter period for the specified filter index.
+ *
+ * Sets the filter period in FPGA cycles. Even though there are 2 different
+ * filter index domains (MXP vs HDR), ignore that distinction for now since it
+ * compilicates the interface. That can be changed later.
+ *
+ * @param filterIndex the filter index, 0 - 2
+ * @param value the number of cycles that the signal must not transition
+ * to be counted as a transition.
+ */
+void HAL_SetFilterPeriod(int32_t filterIndex, int64_t value, int32_t* status);
+
+/**
+ * Gets the filter period for the specified filter index.
+ *
+ * Gets the filter period in FPGA cycles. Even though there are 2 different
+ * filter index domains (MXP vs HDR), ignore that distinction for now since it
+ * compilicates the interface. Set status to NiFpga_Status_SoftwareFault if the
+ * filter values miss-match.
+ *
+ * @param filterIndex the filter index, 0 - 2
+ * @param value the number of cycles that the signal must not transition
+ * to be counted as a transition.
+ */
+int64_t HAL_GetFilterPeriod(int32_t filterIndex, int32_t* status);
+#ifdef __cplusplus
+} // extern "C"
+#endif
+/** @} */
diff --git a/hal/src/main/native/include/hal/DMA.h b/hal/src/main/native/include/hal/DMA.h
new file mode 100644
index 0000000..38aaca2
--- /dev/null
+++ b/hal/src/main/native/include/hal/DMA.h
@@ -0,0 +1,129 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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 <stdint.h>
+
+#include "hal/AnalogTrigger.h"
+#include "hal/Types.h"
+
+// clang-format off
+/**
+ * The DMA Read Status.
+ */
+HAL_ENUM(HAL_DMAReadStatus ) {
+ HAL_DMA_OK = 1,
+ HAL_DMA_TIMEOUT = 2,
+ HAL_DMA_ERROR = 3,
+};
+// clang-format on
+
+struct HAL_DMASample {
+ uint32_t readBuffer[74];
+ int32_t channelOffsets[22];
+ uint64_t timeStamp;
+ uint32_t captureSize;
+ uint8_t triggerChannels;
+};
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+HAL_DMAHandle HAL_InitializeDMA(int32_t* status);
+void HAL_FreeDMA(HAL_DMAHandle handle);
+
+void HAL_SetDMAPause(HAL_DMAHandle handle, HAL_Bool pause, int32_t* status);
+void HAL_SetDMARate(HAL_DMAHandle handle, int32_t cycles, int32_t* status);
+
+void HAL_AddDMAEncoder(HAL_DMAHandle handle, HAL_EncoderHandle encoderHandle,
+ int32_t* status);
+void HAL_AddDMAEncoderPeriod(HAL_DMAHandle handle,
+ HAL_EncoderHandle encoderHandle, int32_t* status);
+void HAL_AddDMACounter(HAL_DMAHandle handle, HAL_CounterHandle counterHandle,
+ int32_t* status);
+void HAL_AddDMACounterPeriod(HAL_DMAHandle handle,
+ HAL_CounterHandle counterHandle, int32_t* status);
+void HAL_AddDMADigitalSource(HAL_DMAHandle handle,
+ HAL_Handle digitalSourceHandle, int32_t* status);
+void HAL_AddDMAAnalogInput(HAL_DMAHandle handle,
+ HAL_AnalogInputHandle aInHandle, int32_t* status);
+
+void HAL_AddDMAAveragedAnalogInput(HAL_DMAHandle handle,
+ HAL_AnalogInputHandle aInHandle,
+ int32_t* status);
+
+void HAL_AddDMAAnalogAccumulator(HAL_DMAHandle handle,
+ HAL_AnalogInputHandle aInHandle,
+ int32_t* status);
+
+void HAL_AddDMADutyCycle(HAL_DMAHandle handle,
+ HAL_DutyCycleHandle dutyCycleHandle, int32_t* status);
+
+void HAL_SetDMAExternalTrigger(HAL_DMAHandle handle,
+ HAL_Handle digitalSourceHandle,
+ HAL_AnalogTriggerType analogTriggerType,
+ HAL_Bool rising, HAL_Bool falling,
+ int32_t* status);
+
+void HAL_StartDMA(HAL_DMAHandle handle, int32_t queueDepth, int32_t* status);
+void HAL_StopDMA(HAL_DMAHandle handle, int32_t* status);
+
+void* HAL_GetDMADirectPointer(HAL_DMAHandle handle);
+
+enum HAL_DMAReadStatus HAL_ReadDMADirect(void* dmaPointer,
+ HAL_DMASample* dmaSample,
+ int32_t timeoutMs,
+ int32_t* remainingOut,
+ int32_t* status);
+
+enum HAL_DMAReadStatus HAL_ReadDMA(HAL_DMAHandle handle,
+ HAL_DMASample* dmaSample, int32_t timeoutMs,
+ int32_t* remainingOut, int32_t* status);
+
+// Sampling Code
+uint64_t HAL_GetDMASampleTime(const HAL_DMASample* dmaSample, int32_t* status);
+
+int32_t HAL_GetDMASampleEncoderRaw(const HAL_DMASample* dmaSample,
+ HAL_EncoderHandle encoderHandle,
+ int32_t* status);
+
+int32_t HAL_GetDMASampleCounter(const HAL_DMASample* dmaSample,
+ HAL_CounterHandle counterHandle,
+ int32_t* status);
+
+int32_t HAL_GetDMASampleEncoderPeriodRaw(const HAL_DMASample* dmaSample,
+ HAL_EncoderHandle encoderHandle,
+ int32_t* status);
+
+int32_t HAL_GetDMASampleCounterPeriod(const HAL_DMASample* dmaSample,
+ HAL_CounterHandle counterHandle,
+ int32_t* status);
+HAL_Bool HAL_GetDMASampleDigitalSource(const HAL_DMASample* dmaSample,
+ HAL_Handle dSourceHandle,
+ int32_t* status);
+int32_t HAL_GetDMASampleAnalogInputRaw(const HAL_DMASample* dmaSample,
+ HAL_AnalogInputHandle aInHandle,
+ int32_t* status);
+
+int32_t HAL_GetDMASampleAveragedAnalogInputRaw(const HAL_DMASample* dmaSample,
+ HAL_AnalogInputHandle aInHandle,
+ int32_t* status);
+
+void HAL_GetDMASampleAnalogAccumulator(const HAL_DMASample* dmaSample,
+ HAL_AnalogInputHandle aInHandle,
+ int64_t* count, int64_t* value,
+ int32_t* status);
+
+int32_t HAL_GetDMASampleDutyCycleOutputRaw(const HAL_DMASample* dmaSample,
+ HAL_DutyCycleHandle dutyCycleHandle,
+ int32_t* status);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
diff --git a/hal/src/main/native/include/hal/DriverStation.h b/hal/src/main/native/include/hal/DriverStation.h
new file mode 100644
index 0000000..471c18b
--- /dev/null
+++ b/hal/src/main/native/include/hal/DriverStation.h
@@ -0,0 +1,290 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2013-2019 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 <stdint.h>
+
+#include "hal/DriverStationTypes.h"
+#include "hal/Types.h"
+
+/**
+ * @defgroup hal_driverstation Driver Station Functions
+ * @ingroup hal_capi
+ * @{
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * Sends an error to the driver station.
+ *
+ * @param isError true for error, false for warning
+ * @param errorCode the error code
+ * @param isLVCode true for a LV error code, false for a standard error code
+ * @param details the details of the error
+ * @param location the file location of the errror
+ * @param callstack the callstack of the error
+ * @param printMsg true to print the error message to stdout as well as to the
+ * DS
+ */
+int32_t HAL_SendError(HAL_Bool isError, int32_t errorCode, HAL_Bool isLVCode,
+ const char* details, const char* location,
+ const char* callStack, HAL_Bool printMsg);
+
+/**
+ * Gets the current control word of the driver station.
+ *
+ * The control work contains the robot state.
+ *
+ * @param controlWord the control word (out)
+ * @return the error code, or 0 for success
+ */
+int32_t HAL_GetControlWord(HAL_ControlWord* controlWord);
+
+/**
+ * Gets the current alliance station ID.
+ *
+ * @param status the error code, or 0 for success
+ * @return the alliance station ID
+ */
+HAL_AllianceStationID HAL_GetAllianceStation(int32_t* status);
+
+/**
+ * Gets the axes of a specific joystick.
+ *
+ * @param joystickNum the joystick number
+ * @param axes the axes values (output)
+ * @return the error code, or 0 for success
+ */
+int32_t HAL_GetJoystickAxes(int32_t joystickNum, HAL_JoystickAxes* axes);
+
+/**
+ * Gets the POVs of a specific joystick.
+ *
+ * @param joystickNum the joystick number
+ * @param povs the POV values (output)
+ * @return the error code, or 0 for success
+ */
+int32_t HAL_GetJoystickPOVs(int32_t joystickNum, HAL_JoystickPOVs* povs);
+
+/**
+ * Gets the buttons of a specific joystick.
+ *
+ * @param joystickNum the joystick number
+ * @param buttons the button values (output)
+ * @return the error code, or 0 for success
+ */
+int32_t HAL_GetJoystickButtons(int32_t joystickNum,
+ HAL_JoystickButtons* buttons);
+
+/**
+ * Retrieves the Joystick Descriptor for particular slot.
+ *
+ * @param desc [out] descriptor (data transfer object) to fill in. desc is
+ * filled in regardless of success. In other words, if descriptor is not
+ * available, desc is filled in with default values matching the init-values in
+ * Java and C++ Driverstation for when caller requests a too-large joystick
+ * index.
+ *
+ * @return error code reported from Network Comm back-end. Zero is good,
+ * nonzero is bad.
+ */
+int32_t HAL_GetJoystickDescriptor(int32_t joystickNum,
+ HAL_JoystickDescriptor* desc);
+
+/**
+ * Gets is a specific joystick is considered to be an XBox controller.
+ *
+ * @param joystickNum the joystick number
+ * @return true if xbox, false otherwise
+ */
+HAL_Bool HAL_GetJoystickIsXbox(int32_t joystickNum);
+
+/**
+ * Gets the type of joystick connected.
+ *
+ * This is device specific, and different depending on what system input type
+ * the joystick uses.
+ *
+ * @param joystickNum the joystick number
+ * @return the enumerated joystick type
+ */
+int32_t HAL_GetJoystickType(int32_t joystickNum);
+
+/**
+ * Gets the name of a joystick.
+ *
+ * The returned array must be freed with HAL_FreeJoystickName.
+ *
+ * Will be null terminated.
+ *
+ * @param joystickNum the joystick number
+ * @return the joystick name
+ */
+char* HAL_GetJoystickName(int32_t joystickNum);
+
+/**
+ * Frees a joystick name received with HAL_GetJoystickName
+ *
+ * @param name the name storage
+ */
+void HAL_FreeJoystickName(char* name);
+
+/**
+ * Gets the type of a specific joystick axis.
+ *
+ * This is device specific, and different depending on what system input type
+ * the joystick uses.
+ *
+ * @param joystickNum the joystick number
+ * @param axis the axis number
+ * @return the enumerated axis type
+ */
+int32_t HAL_GetJoystickAxisType(int32_t joystickNum, int32_t axis);
+
+/**
+ * Set joystick outputs.
+ *
+ * @param joystickNum the joystick numer
+ * @param outputs bitmask of outputs, 1 for on 0 for off
+ * @param leftRumble the left rumble value (0-FFFF)
+ * @param rightRumble the right rumble value (0-FFFF)
+ * @return the error code, or 0 for success
+ */
+int32_t HAL_SetJoystickOutputs(int32_t joystickNum, int64_t outputs,
+ int32_t leftRumble, int32_t rightRumble);
+
+/**
+ * Returns the approximate match time.
+ *
+ * The FMS does not send an official match time to the robots, but does send
+ * an approximate match time. The value will count down the time remaining in
+ * the current period (auto or teleop).
+ *
+ * Warning: This is not an official time (so it cannot be used to dispute ref
+ * calls or guarantee that a function will trigger before the match ends).
+ *
+ * The Practice Match function of the DS approximates the behaviour seen on
+ * the field.
+ *
+ * @param status the error code, or 0 for success
+ * @return time remaining in current match period (auto or teleop)
+ */
+double HAL_GetMatchTime(int32_t* status);
+
+/**
+ * Gets info about a specific match.
+ *
+ * @param info the match info (output)
+ * @return the error code, or 0 for success
+ */
+int32_t HAL_GetMatchInfo(HAL_MatchInfo* info);
+
+/**
+ * Releases the DS Mutex to allow proper shutdown of any threads that are
+ * waiting on it.
+ */
+void HAL_ReleaseDSMutex(void);
+
+/**
+ * Checks if new control data has arrived since the last
+ * HAL_WaitForCachedControlData or HAL_IsNewControlData call. If new data has
+ * not arrived, waits for new data to arrive. Otherwise, returns immediately.
+ */
+void HAL_WaitForCachedControlData(void);
+
+/**
+ * Checks if new control data has arrived since the last
+ * HAL_WaitForCachedControlData or HAL_IsNewControlData call. If new data has
+ * not arrived, waits for new data to arrive, or a timeout. Otherwise, returns
+ * immediately.
+ *
+ * @param timeout timeout in seconds
+ * @return true for new data, false for timeout
+ */
+HAL_Bool HAL_WaitForCachedControlDataTimeout(double timeout);
+
+/**
+ * Has a new control packet from the driver station arrived since the last
+ * time this function was called?
+ *
+ * @return true if the control data has been updated since the last call
+ */
+HAL_Bool HAL_IsNewControlData(void);
+
+/**
+ * Waits for the newest DS packet to arrive. Note that this is a blocking call.
+ */
+void HAL_WaitForDSData(void);
+
+/**
+ * Waits for the newest DS packet to arrive. If timeout is <= 0, this will wait
+ * forever. Otherwise, it will wait until either a new packet, or the timeout
+ * time has passed.
+ *
+ * @param timeout timeout in seconds
+ * @return true for new data, false for timeout
+ */
+HAL_Bool HAL_WaitForDSDataTimeout(double timeout);
+
+/**
+ * Initializes the driver station communication. This will properly
+ * handle multiple calls. However note that this CANNOT be called from a library
+ * that interfaces with LabVIEW.
+ */
+void HAL_InitializeDriverStation(void);
+
+/**
+ * Sets the program starting flag in the DS.
+ *
+ * This is what changes the DS to showing robot code ready.
+ */
+void HAL_ObserveUserProgramStarting(void);
+
+/**
+ * Sets the disabled flag in the DS.
+ *
+ * This is used for the DS to ensure the robot is properly responding to its
+ * state request. Ensure this get called about every 50ms, or the robot will be
+ * disabled by the DS.
+ */
+void HAL_ObserveUserProgramDisabled(void);
+
+/**
+ * Sets the autonomous enabled flag in the DS.
+ *
+ * This is used for the DS to ensure the robot is properly responding to its
+ * state request. Ensure this get called about every 50ms, or the robot will be
+ * disabled by the DS.
+ */
+void HAL_ObserveUserProgramAutonomous(void);
+
+/**
+ * Sets the teleoperated enabled flag in the DS.
+ *
+ * This is used for the DS to ensure the robot is properly responding to its
+ * state request. Ensure this get called about every 50ms, or the robot will be
+ * disabled by the DS.
+ */
+void HAL_ObserveUserProgramTeleop(void);
+
+/**
+ * Sets the test mode flag in the DS.
+ *
+ * This is used for the DS to ensure the robot is properly responding to its
+ * state request. Ensure this get called about every 50ms, or the robot will be
+ * disabled by the DS.
+ */
+void HAL_ObserveUserProgramTest(void);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
+/** @} */
diff --git a/hal/src/main/native/include/hal/DriverStationTypes.h b/hal/src/main/native/include/hal/DriverStationTypes.h
new file mode 100644
index 0000000..7c5d6f6
--- /dev/null
+++ b/hal/src/main/native/include/hal/DriverStationTypes.h
@@ -0,0 +1,110 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 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 <stdint.h>
+
+#include "hal/Types.h"
+
+/**
+ * @defgroup hal_driverstation Driver Station Functions
+ * @ingroup hal_capi
+ * @{
+ */
+
+#define HAL_IO_CONFIG_DATA_SIZE 32
+#define HAL_SYS_STATUS_DATA_SIZE 44
+#define HAL_USER_STATUS_DATA_SIZE \
+ (984 - HAL_IO_CONFIG_DATA_SIZE - HAL_SYS_STATUS_DATA_SIZE)
+
+#define HALFRC_NetworkCommunication_DynamicType_DSEnhancedIO_Input 17
+#define HALFRC_NetworkCommunication_DynamicType_DSEnhancedIO_Output 18
+#define HALFRC_NetworkCommunication_DynamicType_Kinect_Header 19
+#define HALFRC_NetworkCommunication_DynamicType_Kinect_Extra1 20
+#define HALFRC_NetworkCommunication_DynamicType_Kinect_Vertices1 21
+#define HALFRC_NetworkCommunication_DynamicType_Kinect_Extra2 22
+#define HALFRC_NetworkCommunication_DynamicType_Kinect_Vertices2 23
+#define HALFRC_NetworkCommunication_DynamicType_Kinect_Joystick 24
+#define HALFRC_NetworkCommunication_DynamicType_Kinect_Custom 25
+
+struct HAL_ControlWord {
+ uint32_t enabled : 1;
+ uint32_t autonomous : 1;
+ uint32_t test : 1;
+ uint32_t eStop : 1;
+ uint32_t fmsAttached : 1;
+ uint32_t dsAttached : 1;
+ uint32_t control_reserved : 26;
+};
+typedef struct HAL_ControlWord HAL_ControlWord;
+
+// clang-format off
+HAL_ENUM(HAL_AllianceStationID) {
+ HAL_AllianceStationID_kRed1,
+ HAL_AllianceStationID_kRed2,
+ HAL_AllianceStationID_kRed3,
+ HAL_AllianceStationID_kBlue1,
+ HAL_AllianceStationID_kBlue2,
+ HAL_AllianceStationID_kBlue3,
+};
+
+HAL_ENUM(HAL_MatchType) {
+ HAL_kMatchType_none,
+ HAL_kMatchType_practice,
+ HAL_kMatchType_qualification,
+ HAL_kMatchType_elimination,
+};
+// clang-format on
+
+/* The maximum number of axes that will be stored in a single HALJoystickAxes
+ * struct. This is used for allocating buffers, not bounds checking, since
+ * there are usually less axes in practice.
+ */
+#define HAL_kMaxJoystickAxes 12
+#define HAL_kMaxJoystickPOVs 12
+#define HAL_kMaxJoysticks 6
+
+struct HAL_JoystickAxes {
+ int16_t count;
+ float axes[HAL_kMaxJoystickAxes];
+};
+typedef struct HAL_JoystickAxes HAL_JoystickAxes;
+
+struct HAL_JoystickPOVs {
+ int16_t count;
+ int16_t povs[HAL_kMaxJoystickPOVs];
+};
+typedef struct HAL_JoystickPOVs HAL_JoystickPOVs;
+
+struct HAL_JoystickButtons {
+ uint32_t buttons;
+ uint8_t count;
+};
+typedef struct HAL_JoystickButtons HAL_JoystickButtons;
+
+struct HAL_JoystickDescriptor {
+ uint8_t isXbox;
+ uint8_t type;
+ char name[256];
+ uint8_t axisCount;
+ uint8_t axisTypes[HAL_kMaxJoystickAxes];
+ uint8_t buttonCount;
+ uint8_t povCount;
+};
+typedef struct HAL_JoystickDescriptor HAL_JoystickDescriptor;
+
+struct HAL_MatchInfo {
+ char eventName[64];
+ HAL_MatchType matchType;
+ uint16_t matchNumber;
+ uint8_t replayNumber;
+ uint8_t gameSpecificMessage[64];
+ uint16_t gameSpecificMessageSize;
+};
+typedef struct HAL_MatchInfo HAL_MatchInfo;
+/** @} */
diff --git a/hal/src/main/native/include/hal/DutyCycle.h b/hal/src/main/native/include/hal/DutyCycle.h
new file mode 100644
index 0000000..357d8f3
--- /dev/null
+++ b/hal/src/main/native/include/hal/DutyCycle.h
@@ -0,0 +1,109 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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 "hal/AnalogTrigger.h"
+#include "hal/Types.h"
+
+/**
+ * @defgroup hal_dutycycle DutyCycle Functions
+ * @ingroup hal_capi
+ * @{
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * Initialize a DutyCycle input.
+ *
+ * @param digitalSourceHandle the digital source to use (either a
+ * HAL_DigitalHandle or a HAL_AnalogTriggerHandle)
+ * @param triggerType the analog trigger type of the source if it is
+ * an analog trigger
+ * @return thre created duty cycle handle
+ */
+HAL_DutyCycleHandle HAL_InitializeDutyCycle(HAL_Handle digitalSourceHandle,
+ HAL_AnalogTriggerType triggerType,
+ int32_t* status);
+
+/**
+ * Free a DutyCycle.
+ *
+ * @param dutyCycleHandle the duty cycle handle
+ */
+void HAL_FreeDutyCycle(HAL_DutyCycleHandle dutyCycleHandle);
+
+/**
+ * Indicates the duty cycle is used by a simulated device.
+ *
+ * @param handle the duty cycle handle
+ * @param device simulated device handle
+ */
+void HAL_SetDutyCycleSimDevice(HAL_DutyCycleHandle handle,
+ HAL_SimDeviceHandle device);
+
+/**
+ * Get the frequency of the duty cycle signal.
+ *
+ * @param dutyCycleHandle the duty cycle handle
+ * @return frequency in Hertz
+ */
+int32_t HAL_GetDutyCycleFrequency(HAL_DutyCycleHandle dutyCycleHandle,
+ int32_t* status);
+
+/**
+ * Get the output ratio of the duty cycle signal.
+ *
+ * <p> 0 means always low, 1 means always high.
+ *
+ * @param dutyCycleHandle the duty cycle handle
+ * @return output ratio between 0 and 1
+ */
+double HAL_GetDutyCycleOutput(HAL_DutyCycleHandle dutyCycleHandle,
+ int32_t* status);
+
+/**
+ * Get the raw output ratio of the duty cycle signal.
+ *
+ * <p> 0 means always low, an output equal to
+ * GetOutputScaleFactor() means always high.
+ *
+ * @param dutyCycleHandle the duty cycle handle
+ * @return output ratio in raw units
+ */
+int32_t HAL_GetDutyCycleOutputRaw(HAL_DutyCycleHandle dutyCycleHandle,
+ int32_t* status);
+
+/**
+ * Get the scale factor of the output.
+ *
+ * <p> An output equal to this value is always high, and then linearly scales
+ * down to 0. Divide the result of getOutputRaw by this in order to get the
+ * percentage between 0 and 1.
+ *
+ * @param dutyCycleHandle the duty cycle handle
+ * @return the output scale factor
+ */
+int32_t HAL_GetDutyCycleOutputScaleFactor(HAL_DutyCycleHandle dutyCycleHandle,
+ int32_t* status);
+
+/**
+ * Get the FPGA index for the DutyCycle.
+ *
+ * @param dutyCycleHandle the duty cycle handle
+ * @return the FPGA index
+ */
+int32_t HAL_GetDutyCycleFPGAIndex(HAL_DutyCycleHandle dutyCycleHandle,
+ int32_t* status);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
+/** @} */
diff --git a/hal/src/main/native/include/hal/Encoder.h b/hal/src/main/native/include/hal/Encoder.h
new file mode 100644
index 0000000..9d2b5d0
--- /dev/null
+++ b/hal/src/main/native/include/hal/Encoder.h
@@ -0,0 +1,310 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 <stdint.h>
+
+#include "hal/AnalogTrigger.h"
+#include "hal/Types.h"
+
+/**
+ * @defgroup hal_encoder Encoder Functions
+ * @ingroup hal_capi
+ * @{
+ */
+
+// clang-format off
+/**
+ * The type of index pulse for the encoder.
+ */
+HAL_ENUM(HAL_EncoderIndexingType) {
+ HAL_kResetWhileHigh,
+ HAL_kResetWhileLow,
+ HAL_kResetOnFallingEdge,
+ HAL_kResetOnRisingEdge
+};
+
+/**
+ * The encoding scaling of the encoder.
+ */
+HAL_ENUM(HAL_EncoderEncodingType) {
+ HAL_Encoder_k1X,
+ HAL_Encoder_k2X,
+ HAL_Encoder_k4X
+};
+// clang-format on
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * Initializes an encoder.
+ *
+ * @param digitalSourceHandleA the A source (either a HAL_DigitalHandle or a
+ * HAL_AnalogTriggerHandle)
+ * @param analogTriggerTypeA the analog trigger type of the A source if it is
+ * an analog trigger
+ * @param digitalSourceHandleB the B source (either a HAL_DigitalHandle or a
+ * HAL_AnalogTriggerHandle)
+ * @param analogTriggerTypeB the analog trigger type of the B source if it is
+ * an analog trigger
+ * @param reverseDirection true to reverse the counting direction from
+ * standard, otherwise false
+ * @param encodingType the encoding type
+ @return the created encoder handle
+ */
+HAL_EncoderHandle HAL_InitializeEncoder(
+ HAL_Handle digitalSourceHandleA, HAL_AnalogTriggerType analogTriggerTypeA,
+ HAL_Handle digitalSourceHandleB, HAL_AnalogTriggerType analogTriggerTypeB,
+ HAL_Bool reverseDirection, HAL_EncoderEncodingType encodingType,
+ int32_t* status);
+
+/**
+ * Frees an encoder.
+ *
+ * @param encoderHandle the encoder handle
+ */
+void HAL_FreeEncoder(HAL_EncoderHandle encoderHandle, int32_t* status);
+
+/**
+ * Indicates the encoder is used by a simulated device.
+ *
+ * @param handle the encoder handle
+ * @param device simulated device handle
+ */
+void HAL_SetEncoderSimDevice(HAL_EncoderHandle handle,
+ HAL_SimDeviceHandle device);
+
+/**
+ * Gets the current counts of the encoder after encoding type scaling.
+ *
+ * This is scaled by the value passed duing initialization to encodingType.
+ *
+ * @param encoderHandle the encoder handle
+ * @return the current scaled count
+ */
+int32_t HAL_GetEncoder(HAL_EncoderHandle encoderHandle, int32_t* status);
+
+/**
+ * Gets the raw counts of the encoder.
+ *
+ * This is not scaled by any values.
+ *
+ * @param encoderHandle the encoder handle
+ * @return the raw encoder count
+ */
+int32_t HAL_GetEncoderRaw(HAL_EncoderHandle encoderHandle, int32_t* status);
+
+/**
+ * Gets the encoder scale value.
+ *
+ * This is set by the value passed during initialization to encodingType.
+ *
+ * @param encoderHandle the encoder handle
+ * @return the encoder scale value
+ */
+int32_t HAL_GetEncoderEncodingScale(HAL_EncoderHandle encoderHandle,
+ int32_t* status);
+
+/**
+ * Reads the current encoder value.
+ *
+ * Read the value at this instant. It may still be running, so it reflects the
+ * current value. Next time it is read, it might have a different value.
+ *
+ * @param encoderHandle the encoder handle
+ * @return the current encoder value
+ */
+void HAL_ResetEncoder(HAL_EncoderHandle encoderHandle, int32_t* status);
+
+/*
+ * Gets the Period of the most recent count.
+ *
+ * Returns the time interval of the most recent count. This can be used for
+ * velocity calculations to determine shaft speed.
+ *
+ * @param encoderHandle the encoder handle
+ * @returns the period of the last two pulses in units of seconds
+ */
+double HAL_GetEncoderPeriod(HAL_EncoderHandle encoderHandle, int32_t* status);
+
+/**
+ * Sets the maximum period where the device is still considered "moving".
+ *
+ * Sets the maximum period where the device is considered moving. This value is
+ * used to determine the "stopped" state of the encoder using the
+ * HAL_GetEncoderStopped method.
+ *
+ * @param encoderHandle the encoder handle
+ * @param maxPeriod the maximum period where the counted device is
+ * considered moving in seconds
+ */
+void HAL_SetEncoderMaxPeriod(HAL_EncoderHandle encoderHandle, double maxPeriod,
+ int32_t* status);
+
+/**
+ * Determines if the clock is stopped.
+ *
+ * Determines if the clocked input is stopped based on the MaxPeriod value set
+ * using the SetMaxPeriod method. If the clock exceeds the MaxPeriod, then the
+ * device (and encoder) are assumed to be stopped and it returns true.
+ *
+ * @param encoderHandle the encoder handle
+ * @return true if the most recent encoder period exceeds the
+ * MaxPeriod value set by SetMaxPeriod
+ */
+HAL_Bool HAL_GetEncoderStopped(HAL_EncoderHandle encoderHandle,
+ int32_t* status);
+
+/**
+ * Gets the last direction the encoder value changed.
+ *
+ * @param encoderHandle the encoder handle
+ * @return the last direction the encoder value changed
+ */
+HAL_Bool HAL_GetEncoderDirection(HAL_EncoderHandle encoderHandle,
+ int32_t* status);
+
+/**
+ * Gets the current distance traveled by the encoder.
+ *
+ * This is the encoder count scaled by the distance per pulse set for the
+ * encoder.
+ *
+ * @param encoderHandle the encoder handle
+ * @return the encoder distance (units are determined by the units
+ * passed to HAL_SetEncoderDistancePerPulse)
+ */
+double HAL_GetEncoderDistance(HAL_EncoderHandle encoderHandle, int32_t* status);
+
+/**
+ * Gets the current rate of the encoder.
+ *
+ * This is the encoder period scaled by the distance per pulse set for the
+ * encoder.
+ *
+ * @param encoderHandle the encoder handle
+ * @return the encoder rate (units are determined by the units
+ * passed to HAL_SetEncoderDistancePerPulse, time value is seconds)
+ */
+double HAL_GetEncoderRate(HAL_EncoderHandle encoderHandle, int32_t* status);
+
+/**
+ * Sets the minimum rate to be considered moving by the encoder.
+ *
+ * Units need to match what is set by HAL_SetEncoderDistancePerPulse, with time
+ * as seconds.
+ *
+ * @param encoderHandle the encoder handle
+ * @param minRate the minimum rate to be considered moving (units are
+ * determined by the units passed to HAL_SetEncoderDistancePerPulse, time value
+ * is seconds)
+ */
+void HAL_SetEncoderMinRate(HAL_EncoderHandle encoderHandle, double minRate,
+ int32_t* status);
+
+/**
+ * Sets the distance traveled per encoder pulse. This is used as a scaling
+ * factor for the rate and distance calls.
+ *
+ * @param encoderHandle the encoder handle
+ * @param distancePerPulse the distance traveled per encoder pulse (units user
+ * defined)
+ */
+void HAL_SetEncoderDistancePerPulse(HAL_EncoderHandle encoderHandle,
+ double distancePerPulse, int32_t* status);
+
+/**
+ * Sets if to reverse the direction of the encoder.
+ *
+ * Note that this is not a toggle. It is an absolute set.
+ *
+ * @param encoderHandle the encoder handle
+ * @param reverseDirection true to reverse the direction, false to not.
+ */
+void HAL_SetEncoderReverseDirection(HAL_EncoderHandle encoderHandle,
+ HAL_Bool reverseDirection, int32_t* status);
+
+/**
+ * Sets the number of encoder samples to average when calculating encoder rate.
+ *
+ * @param encoderHandle the encoder handle
+ * @param samplesToAverage the number of samples to average
+ */
+void HAL_SetEncoderSamplesToAverage(HAL_EncoderHandle encoderHandle,
+ int32_t samplesToAverage, int32_t* status);
+
+/**
+ * Gets the current samples to average value.
+ *
+ * @param encoderHandle the encoder handle
+ * @return the current samples to average value
+ */
+int32_t HAL_GetEncoderSamplesToAverage(HAL_EncoderHandle encoderHandle,
+ int32_t* status);
+
+/**
+ * Sets the source for an index pulse on the encoder.
+ *
+ * The index pulse can be used to cause an encoder to reset based on an external
+ * input.
+ *
+ * @param encoderHandle the encoder handle
+ * @param digitalSourceHandle the index source handle (either a
+ * HAL_AnalogTriggerHandle of a HAL_DigitalHandle)
+ * @param analogTriggerType the analog trigger type if the source is an analog
+ * trigger
+ * @param type the index triggering type
+ */
+void HAL_SetEncoderIndexSource(HAL_EncoderHandle encoderHandle,
+ HAL_Handle digitalSourceHandle,
+ HAL_AnalogTriggerType analogTriggerType,
+ HAL_EncoderIndexingType type, int32_t* status);
+
+/**
+ * Gets the FPGA index of the encoder.
+ *
+ * @param encoderHandle the encoder handle
+ * @return the FPGA index of the encoder
+ */
+int32_t HAL_GetEncoderFPGAIndex(HAL_EncoderHandle encoderHandle,
+ int32_t* status);
+
+/**
+ * Gets the decoding scale factor of the encoder.
+ *
+ * This is used to perform the scaling from raw to type scaled values.
+ *
+ * @param encoderHandle the encoder handle
+ * @return the scale value for the encoder
+ */
+double HAL_GetEncoderDecodingScaleFactor(HAL_EncoderHandle encoderHandle,
+ int32_t* status);
+
+/**
+ * Gets the user set distance per pulse of the encoder.
+ *
+ * @param encoderHandle the encoder handle
+ * @return the set distance per pulse
+ */
+double HAL_GetEncoderDistancePerPulse(HAL_EncoderHandle encoderHandle,
+ int32_t* status);
+
+/**
+ * Gets the encoding type of the encoder.
+ *
+ * @param encoderHandle the encoder handle
+ * @return the encoding type
+ */
+HAL_EncoderEncodingType HAL_GetEncoderEncodingType(
+ HAL_EncoderHandle encoderHandle, int32_t* status);
+#ifdef __cplusplus
+} // extern "C"
+#endif
+/** @} */
diff --git a/hal/src/main/native/include/hal/Errors.h b/hal/src/main/native/include/hal/Errors.h
new file mode 100644
index 0000000..9f74f8c
--- /dev/null
+++ b/hal/src/main/native/include/hal/Errors.h
@@ -0,0 +1,147 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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
+
+/**
+ * @defgroup hal_errors Error Defines
+ * @ingroup hal_capi
+ * @{
+ */
+
+#define CTR_RxTimeout_MESSAGE "CTRE CAN Receive Timeout"
+#define CTR_TxTimeout_MESSAGE "CTRE CAN Transmit Timeout"
+#define CTR_InvalidParamValue_MESSAGE "CTRE CAN Invalid Parameter"
+#define CTR_UnexpectedArbId_MESSAGE \
+ "CTRE Unexpected Arbitration ID (CAN Node ID)"
+#define CTR_TxFailed_MESSAGE "CTRE CAN Transmit Error"
+#define CTR_SigNotUpdated_MESSAGE "CTRE CAN Signal Not Updated"
+
+#define NiFpga_Status_FifoTimeout_MESSAGE "NIFPGA: FIFO timeout error"
+#define NiFpga_Status_TransferAborted_MESSAGE "NIFPGA: Transfer aborted error"
+#define NiFpga_Status_MemoryFull_MESSAGE \
+ "NIFPGA: Memory Allocation failed, memory full"
+#define NiFpga_Status_SoftwareFault_MESSAGE "NIFPGA: Unexpected software error"
+#define NiFpga_Status_InvalidParameter_MESSAGE "NIFPGA: Invalid Parameter"
+#define NiFpga_Status_ResourceNotFound_MESSAGE "NIFPGA: Resource not found"
+#define NiFpga_Status_ResourceNotInitialized_MESSAGE \
+ "NIFPGA: Resource not initialized"
+#define NiFpga_Status_HardwareFault_MESSAGE "NIFPGA: Hardware Fault"
+#define NiFpga_Status_IrqTimeout_MESSAGE "NIFPGA: Interrupt timeout"
+
+#define ERR_CANSessionMux_InvalidBuffer_MESSAGE "CAN: Invalid Buffer"
+#define ERR_CANSessionMux_MessageNotFound_MESSAGE "CAN: Message not found"
+#define WARN_CANSessionMux_NoToken_MESSAGE "CAN: No token"
+#define ERR_CANSessionMux_NotAllowed_MESSAGE "CAN: Not allowed"
+#define ERR_CANSessionMux_NotInitialized_MESSAGE "CAN: Not initialized"
+
+#define ERR_FRCSystem_NetCommNotResponding_MESSAGE \
+ "FRCSystem: NetComm not responding"
+#define ERR_FRCSystem_NoDSConnection_MESSAGE \
+ "FRCSystem: No driver station connected"
+
+#define SAMPLE_RATE_TOO_HIGH 1001
+#define SAMPLE_RATE_TOO_HIGH_MESSAGE \
+ "HAL: Analog module sample rate is too high"
+#define VOLTAGE_OUT_OF_RANGE 1002
+#define VOLTAGE_OUT_OF_RANGE_MESSAGE \
+ "HAL: Voltage to convert to raw value is out of range [0; 5]"
+#define LOOP_TIMING_ERROR 1004
+#define LOOP_TIMING_ERROR_MESSAGE \
+ "HAL: Digital module loop timing is not the expected value"
+#define SPI_WRITE_NO_MOSI 1012
+#define SPI_WRITE_NO_MOSI_MESSAGE \
+ "HAL: Cannot write to SPI port with no MOSI output"
+#define SPI_READ_NO_MISO 1013
+#define SPI_READ_NO_MISO_MESSAGE \
+ "HAL: Cannot read from SPI port with no MISO input"
+#define SPI_READ_NO_DATA 1014
+#define SPI_READ_NO_DATA_MESSAGE "HAL: No data available to read from SPI"
+#define INCOMPATIBLE_STATE 1015
+#define INCOMPATIBLE_STATE_MESSAGE \
+ "HAL: Incompatible State: The operation cannot be completed"
+#define NO_AVAILABLE_RESOURCES -1004
+#define NO_AVAILABLE_RESOURCES_MESSAGE "HAL: No available resources to allocate"
+#define NULL_PARAMETER -1005
+#define NULL_PARAMETER_MESSAGE "HAL: A pointer parameter to a method is NULL"
+#define ANALOG_TRIGGER_LIMIT_ORDER_ERROR -1010
+#define ANALOG_TRIGGER_LIMIT_ORDER_ERROR_MESSAGE \
+ "HAL: AnalogTrigger limits error. Lower limit > Upper Limit"
+#define ANALOG_TRIGGER_PULSE_OUTPUT_ERROR -1011
+#define ANALOG_TRIGGER_PULSE_OUTPUT_ERROR_MESSAGE \
+ "HAL: Attempted to read AnalogTrigger pulse output."
+#define PARAMETER_OUT_OF_RANGE -1028
+#define PARAMETER_OUT_OF_RANGE_MESSAGE "HAL: A parameter is out of range."
+#define RESOURCE_IS_ALLOCATED -1029
+#define RESOURCE_IS_ALLOCATED_MESSAGE "HAL: Resource already allocated"
+#define RESOURCE_OUT_OF_RANGE -1030
+#define RESOURCE_OUT_OF_RANGE_MESSAGE \
+ "HAL: The requested resource is out of range."
+#define HAL_INVALID_ACCUMULATOR_CHANNEL -1035
+#define HAL_INVALID_ACCUMULATOR_CHANNEL_MESSAGE \
+ "HAL: The requested input is not an accumulator channel"
+#define HAL_COUNTER_NOT_SUPPORTED -1058
+#define HAL_COUNTER_NOT_SUPPORTED_MESSAGE \
+ "HAL: Counter mode not supported for encoder method"
+#define HAL_PWM_SCALE_ERROR -1072
+#define HAL_PWM_SCALE_ERROR_MESSAGE \
+ "HAL: The PWM Scale Factors are out of range"
+#define HAL_HANDLE_ERROR -1098
+#define HAL_HANDLE_ERROR_MESSAGE \
+ "HAL: A handle parameter was passed incorrectly"
+
+#define HAL_LED_CHANNEL_ERROR -1099
+#define HAL_LED_CHANNEL_ERROR_MESSAGE \
+ "HAL: Addressable LEDs only supported on PWM Headers, not MXP or DIO"
+
+#define HAL_INVALID_DMA_ADDITION -1102
+#define HAL_INVALID_DMA_ADDITION_MESSAGE \
+ "HAL_AddDMA() only works before HAL_StartDMA()"
+
+#define HAL_SERIAL_PORT_NOT_FOUND -1123
+#define HAL_SERIAL_PORT_NOT_FOUND_MESSAGE \
+ "HAL: The specified serial port device was not found"
+
+#define HAL_SERIAL_PORT_OPEN_ERROR -1124
+#define HAL_SERIAL_PORT_OPEN_ERROR_MESSAGE \
+ "HAL: The serial port could not be opened"
+
+#define HAL_SERIAL_PORT_ERROR -1125
+#define HAL_SERIAL_PORT_ERROR_MESSAGE \
+ "HAL: There was an error on the serial port"
+
+#define HAL_THREAD_PRIORITY_ERROR -1152
+#define HAL_THREAD_PRIORITY_ERROR_MESSAGE \
+ "HAL: Getting or setting the priority of a thread has failed";
+
+#define HAL_THREAD_PRIORITY_RANGE_ERROR -1153
+#define HAL_THREAD_PRIORITY_RANGE_ERROR_MESSAGE \
+ "HAL: The priority requested to be set is invalid"
+
+#define HAL_CAN_TIMEOUT -1154
+#define HAL_CAN_TIMEOUT_MESSAGE "HAL: CAN Receive has Timed Out"
+
+#define HAL_SIM_NOT_SUPPORTED -1155
+#define HAL_SIM_NOT_SUPPORTED_MESSAGE "HAL: Method not supported in sim"
+
+#define HAL_CAN_BUFFER_OVERRUN -35007
+#define HAL_CAN_BUFFER_OVERRUN_MESSAGE \
+ "HAL: CAN Output Buffer Full. Ensure a device is attached"
+
+#define VI_ERROR_SYSTEM_ERROR_MESSAGE "HAL - VISA: System Error";
+#define VI_ERROR_INV_OBJECT_MESSAGE "HAL - VISA: Invalid Object"
+#define VI_ERROR_RSRC_LOCKED_MESSAGE "HAL - VISA: Resource Locked"
+#define VI_ERROR_RSRC_NFOUND_MESSAGE "HAL - VISA: Resource Not Found"
+#define VI_ERROR_INV_RSRC_NAME_MESSAGE "HAL - VISA: Invalid Resource Name"
+#define VI_ERROR_QUEUE_OVERFLOW_MESSAGE "HAL - VISA: Queue Overflow"
+#define VI_ERROR_IO_MESSAGE "HAL - VISA: General IO Error"
+#define VI_ERROR_ASRL_PARITY_MESSAGE "HAL - VISA: Parity Error"
+#define VI_ERROR_ASRL_FRAMING_MESSAGE "HAL - VISA: Framing Error"
+#define VI_ERROR_ASRL_OVERRUN_MESSAGE "HAL - VISA: Buffer Overrun Error"
+#define VI_ERROR_RSRC_BUSY_MESSAGE "HAL - VISA: Resource Busy"
+#define VI_ERROR_INV_PARAMETER_MESSAGE "HAL - VISA: Invalid Parameter"
+/** @} */
diff --git a/hal/src/main/native/include/hal/Extensions.h b/hal/src/main/native/include/hal/Extensions.h
new file mode 100644
index 0000000..3a435c0
--- /dev/null
+++ b/hal/src/main/native/include/hal/Extensions.h
@@ -0,0 +1,61 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "hal/Types.h"
+
+/**
+ * @defgroup hal_extensions Simulator Extensions
+ * @ingroup hal_capi
+ * HAL Simulator Extensions. These are libraries that provide additional
+ * simulator functionality, such as a Gazebo interface, or a more light weight
+ * simulation.
+ *
+ * An extension must expose the HALSIM_InitExtension entry point which is
+ * invoked after the library is loaded.
+ *
+ * The entry point is expected to return < 0 for errors that should stop
+ * the HAL completely, 0 for success, and > 0 for a non fatal error.
+ * @{
+ */
+typedef int halsim_extension_init_func_t(void);
+
+extern "C" {
+/**
+ * Loads a single extension from a direct path.
+ *
+ * Expected to be called internally, not by users.
+ *
+ * @param library the library path
+ * @return the succes state of the initialization
+ */
+int HAL_LoadOneExtension(const char* library);
+
+/**
+ * Loads any extra halsim libraries provided in the HALSIM_EXTENSIONS
+ * environment variable.
+ *
+ * @return the succes state of the initialization
+ */
+int HAL_LoadExtensions(void);
+
+/**
+ * Enables or disables the message saying no HAL extensions were found.
+ *
+ * Some apps don't care, and the message create clutter. For general team code,
+ * we want it.
+ *
+ * This must be called before HAL_Initialize is called.
+ *
+ * This defaults to true.
+ *
+ * @param showMessage true to show message, false to not.
+ */
+void HAL_SetShowExtensionsNotFoundMessages(HAL_Bool showMessage);
+} // extern "C"
+/** @} */
diff --git a/hal/src/main/native/include/hal/HAL.h b/hal/src/main/native/include/hal/HAL.h
new file mode 100644
index 0000000..cf9b2f8
--- /dev/null
+++ b/hal/src/main/native/include/hal/HAL.h
@@ -0,0 +1,42 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2013-2019 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 <stdint.h>
+
+#include "hal/Accelerometer.h"
+#include "hal/AnalogAccumulator.h"
+#include "hal/AnalogInput.h"
+#include "hal/AnalogOutput.h"
+#include "hal/AnalogTrigger.h"
+#include "hal/CAN.h"
+#include "hal/CANAPI.h"
+#include "hal/Compressor.h"
+#include "hal/Constants.h"
+#include "hal/Counter.h"
+#include "hal/DIO.h"
+#include "hal/DriverStation.h"
+#include "hal/Encoder.h"
+#include "hal/Errors.h"
+#include "hal/FRCUsageReporting.h"
+#include "hal/HALBase.h"
+#include "hal/I2C.h"
+#include "hal/Interrupts.h"
+#include "hal/Main.h"
+#include "hal/Notifier.h"
+#include "hal/PDP.h"
+#include "hal/PWM.h"
+#include "hal/Ports.h"
+#include "hal/Power.h"
+#include "hal/Relay.h"
+#include "hal/SPI.h"
+#include "hal/SimDevice.h"
+#include "hal/Solenoid.h"
+#include "hal/Threads.h"
+#include "hal/Types.h"
+#include "hal/Value.h"
diff --git a/hal/src/main/native/include/hal/HALBase.h b/hal/src/main/native/include/hal/HALBase.h
new file mode 100644
index 0000000..f61d9e4
--- /dev/null
+++ b/hal/src/main/native/include/hal/HALBase.h
@@ -0,0 +1,155 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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 <stdint.h>
+
+#include "hal/Types.h"
+
+/**
+ * @defgroup hal_capi WPILib HAL API
+ * Hardware Abstraction Layer to hardware or simulator
+ * @{
+ */
+
+// clang-format off
+HAL_ENUM(HAL_RuntimeType) { HAL_Athena, HAL_Mock };
+// clang-format on
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * Gets the error message for a specific status code.
+ *
+ * @param code the status code
+ * @return the error message for the code. This does not need to be freed.
+ */
+const char* HAL_GetErrorMessage(int32_t code);
+
+/**
+ * Returns the FPGA Version number.
+ *
+ * For now, expect this to be competition year.
+ *
+ * @return FPGA Version number.
+ */
+int32_t HAL_GetFPGAVersion(int32_t* status);
+
+/**
+ * Returns the FPGA Revision number.
+ *
+ * The format of the revision is 3 numbers.
+ * The 12 most significant bits are the Major Revision.
+ * the next 8 bits are the Minor Revision.
+ * The 12 least significant bits are the Build Number.
+ *
+ * @return FPGA Revision number.
+ */
+int64_t HAL_GetFPGARevision(int32_t* status);
+
+HAL_RuntimeType HAL_GetRuntimeType(void);
+
+/**
+ * Gets the state of the "USER" button on the roboRIO.
+ *
+ * @return true if the button is currently pressed down
+ */
+HAL_Bool HAL_GetFPGAButton(int32_t* status);
+
+/**
+ * Gets if the system outputs are currently active
+ *
+ * @return true if the system outputs are active, false if disabled
+ */
+HAL_Bool HAL_GetSystemActive(int32_t* status);
+
+/**
+ * Gets if the system is in a browned out state.
+ *
+ * @return true if the system is in a low voltage brown out, false otherwise
+ */
+HAL_Bool HAL_GetBrownedOut(int32_t* status);
+
+/**
+ * Gets a port handle for a specific channel.
+ *
+ * The created handle does not need to be freed.
+ *
+ * @param channel the channel number
+ * @return the created port
+ */
+HAL_PortHandle HAL_GetPort(int32_t channel);
+
+/**
+ * Gets a port handle for a specific channel and module.
+ *
+ * This is expected to be used for PCMs, as the roboRIO does not work with
+ * modules anymore.
+ *
+ * The created handle does not need to be freed.
+ *
+ * @param module the module number
+ * @param channel the channel number
+ * @return the created port
+ */
+HAL_PortHandle HAL_GetPortWithModule(int32_t module, int32_t channel);
+
+/**
+ * Reads the microsecond-resolution timer on the FPGA.
+ *
+ * @return The current time in microseconds according to the FPGA (since FPGA
+ * reset).
+ */
+uint64_t HAL_GetFPGATime(int32_t* status);
+
+/**
+ * Given an 32 bit FPGA time, expand it to the nearest likely 64 bit FPGA time.
+ *
+ * Note: This is making the assumption that the timestamp being converted is
+ * always in the past. If you call this with a future timestamp, it probably
+ * will make it in the past. If you wait over 70 minutes between capturing the
+ * bottom 32 bits of the timestamp and expanding it, you will be off by
+ * multiples of 1<<32 microseconds.
+ *
+ * @return The current time in microseconds according to the FPGA (since FPGA
+ * reset) as a 64 bit number.
+ */
+uint64_t HAL_ExpandFPGATime(uint32_t unexpanded_lower, int32_t* status);
+
+/**
+ * Call this to start up HAL. This is required for robot programs.
+ *
+ * This must be called before any other HAL functions. Failure to do so will
+ * result in undefined behavior, and likely segmentation faults. This means that
+ * any statically initialized variables in a program MUST call this function in
+ * their constructors if they want to use other HAL calls.
+ *
+ * The common parameters are 500 for timeout and 0 for mode.
+ *
+ * This function is safe to call from any thread, and as many times as you wish.
+ * It internally guards from any reentrancy.
+ *
+ * The applicable modes are:
+ * 0: Try to kill an existing HAL from another program, if not successful,
+ * error.
+ * 1: Force kill a HAL from another program.
+ * 2: Just warn if another hal exists and cannot be killed. Will likely result
+ * in undefined behavior.
+ *
+ * @param timeout the initialization timeout (ms)
+ * @param mode the initialization mode (see remarks)
+ * @return true if initialization was successful, otherwise false.
+ */
+HAL_Bool HAL_Initialize(int32_t timeout, int32_t mode);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
+/** @} */
diff --git a/hal/src/main/native/include/hal/I2C.h b/hal/src/main/native/include/hal/I2C.h
new file mode 100644
index 0000000..fc8b5c8
--- /dev/null
+++ b/hal/src/main/native/include/hal/I2C.h
@@ -0,0 +1,93 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 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 <stdint.h>
+
+#include "hal/I2CTypes.h"
+#include "hal/Types.h"
+
+/**
+ * @defgroup hal_i2c I2C Functions
+ * @ingroup hal_capi
+ * @{
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * Initializes the I2C port.
+ *
+ * Opens the port if necessary and saves the handle.
+ * If opening the MXP port, also sets up the channel functions appropriately.
+ *
+ * @param port The port to open, 0 for the on-board, 1 for the MXP.
+ */
+void HAL_InitializeI2C(HAL_I2CPort port, int32_t* status);
+
+/**
+ * Generic I2C read/write transaction.
+ *
+ * This is a lower-level interface to the I2C hardware giving you more control
+ * over each transaction.
+ *
+ * @param port The I2C port, 0 for the on-board, 1 for the MXP.
+ * @param dataToSend Buffer of data to send as part of the transaction.
+ * @param sendSize Number of bytes to send as part of the transaction.
+ * @param dataReceived Buffer to read data into.
+ * @param receiveSize Number of bytes to read from the device.
+ * @return >= 0 on success or -1 on transfer abort.
+ */
+int32_t HAL_TransactionI2C(HAL_I2CPort port, int32_t deviceAddress,
+ const uint8_t* dataToSend, int32_t sendSize,
+ uint8_t* dataReceived, int32_t receiveSize);
+
+/**
+ * Executes a write transaction with the device.
+ *
+ * Writes a single byte to a register on a device and wait until the
+ * transaction is complete.
+ *
+ * @param port The I2C port, 0 for the on-board, 1 for the MXP.
+ * @param registerAddress The address of the register on the device to be
+ * written.
+ * @param data The byte to write to the register on the device.
+ * @return >= 0 on success or -1 on transfer abort.
+ */
+int32_t HAL_WriteI2C(HAL_I2CPort port, int32_t deviceAddress,
+ const uint8_t* dataToSend, int32_t sendSize);
+
+/**
+ * Executes a read transaction with the device.
+ *
+ * Reads bytes from a device.
+ * Most I2C devices will auto-increment the register pointer internally allowing
+ * you to read consecutive registers on a device in a single transaction.
+ *
+ * @param port The I2C port, 0 for the on-board, 1 for the MXP.
+ * @param registerAddress The register to read first in the transaction.
+ * @param count The number of bytes to read in the transaction.
+ * @param buffer A pointer to the array of bytes to store the data read from the
+ * device.
+ * @return >= 0 on success or -1 on transfer abort.
+ */
+int32_t HAL_ReadI2C(HAL_I2CPort port, int32_t deviceAddress, uint8_t* buffer,
+ int32_t count);
+
+/**
+ * Closes an I2C port
+ *
+ * @param port The I2C port, 0 for the on-board, 1 for the MXP.
+ */
+void HAL_CloseI2C(HAL_I2CPort port);
+#ifdef __cplusplus
+} // extern "C"
+#endif
+/** @} */
diff --git a/hal/src/main/native/include/hal/I2CTypes.h b/hal/src/main/native/include/hal/I2CTypes.h
new file mode 100644
index 0000000..b5e8235
--- /dev/null
+++ b/hal/src/main/native/include/hal/I2CTypes.h
@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 <stdint.h>
+
+#include "hal/Types.h"
+
+/**
+ * @defgroup hal_i2c I2C Functions
+ * @ingroup hal_capi
+ * @{
+ */
+
+// clang-format off
+HAL_ENUM(HAL_I2CPort) { HAL_I2C_kInvalid = -1, HAL_I2C_kOnboard, HAL_I2C_kMXP };
+// clang-format on
+
+#ifdef __cplusplus
+namespace hal {
+
+/**
+ * A move-only C++ wrapper around HAL_I2CPort.
+ * Does not ensure destruction.
+ */
+using I2CPort = Handle<HAL_I2CPort, HAL_I2C_kInvalid>;
+
+} // namespace hal
+#endif
+/** @} */
diff --git a/hal/src/main/native/include/hal/Interrupts.h b/hal/src/main/native/include/hal/Interrupts.h
new file mode 100644
index 0000000..126b92c
--- /dev/null
+++ b/hal/src/main/native/include/hal/Interrupts.h
@@ -0,0 +1,159 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 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 <stdint.h>
+
+#include "hal/AnalogTrigger.h"
+#include "hal/Types.h"
+
+/**
+ * @defgroup hal_interrupts Interrupts Functions
+ * @ingroup hal_capi
+ * @{
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef void (*HAL_InterruptHandlerFunction)(uint32_t interruptAssertedMask,
+ void* param);
+
+/**
+ * Initializes an interrupt.
+ *
+ * @param watcher true for synchronous interrupts, false for asynchronous
+ * @return the created interrupt handle
+ */
+HAL_InterruptHandle HAL_InitializeInterrupts(HAL_Bool watcher, int32_t* status);
+
+/**
+ * Frees an interrupt.
+ *
+ * @param interruptHandle the interrupt handle
+ * @return the param passed to the interrupt, or nullptr if one
+ * wasn't passed.
+ */
+void* HAL_CleanInterrupts(HAL_InterruptHandle interruptHandle, int32_t* status);
+
+/**
+ * In synchronous mode, waits for the defined interrupt to occur.
+ *
+ * @param interruptHandle the interrupt handle
+ * @param timeout timeout in seconds
+ * @param ignorePrevious if true, ignore interrupts that happened before
+ * waitForInterrupt was called
+ * @return the mask of interrupts that fired
+ */
+int64_t HAL_WaitForInterrupt(HAL_InterruptHandle interruptHandle,
+ double timeout, HAL_Bool ignorePrevious,
+ int32_t* status);
+
+/**
+ * Enables interrupts to occur on this input.
+ *
+ * Interrupts are disabled when the RequestInterrupt call is made. This gives
+ * time to do the setup of the other options before starting to field
+ * interrupts.
+ *
+ * @param interruptHandle the interrupt handle
+ */
+void HAL_EnableInterrupts(HAL_InterruptHandle interruptHandle, int32_t* status);
+
+/**
+ * Disables interrupts without without deallocating structures.
+ *
+ * @param interruptHandle the interrupt handle
+ */
+void HAL_DisableInterrupts(HAL_InterruptHandle interruptHandle,
+ int32_t* status);
+
+/**
+ * Returns the timestamp for the rising interrupt that occurred most recently.
+ *
+ * This is in the same time domain as HAL_GetFPGATime(). It only contains the
+ * bottom 32 bits of the timestamp. If your robot has been running for over 1
+ * hour, you will need to fill in the upper 32 bits yourself.
+ *
+ * @param interruptHandle the interrupt handle
+ * @return timestamp in microseconds since FPGA Initialization
+ */
+int64_t HAL_ReadInterruptRisingTimestamp(HAL_InterruptHandle interruptHandle,
+ int32_t* status);
+
+/**
+ * Returns the timestamp for the falling interrupt that occurred most recently.
+ *
+ * This is in the same time domain as HAL_GetFPGATime(). It only contains the
+ * bottom 32 bits of the timestamp. If your robot has been running for over 1
+ * hour, you will need to fill in the upper 32 bits yourself.
+ *
+ * @param interruptHandle the interrupt handle
+ * @return timestamp in microseconds since FPGA Initialization
+ */
+int64_t HAL_ReadInterruptFallingTimestamp(HAL_InterruptHandle interruptHandle,
+ int32_t* status);
+
+/**
+ * Requests interrupts on a specific digital source.
+ *
+ * @param interruptHandle the interrupt handle
+ * @param digitalSourceHandle the digital source handle (either a
+ * HAL_AnalogTriggerHandle of a HAL_DigitalHandle)
+ * @param analogTriggerType the trigger type if the source is an AnalogTrigger
+ */
+void HAL_RequestInterrupts(HAL_InterruptHandle interruptHandle,
+ HAL_Handle digitalSourceHandle,
+ HAL_AnalogTriggerType analogTriggerType,
+ int32_t* status);
+
+/**
+ * Attaches an asynchronous interrupt handler to the interrupt.
+ *
+ * This interrupt gets called directly on the FPGA interrupt thread, so will
+ * block other interrupts while running.
+ *
+ * @param interruptHandle the interrupt handle
+ * @param handler the handler function for the interrupt to call
+ * @param param a parameter to be passed to the handler
+ */
+void HAL_AttachInterruptHandler(HAL_InterruptHandle interruptHandle,
+ HAL_InterruptHandlerFunction handler,
+ void* param, int32_t* status);
+
+/**
+ * Attaches an asynchronous interrupt handler to the interrupt.
+ *
+ * This interrupt gets called on a thread specific to the interrupt, so will not
+ * block other interrupts.
+ *
+ * @param interruptHandle the interrupt handle
+ * @param handler the handler function for the interrupt to call
+ * @param param a parameter to be passed to the handler
+ */
+void HAL_AttachInterruptHandlerThreaded(HAL_InterruptHandle interruptHandle,
+ HAL_InterruptHandlerFunction handler,
+ void* param, int32_t* status);
+
+/**
+ * Sets the edges to trigger the interrupt on.
+ *
+ * Note that both edges triggered is a valid configuration.
+ *
+ * @param interruptHandle the interrupt handle
+ * @param risingEdge true for triggering on rising edge
+ * @param fallingEdge true for triggering on falling edge
+ */
+void HAL_SetInterruptUpSourceEdge(HAL_InterruptHandle interruptHandle,
+ HAL_Bool risingEdge, HAL_Bool fallingEdge,
+ int32_t* status);
+#ifdef __cplusplus
+} // extern "C"
+#endif
+/** @} */
diff --git a/hal/src/main/native/include/hal/Main.h b/hal/src/main/native/include/hal/Main.h
new file mode 100644
index 0000000..866e274
--- /dev/null
+++ b/hal/src/main/native/include/hal/Main.h
@@ -0,0 +1,67 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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 <stdint.h>
+
+#include "hal/Types.h"
+
+/**
+ * @defgroup hal_relay Main loop functions
+ * @ingroup hal_capi
+ * @{
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * Sets up the system to run the provided main loop in the main thread (e.g.
+ * the thread in which main() starts execution) and run the robot code in a
+ * separate thread.
+ *
+ * Normally the robot code runs in the main thread, but some GUI systems
+ * require the GUI be run in the main thread.
+ *
+ * To be effective, this function must be called before the robot code starts
+ * the main loop (e.g. by frc::StartRobot()).
+ *
+ * @param param parameter data to pass to mainFunc and exitFunc
+ * @param mainFunc the function to be run when HAL_RunMain() is called.
+ * @param exitFunc the function to be run when HAL_ExitMain() is called.
+ */
+void HAL_SetMain(void* param, void (*mainFunc)(void*), void (*exitFunc)(void*));
+
+/**
+ * Returns true if HAL_SetMain() has been called.
+ *
+ * @return True if HAL_SetMain() has been called, false otherwise.
+ */
+HAL_Bool HAL_HasMain(void);
+
+/**
+ * Runs the main function provided to HAL_SetMain().
+ *
+ * If HAL_SetMain() has not been called, simply sleeps until HAL_ExitMain()
+ * is called.
+ */
+void HAL_RunMain(void);
+
+/**
+ * Causes HAL_RunMain() to exit.
+ *
+ * If HAL_SetMain() has been called, this calls the exit function provided
+ * to that function.
+ */
+void HAL_ExitMain(void);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
+/** @} */
diff --git a/hal/src/main/native/include/hal/Notifier.h b/hal/src/main/native/include/hal/Notifier.h
new file mode 100644
index 0000000..6b8e39f
--- /dev/null
+++ b/hal/src/main/native/include/hal/Notifier.h
@@ -0,0 +1,97 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 <stdint.h>
+
+#include "hal/Types.h"
+
+/**
+ * @defgroup hal_notifier Notifier Functions
+ * @ingroup hal_capi
+ * @{
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * Initializes a notifier.
+ *
+ * A notifier is an FPGA controller timer that triggers at requested intervals
+ * based on the FPGA time. This can be used to make precise control loops.
+ *
+ * @return the created notifier
+ */
+HAL_NotifierHandle HAL_InitializeNotifier(int32_t* status);
+
+/**
+ * Sets the name of a notifier.
+ *
+ * @param notifierHandle the notifier handle
+ * @param name name
+ */
+void HAL_SetNotifierName(HAL_NotifierHandle notifierHandle, const char* name,
+ int32_t* status);
+
+/**
+ * Stops a notifier from running.
+ *
+ * This will cause any call into HAL_WaitForNotifierAlarm to return.
+ *
+ * @param notifierHandle the notifier handle
+ */
+void HAL_StopNotifier(HAL_NotifierHandle notifierHandle, int32_t* status);
+
+/**
+ * Cleans a notifier.
+ *
+ * Note this also stops a notifier if it is already running.
+ *
+ * @param notifierHandle the notifier handle
+ */
+void HAL_CleanNotifier(HAL_NotifierHandle notifierHandle, int32_t* status);
+
+/**
+ * Updates the trigger time for a notifier.
+ *
+ * Note that this time is an absolute time relative to HAL_GetFPGATime()
+ *
+ * @param notifierHandle the notifier handle
+ * @param triggerTime the updated trigger time
+ */
+void HAL_UpdateNotifierAlarm(HAL_NotifierHandle notifierHandle,
+ uint64_t triggerTime, int32_t* status);
+
+/**
+ * Cancels the next notifier alarm.
+ *
+ * This does not cause HAL_WaitForNotifierAlarm to return.
+ *
+ * @param notifierHandle the notifier handle
+ */
+void HAL_CancelNotifierAlarm(HAL_NotifierHandle notifierHandle,
+ int32_t* status);
+
+/**
+ * Waits for the next alarm for the specific notifier.
+ *
+ * This is a blocking call until either the time elapses or HAL_StopNotifier
+ * gets called.
+ *
+ * @param notifierHandle the notifier handle
+ * @return the FPGA time the notifier returned
+ */
+uint64_t HAL_WaitForNotifierAlarm(HAL_NotifierHandle notifierHandle,
+ int32_t* status);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
+/** @} */
diff --git a/hal/src/main/native/include/hal/PDP.h b/hal/src/main/native/include/hal/PDP.h
new file mode 100644
index 0000000..c80e8b6
--- /dev/null
+++ b/hal/src/main/native/include/hal/PDP.h
@@ -0,0 +1,133 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 <stdint.h>
+
+#include "hal/Types.h"
+
+/**
+ * @defgroup hal_pdp PDP Functions
+ * @ingroup hal_capi
+ * Functions to control the Power Distribution Panel.
+ * @{
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * Initializes a Power Distribution Panel.
+ *
+ * @param module the module number to initialize
+ * @return the created PDP
+ */
+HAL_PDPHandle HAL_InitializePDP(int32_t module, int32_t* status);
+
+/**
+ * Cleans a PDP module.
+ *
+ * @param handle the module handle
+ */
+void HAL_CleanPDP(HAL_PDPHandle handle);
+
+/**
+ * Checks if a PDP channel is valid.
+ *
+ * @param channel the channel to check
+ * @return true if the channel is valid, otherwise false
+ */
+HAL_Bool HAL_CheckPDPChannel(int32_t channel);
+
+/**
+ * Checks if a PDP module is valid.
+ *
+ * @param channel the module to check
+ * @return true if the module is valid, otherwise false
+ */
+HAL_Bool HAL_CheckPDPModule(int32_t module);
+
+/**
+ * Gets the temperature of the PDP.
+ *
+ * @param handle the module handle
+ * @return the module temperature (celsius)
+ */
+double HAL_GetPDPTemperature(HAL_PDPHandle handle, int32_t* status);
+
+/**
+ * Gets the PDP input voltage.
+ *
+ * @param handle the module handle
+ * @return the input voltage (volts)
+ */
+double HAL_GetPDPVoltage(HAL_PDPHandle handle, int32_t* status);
+
+/**
+ * Gets the current of a specific PDP channel.
+ *
+ * @param module the module
+ * @param channel the channel
+ * @return the channel current (amps)
+ */
+double HAL_GetPDPChannelCurrent(HAL_PDPHandle handle, int32_t channel,
+ int32_t* status);
+
+/**
+ * Gets the current of all 16 channels on the PDP.
+ *
+ * The array must be large enough to hold all channels.
+ *
+ * @param handle the module handle
+ * @param current the currents (output)
+ */
+void HAL_GetPDPAllChannelCurrents(HAL_PDPHandle handle, double* currents,
+ int32_t* status);
+
+/**
+ * Gets the total current of the PDP.
+ *
+ * @param handle the module handle
+ * @return the total current (amps)
+ */
+double HAL_GetPDPTotalCurrent(HAL_PDPHandle handle, int32_t* status);
+
+/**
+ * Gets the total power of the PDP.
+ *
+ * @param handle the module handle
+ * @return the total power (watts)
+ */
+double HAL_GetPDPTotalPower(HAL_PDPHandle handle, int32_t* status);
+
+/**
+ * Gets the total energy of the PDP.
+ *
+ * @param handle the module handle
+ * @return the total energy (joules)
+ */
+double HAL_GetPDPTotalEnergy(HAL_PDPHandle handle, int32_t* status);
+
+/**
+ * Resets the PDP accumulated energy.
+ *
+ * @param handle the module handle
+ */
+void HAL_ResetPDPTotalEnergy(HAL_PDPHandle handle, int32_t* status);
+
+/**
+ * Clears any PDP sticky faults.
+ *
+ * @param handle the module handle
+ */
+void HAL_ClearPDPStickyFaults(HAL_PDPHandle handle, int32_t* status);
+#ifdef __cplusplus
+} // extern "C"
+#endif
+/** @} */
diff --git a/hal/src/main/native/include/hal/PWM.h b/hal/src/main/native/include/hal/PWM.h
new file mode 100644
index 0000000..781a423
--- /dev/null
+++ b/hal/src/main/native/include/hal/PWM.h
@@ -0,0 +1,233 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 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 <stdint.h>
+
+#include "hal/Types.h"
+
+/**
+ * @defgroup hal_pwm PWM Output Functions
+ * @ingroup hal_capi
+ * @{
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * Initializes a PWM port.
+ *
+ * @param portHandle the port to initialize
+ * @return the created pwm handle
+ */
+HAL_DigitalHandle HAL_InitializePWMPort(HAL_PortHandle portHandle,
+ int32_t* status);
+
+/**
+ * Frees a PWM port.
+ *
+ * @param pwmPortHandle the pwm handle
+ */
+void HAL_FreePWMPort(HAL_DigitalHandle pwmPortHandle, int32_t* status);
+
+/**
+ * Checks if a pwm channel is valid.
+ *
+ * @param channel the channel to check
+ * @return true if the channel is valid, otherwise false
+ */
+HAL_Bool HAL_CheckPWMChannel(int32_t channel);
+
+/**
+ * Sets the configuration settings for the PWM channel.
+ *
+ * All values are in milliseconds.
+ *
+ * @param pwmPortHandle the PWM handle
+ * @param maxPwm the maximum PWM value
+ * @param deadbandMaxPwm the high range of the center deadband
+ * @param centerPwm the center PWM value
+ * @param deadbandMinPwm the low range of the center deadband
+ * @param minPwm the minimum PWM value
+ */
+void HAL_SetPWMConfig(HAL_DigitalHandle pwmPortHandle, double maxPwm,
+ double deadbandMaxPwm, double centerPwm,
+ double deadbandMinPwm, double minPwm, int32_t* status);
+
+/**
+ * Sets the raw configuration settings for the PWM channel.
+ *
+ * We recommend using HAL_SetPWMConfig() instead, as those values are properly
+ * scaled. Usually used for values grabbed by HAL_GetPWMConfigRaw().
+ *
+ * Values are in raw FPGA units.
+ *
+ * @param pwmPortHandle the PWM handle
+ * @param maxPwm the maximum PWM value
+ * @param deadbandMaxPwm the high range of the center deadband
+ * @param centerPwm the center PWM value
+ * @param deadbandMinPwm the low range of the center deadband
+ * @param minPwm the minimum PWM value
+ */
+void HAL_SetPWMConfigRaw(HAL_DigitalHandle pwmPortHandle, int32_t maxPwm,
+ int32_t deadbandMaxPwm, int32_t centerPwm,
+ int32_t deadbandMinPwm, int32_t minPwm,
+ int32_t* status);
+
+/**
+ * Gets the raw pwm configuration settings for the PWM channel.
+ *
+ * Values are in raw FPGA units. These units have the potential to change for
+ * any FPGA release.
+ *
+ * @param pwmPortHandle the PWM handle
+ * @param maxPwm the maximum PWM value
+ * @param deadbandMaxPwm the high range of the center deadband
+ * @param centerPwm the center PWM value
+ * @param deadbandMinPwm the low range of the center deadband
+ * @param minPwm the minimum PWM value
+ */
+void HAL_GetPWMConfigRaw(HAL_DigitalHandle pwmPortHandle, int32_t* maxPwm,
+ int32_t* deadbandMaxPwm, int32_t* centerPwm,
+ int32_t* deadbandMinPwm, int32_t* minPwm,
+ int32_t* status);
+
+/**
+ * Sets if the FPGA should output the center value if the input value is within
+ * the deadband.
+ *
+ * @param pwmPortHandle the PWM handle
+ * @param eliminateDeadband true to eliminate deadband, otherwise false
+ */
+void HAL_SetPWMEliminateDeadband(HAL_DigitalHandle pwmPortHandle,
+ HAL_Bool eliminateDeadband, int32_t* status);
+
+/**
+ * Gets the current eliminate deadband value.
+ *
+ * @param pwmPortHandle the PWM handle
+ * @return true if set, otherwise false
+ */
+HAL_Bool HAL_GetPWMEliminateDeadband(HAL_DigitalHandle pwmPortHandle,
+ int32_t* status);
+
+/**
+ * Sets a PWM channel to the desired value.
+ *
+ * The values are in raw FPGA units, and have the potential to change with any
+ * FPGA release.
+ *
+ * @param pwmPortHandle the PWM handle
+ * @param value the PWM value to set
+ */
+void HAL_SetPWMRaw(HAL_DigitalHandle pwmPortHandle, int32_t value,
+ int32_t* status);
+
+/**
+ * Sets a PWM channel to the desired scaled value.
+ *
+ * The values range from -1 to 1 and the period is controlled by the PWM Period
+ * and MinHigh registers.
+ *
+ * @param pwmPortHandle the PWM handle
+ * @param value the scaled PWM value to set
+ */
+void HAL_SetPWMSpeed(HAL_DigitalHandle pwmPortHandle, double speed,
+ int32_t* status);
+
+/**
+ * Sets a PWM channel to the desired position value.
+ *
+ * The values range from 0 to 1 and the period is controlled by the PWM Period
+ * and MinHigh registers.
+ *
+ * @param pwmPortHandle the PWM handle
+ * @param value the positional PWM value to set
+ */
+void HAL_SetPWMPosition(HAL_DigitalHandle pwmPortHandle, double position,
+ int32_t* status);
+
+/**
+ * Sets a PWM channel to be disabled.
+ *
+ * The channel is disabled until the next time it is set. Note this is different
+ * from just setting a 0 speed, as this will actively stop all signalling on the
+ * channel.
+ *
+ * @param pwmPortHandle the PWM handle.
+ */
+void HAL_SetPWMDisabled(HAL_DigitalHandle pwmPortHandle, int32_t* status);
+
+/**
+ * Gets a value from a PWM channel.
+ *
+ * The values are in raw FPGA units, and have the potential to change with any
+ * FPGA release.
+ *
+ * @param pwmPortHandle the PWM handle
+ * @return the current raw PWM value
+ */
+int32_t HAL_GetPWMRaw(HAL_DigitalHandle pwmPortHandle, int32_t* status);
+
+/**
+ * Gets a scaled value from a PWM channel.
+ *
+ * The values range from -1 to 1.
+ *
+ * @param pwmPortHandle the PWM handle
+ * @return the current speed PWM value
+ */
+double HAL_GetPWMSpeed(HAL_DigitalHandle pwmPortHandle, int32_t* status);
+
+/**
+ * Gets a position value from a PWM channel.
+ *
+ * The values range from 0 to 1.
+ *
+ * @param pwmPortHandle the PWM handle
+ * @return the current positional PWM value
+ */
+double HAL_GetPWMPosition(HAL_DigitalHandle pwmPortHandle, int32_t* status);
+
+/**
+ * Forces a PWM signal to go to 0 temporarily.
+ *
+ * @param pwmPortHandle the PWM handle.
+ */
+void HAL_LatchPWMZero(HAL_DigitalHandle pwmPortHandle, int32_t* status);
+
+/**
+ * Sets how how often the PWM signal is squelched, thus scaling the period.
+ *
+ * @param pwmPortHandle the PWM handle.
+ * @param squelchMask the 2-bit mask of outputs to squelch
+ */
+void HAL_SetPWMPeriodScale(HAL_DigitalHandle pwmPortHandle, int32_t squelchMask,
+ int32_t* status);
+
+/**
+ * Gets the loop timing of the PWM system.
+ *
+ * @return the loop time
+ */
+int32_t HAL_GetPWMLoopTiming(int32_t* status);
+
+/**
+ * Gets the pwm starting cycle time.
+ *
+ * This time is relative to the FPGA time.
+ *
+ * @return the pwm cycle start time
+ */
+uint64_t HAL_GetPWMCycleStartTime(int32_t* status);
+#ifdef __cplusplus
+} // extern "C"
+#endif
+/** @} */
diff --git a/hal/src/main/native/include/hal/Ports.h b/hal/src/main/native/include/hal/Ports.h
new file mode 100644
index 0000000..584bc4f
--- /dev/null
+++ b/hal/src/main/native/include/hal/Ports.h
@@ -0,0 +1,164 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 <stdint.h>
+
+/**
+ * @defgroup hal_ports Ports Functions
+ * @ingroup hal_capi
+ * @{
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * Gets the number of analog accumulators in the current system.
+ *
+ * @return the number of analog accumulators
+ */
+int32_t HAL_GetNumAccumulators(void);
+
+/**
+ * Gets the number of analog triggers in the current system.
+ *
+ * @return the number of analog triggers
+ */
+int32_t HAL_GetNumAnalogTriggers(void);
+
+/**
+ * Gets the number of analog inputs in the current system.
+ *
+ * @return the number of analog inputs
+ */
+int32_t HAL_GetNumAnalogInputs(void);
+
+/**
+ * Gets the number of analog outputs in the current system.
+ *
+ * @return the number of analog outputs
+ */
+int32_t HAL_GetNumAnalogOutputs(void);
+
+/**
+ * Gets the number of analog counters in the current system.
+ *
+ * @return the number of counters
+ */
+int32_t HAL_GetNumCounters(void);
+
+/**
+ * Gets the number of digital headers in the current system.
+ *
+ * @return the number of digital headers
+ */
+int32_t HAL_GetNumDigitalHeaders(void);
+
+/**
+ * Gets the number of PWM headers in the current system.
+ *
+ * @return the number of PWM headers
+ */
+int32_t HAL_GetNumPWMHeaders(void);
+
+/**
+ * Gets the number of digital channels in the current system.
+ *
+ * @return the number of digital channels
+ */
+int32_t HAL_GetNumDigitalChannels(void);
+
+/**
+ * Gets the number of PWM channels in the current system.
+ *
+ * @return the number of PWM channels
+ */
+int32_t HAL_GetNumPWMChannels(void);
+
+/**
+ * Gets the number of digital IO PWM outputs in the current system.
+ *
+ * @return the number of digital IO PWM outputs
+ */
+int32_t HAL_GetNumDigitalPWMOutputs(void);
+
+/**
+ * Gets the number of quadrature encoders in the current system.
+ *
+ * @return the number of quadrature encoders
+ */
+int32_t HAL_GetNumEncoders(void);
+
+/**
+ * Gets the number of interrupts in the current system.
+ *
+ * @return the number of interrupts
+ */
+int32_t HAL_GetNumInterrupts(void);
+
+/**
+ * Gets the number of relay channels in the current system.
+ *
+ * @return the number of relay channels
+ */
+int32_t HAL_GetNumRelayChannels(void);
+
+/**
+ * Gets the number of relay headers in the current system.
+ *
+ * @return the number of relay headers
+ */
+int32_t HAL_GetNumRelayHeaders(void);
+
+/**
+ * Gets the number of PCM modules in the current system.
+ *
+ * @return the number of PCM modules
+ */
+int32_t HAL_GetNumPCMModules(void);
+
+/**
+ * Gets the number of solenoid channels in the current system.
+ *
+ * @return the number of solenoid channels
+ */
+int32_t HAL_GetNumSolenoidChannels(void);
+
+/**
+ * Gets the number of PDP modules in the current system.
+ *
+ * @return the number of PDP modules
+ */
+int32_t HAL_GetNumPDPModules(void);
+
+/**
+ * Gets the number of PDP channels in the current system.
+ *
+ * @return the number of PDP channels
+ */
+int32_t HAL_GetNumPDPChannels(void);
+
+/**
+ * Gets the number of duty cycle inputs in the current system.
+ *
+ * @return the number of Duty Cycle inputs
+ */
+int32_t HAL_GetNumDutyCycles(void);
+
+/**
+ * Gets the number of addressable LED generators in the current system.
+ *
+ * @return the number of Addressable LED generators
+ */
+int32_t HAL_GetNumAddressableLEDs(void);
+#ifdef __cplusplus
+} // extern "C"
+#endif
+/** @} */
diff --git a/hal/src/main/native/include/hal/Power.h b/hal/src/main/native/include/hal/Power.h
new file mode 100644
index 0000000..7ac7991
--- /dev/null
+++ b/hal/src/main/native/include/hal/Power.h
@@ -0,0 +1,124 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 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 <stdint.h>
+
+#include "hal/Types.h"
+
+/**
+ * @defgroup hal_power Power Functions
+ * @ingroup hal_capi
+ * @{
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * Gets the roboRIO input voltage.
+ *
+ * @return the input voltage (volts)
+ */
+double HAL_GetVinVoltage(int32_t* status);
+
+/**
+ * Gets the roboRIO input current.
+ *
+ * @return the input current (amps)
+ */
+double HAL_GetVinCurrent(int32_t* status);
+
+/**
+ * Gets the 6V rail voltage.
+ *
+ * @return the 6V rail voltage (volts)
+ */
+double HAL_GetUserVoltage6V(int32_t* status);
+
+/**
+ * Gets the 6V rail current.
+ *
+ * @return the 6V rail current (amps)
+ */
+double HAL_GetUserCurrent6V(int32_t* status);
+
+/**
+ * Gets the active state of the 6V rail.
+ *
+ * @return true if the rail is active, otherwise false
+ */
+HAL_Bool HAL_GetUserActive6V(int32_t* status);
+
+/**
+ * Gets the fault count for the 6V rail.
+ *
+ * @return the number of 6V fault counts
+ */
+int32_t HAL_GetUserCurrentFaults6V(int32_t* status);
+
+/**
+ * Gets the 5V rail voltage.
+ *
+ * @return the 5V rail voltage (volts)
+ */
+double HAL_GetUserVoltage5V(int32_t* status);
+
+/**
+ * Gets the 5V rail current.
+ *
+ * @return the 5V rail current (amps)
+ */
+double HAL_GetUserCurrent5V(int32_t* status);
+
+/**
+ * Gets the active state of the 5V rail.
+ *
+ * @return true if the rail is active, otherwise false
+ */
+HAL_Bool HAL_GetUserActive5V(int32_t* status);
+
+/**
+ * Gets the fault count for the 5V rail.
+ *
+ * @return the number of 5V fault counts
+ */
+int32_t HAL_GetUserCurrentFaults5V(int32_t* status);
+
+/**
+ * Gets the 3V3 rail voltage.
+ *
+ * @return the 3V3 rail voltage (volts)
+ */
+double HAL_GetUserVoltage3V3(int32_t* status);
+
+/**
+ * Gets the 3V3 rail current.
+ *
+ * @return the 3V3 rail current (amps)
+ */
+double HAL_GetUserCurrent3V3(int32_t* status);
+
+/**
+ * Gets the active state of the 3V3 rail.
+ *
+ * @return true if the rail is active, otherwise false
+ */
+HAL_Bool HAL_GetUserActive3V3(int32_t* status);
+
+/**
+ * Gets the fault count for the 3V3 rail.
+ *
+ * @return the number of 3V3 fault counts
+ */
+int32_t HAL_GetUserCurrentFaults3V3(int32_t* status);
+#ifdef __cplusplus
+} // extern "C"
+#endif
+/** @} */
diff --git a/hal/src/main/native/include/hal/Relay.h b/hal/src/main/native/include/hal/Relay.h
new file mode 100644
index 0000000..281aad6
--- /dev/null
+++ b/hal/src/main/native/include/hal/Relay.h
@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 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 <stdint.h>
+
+#include "hal/Types.h"
+
+/**
+ * @defgroup hal_relay Relay Output Functions
+ * @ingroup hal_capi
+ * @{
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * Initializes a relay.
+ *
+ * Note this call will only initialize either the forward or reverse port of the
+ * relay. If you need both, you will need to initialize 2 relays.
+ *
+ * @param portHandle the port handle to initialize
+ * @param fwd true for the forward port, false for the reverse port
+ * @return the created relay handle
+ */
+HAL_RelayHandle HAL_InitializeRelayPort(HAL_PortHandle portHandle, HAL_Bool fwd,
+ int32_t* status);
+
+/**
+ * Frees a relay port.
+ *
+ * @param relayPortHandle the relay handle
+ */
+void HAL_FreeRelayPort(HAL_RelayHandle relayPortHandle);
+
+/**
+ * Checks if a relay channel is valid.
+ *
+ * @param channel the channel to check
+ * @return true if the channel is valid, otherwise false
+ */
+HAL_Bool HAL_CheckRelayChannel(int32_t channel);
+
+/**
+ * Sets the state of a relay output.
+ *
+ * @param relayPortHandle the relay handle
+ * @param on true for on, false for off
+ */
+void HAL_SetRelay(HAL_RelayHandle relayPortHandle, HAL_Bool on,
+ int32_t* status);
+
+/**
+ * Gets the current state of the relay channel.
+ *
+ * @param relayPortHandle the relay handle
+ * @return true for on, false for off
+ */
+HAL_Bool HAL_GetRelay(HAL_RelayHandle relayPortHandle, int32_t* status);
+#ifdef __cplusplus
+} // extern "C"
+#endif
+/** @} */
diff --git a/hal/src/main/native/include/hal/SPI.h b/hal/src/main/native/include/hal/SPI.h
new file mode 100644
index 0000000..abee379
--- /dev/null
+++ b/hal/src/main/native/include/hal/SPI.h
@@ -0,0 +1,263 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 <stdint.h>
+
+#include "hal/AnalogTrigger.h"
+#include "hal/SPITypes.h"
+#include "hal/Types.h"
+
+/**
+ * @defgroup hal_spi SPI Functions
+ * @ingroup hal_capi
+ * @{
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * Initializes the SPI port. Opens the port if necessary and saves the handle.
+ *
+ * If opening the MXP port, also sets up the channel functions appropriately.
+ *
+ * @param port The number of the port to use. 0-3 for Onboard CS0-CS3, 4 for MXP
+ */
+void HAL_InitializeSPI(HAL_SPIPort port, int32_t* status);
+
+/**
+ * Performs an SPI send/receive transaction.
+ *
+ * This is a lower-level interface to the spi hardware giving you more control
+ * over each transaction.
+ *
+ * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4
+ * for MXP
+ * @param dataToSend Buffer of data to send as part of the transaction.
+ * @param dataReceived Buffer to read data into.
+ * @param size Number of bytes to transfer. [0..7]
+ * @return Number of bytes transferred, -1 for error
+ */
+int32_t HAL_TransactionSPI(HAL_SPIPort port, const uint8_t* dataToSend,
+ uint8_t* dataReceived, int32_t size);
+
+/**
+ * Executes a write transaction with the device.
+ *
+ * Writes to a device and wait until the transaction is complete.
+ *
+ * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4
+ * for MXP
+ * @param datToSend The data to write to the register on the device.
+ * @param sendSize The number of bytes to be written
+ * @return The number of bytes written. -1 for an error
+ */
+int32_t HAL_WriteSPI(HAL_SPIPort port, const uint8_t* dataToSend,
+ int32_t sendSize);
+
+/**
+ * Executes a read from the device.
+ *
+ * This method does not write any data out to the device.
+ *
+ * Most spi devices will require a register address to be written before they
+ * begin returning data.
+ *
+ * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for
+ * MXP
+ * @param buffer A pointer to the array of bytes to store the data read from the
+ * device.
+ * @param count The number of bytes to read in the transaction. [1..7]
+ * @return Number of bytes read. -1 for error.
+ */
+int32_t HAL_ReadSPI(HAL_SPIPort port, uint8_t* buffer, int32_t count);
+
+/**
+ * Closes the SPI port.
+ *
+ * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP
+ */
+void HAL_CloseSPI(HAL_SPIPort port);
+
+/**
+ * Sets the clock speed for the SPI bus.
+ *
+ * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for
+ * MXP
+ * @param speed The speed in Hz (0-1MHz)
+ */
+void HAL_SetSPISpeed(HAL_SPIPort port, int32_t speed);
+
+/**
+ * Sets the SPI options.
+ *
+ * @param port The number of the port to use. 0-3 for Onboard
+ * CS0-CS2, 4 for MXP
+ * @param msbFirst True to write the MSB first, False for LSB first
+ * @param sampleOnTrailing True to sample on the trailing edge, False to sample
+ * on the leading edge
+ * @param clkIdleHigh True to set the clock to active low, False to set the
+ * clock active high
+ */
+void HAL_SetSPIOpts(HAL_SPIPort port, HAL_Bool msbFirst,
+ HAL_Bool sampleOnTrailing, HAL_Bool clkIdleHigh);
+
+/**
+ * Sets the CS Active high for a SPI port.
+ *
+ * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP
+ */
+void HAL_SetSPIChipSelectActiveHigh(HAL_SPIPort port, int32_t* status);
+
+/**
+ * Sets the CS Active low for a SPI port.
+ *
+ * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP
+ */
+void HAL_SetSPIChipSelectActiveLow(HAL_SPIPort port, int32_t* status);
+
+/**
+ * Gets the stored handle for a SPI port.
+ *
+ * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP
+ * @return The stored handle for the SPI port. 0 represents no stored
+ * handle.
+ */
+int32_t HAL_GetSPIHandle(HAL_SPIPort port);
+
+/**
+ * Sets the stored handle for a SPI port.
+ *
+ * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for
+ * MXP.
+ * @param handle The value of the handle for the port.
+ */
+void HAL_SetSPIHandle(HAL_SPIPort port, int32_t handle);
+
+/**
+ * Initializes the SPI automatic accumulator.
+ *
+ * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4
+ * for MXP.
+ * @param bufferSize The accumulator buffer size.
+ */
+void HAL_InitSPIAuto(HAL_SPIPort port, int32_t bufferSize, int32_t* status);
+
+/**
+ * Frees an SPI automatic accumulator.
+ *
+ * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for
+ * MXP.
+ */
+void HAL_FreeSPIAuto(HAL_SPIPort port, int32_t* status);
+
+/**
+ * Sets the period for automatic SPI accumulation.
+ *
+ * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for
+ * MXP.
+ * @param period The accumlation period (seconds).
+ */
+void HAL_StartSPIAutoRate(HAL_SPIPort port, double period, int32_t* status);
+
+/**
+ * Starts the auto SPI accumulator on a specific trigger.
+ *
+ * Note that triggering on both rising and falling edges is a valid
+ * configuration.
+ *
+ * @param port The number of the port to use. 0-3 for Onboard
+ * CS0-CS2, 4 for MXP.
+ * @param digitalSourceHandle The trigger source to use (Either
+ * HAL_AnalogTriggerHandle or HAL_DigitalHandle).
+ * @param analogTriggerType The analog trigger type, if the source is an
+ * analog trigger.
+ * @param triggerRising Trigger on the rising edge if true.
+ * @param triggerFalling Trigger on the falling edge if true.
+ */
+void HAL_StartSPIAutoTrigger(HAL_SPIPort port, HAL_Handle digitalSourceHandle,
+ HAL_AnalogTriggerType analogTriggerType,
+ HAL_Bool triggerRising, HAL_Bool triggerFalling,
+ int32_t* status);
+
+/**
+ * Stops an automatic SPI accumlation.
+ *
+ * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for
+ * MXP.
+ */
+void HAL_StopSPIAuto(HAL_SPIPort port, int32_t* status);
+
+/**
+ * Sets the data to be transmitted to the device to initiate a read.
+ *
+ * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4
+ * for MXP.
+ * @param dataToSend Pointer to the data to send (Gets copied for continue use,
+ * so no need to keep alive).
+ * @param dataSize The length of the data to send.
+ * @param zeroSize The number of zeros to send after the data.
+ */
+void HAL_SetSPIAutoTransmitData(HAL_SPIPort port, const uint8_t* dataToSend,
+ int32_t dataSize, int32_t zeroSize,
+ int32_t* status);
+
+/**
+ * Immediately forces an SPI read to happen.
+ *
+ * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for
+ * MXP.
+ */
+void HAL_ForceSPIAutoRead(HAL_SPIPort port, int32_t* status);
+
+/**
+ * Reads data received by the SPI accumulator. Each received data sequence
+ * consists of a timestamp followed by the received data bytes, one byte per
+ * word (in the least significant byte). The length of each received data
+ * sequence is the same as the combined dataSize + zeroSize set in
+ * HAL_SetSPIAutoTransmitData.
+ *
+ * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4
+ * for MXP.
+ * @param buffer The buffer to store the data into.
+ * @param numToRead The number of words to read.
+ * @param timeout The read timeout (in seconds).
+ * @return The number of words actually read.
+ */
+int32_t HAL_ReadSPIAutoReceivedData(HAL_SPIPort port, uint32_t* buffer,
+ int32_t numToRead, double timeout,
+ int32_t* status);
+
+/**
+ * Gets the count of how many SPI accumulations have been missed.
+ *
+ * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for
+ * MXP.
+ * @return The number of missed accumulations.
+ */
+int32_t HAL_GetSPIAutoDroppedCount(HAL_SPIPort port, int32_t* status);
+
+/**
+ * Configure the Auto SPI Stall time between reads.
+ *
+ * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for
+ * MXP.
+ * @param csToSclkTicks the number of ticks to wait before asserting the cs pin
+ * @param stallTicks the number of ticks to stall for
+ * @param pow2BytesPerRead the number of bytes to read before stalling
+ */
+void HAL_ConfigureSPIAutoStall(HAL_SPIPort port, int32_t csToSclkTicks,
+ int32_t stallTicks, int32_t pow2BytesPerRead,
+ int32_t* status);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
+/** @} */
diff --git a/hal/src/main/native/include/hal/SPITypes.h b/hal/src/main/native/include/hal/SPITypes.h
new file mode 100644
index 0000000..170bd27
--- /dev/null
+++ b/hal/src/main/native/include/hal/SPITypes.h
@@ -0,0 +1,42 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 <stdint.h>
+
+#include "hal/Types.h"
+
+/**
+ * @defgroup hal_spi SPI Functions
+ * @ingroup hal_capi
+ * @{
+ */
+
+// clang-format off
+HAL_ENUM(HAL_SPIPort) {
+ HAL_SPI_kInvalid = -1,
+ HAL_SPI_kOnboardCS0,
+ HAL_SPI_kOnboardCS1,
+ HAL_SPI_kOnboardCS2,
+ HAL_SPI_kOnboardCS3,
+ HAL_SPI_kMXP
+};
+// clang-format on
+
+#ifdef __cplusplus
+namespace hal {
+
+/**
+ * A move-only C++ wrapper around HAL_SPIPort.
+ * Does not ensure destruction.
+ */
+using SPIPort = Handle<HAL_SPIPort, HAL_SPI_kInvalid>;
+
+} // namespace hal
+#endif
+/** @} */
diff --git a/hal/src/main/native/include/hal/SerialPort.h b/hal/src/main/native/include/hal/SerialPort.h
new file mode 100644
index 0000000..226a2cb
--- /dev/null
+++ b/hal/src/main/native/include/hal/SerialPort.h
@@ -0,0 +1,246 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 <stdint.h>
+
+#include "hal/Types.h"
+
+/**
+ * @defgroup hal_serialport Serial Port Functions
+ * @ingroup hal_capi
+ * @{
+ */
+
+// clang-format off
+HAL_ENUM(HAL_SerialPort) {
+ HAL_SerialPort_Onboard = 0,
+ HAL_SerialPort_MXP = 1,
+ HAL_SerialPort_USB1 = 2,
+ HAL_SerialPort_USB2 = 3
+};
+// clang-format on
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * Initializes a serial port.
+ *
+ * The channels are either the onboard RS232, the mxp uart, or 2 USB ports. The
+ * top port is USB1, the bottom port is USB2.
+ *
+ * @param port the serial port to initialize
+ */
+HAL_SerialPortHandle HAL_InitializeSerialPort(HAL_SerialPort port,
+ int32_t* status);
+
+/**
+ * Initializes a serial port with a direct name.
+ *
+ * This name is the /dev name for a specific port.
+ * Note these are not always consistent between roboRIO reboots.
+ *
+ * @param port the serial port to initialize
+ * @param portName the dev port name
+ */
+HAL_SerialPortHandle HAL_InitializeSerialPortDirect(HAL_SerialPort port,
+ const char* portName,
+ int32_t* status);
+
+/**
+ * Gets the raw serial port file descriptor from a handle.
+ *
+ * @param handle the serial port handle
+ * @return the raw port descriptor
+ */
+int HAL_GetSerialFD(HAL_SerialPortHandle handle, int32_t* status);
+
+/**
+ * Sets the baud rate of a serial port.
+ *
+ * Any value between 0 and 0xFFFFFFFF may be used. Default is 9600.
+ *
+ * @param handle the serial port handle
+ * @param baud the baud rate to set
+ */
+void HAL_SetSerialBaudRate(HAL_SerialPortHandle handle, int32_t baud,
+ int32_t* status);
+
+/**
+ * Sets the number of data bits on a serial port.
+ *
+ * Defaults to 8.
+ *
+ * @param handle the serial port handle
+ * @param bits the number of data bits (5-8)
+ */
+void HAL_SetSerialDataBits(HAL_SerialPortHandle handle, int32_t bits,
+ int32_t* status);
+
+/**
+ * Sets the number of parity bits on a serial port.
+ *
+ * Valid values are:
+ * 0: None (default)
+ * 1: Odd
+ * 2: Even
+ * 3: Mark - Means exists and always 1
+ * 4: Space - Means exists and always 0
+ *
+ * @param handle the serial port handle
+ * @param parity the parity bit mode (see remarks for valid values)
+ */
+void HAL_SetSerialParity(HAL_SerialPortHandle handle, int32_t parity,
+ int32_t* status);
+
+/**
+ * Sets the number of stop bits on a serial port.
+ *
+ * Valid values are:
+ * 10: One stop bit (default)
+ * 15: One and a half stop bits
+ * 20: Two stop bits
+ *
+ * @param handle the serial port handle
+ * @param stopBits the stop bit value (see remarks for valid values)
+ */
+void HAL_SetSerialStopBits(HAL_SerialPortHandle handle, int32_t stopBits,
+ int32_t* status);
+
+/**
+ * Sets the write mode on a serial port.
+ *
+ * Valid values are:
+ * 1: Flush on access
+ * 2: Flush when full (default)
+ *
+ * @param handle the serial port handle
+ * @param mode the mode to set (see remarks for valid values)
+ */
+void HAL_SetSerialWriteMode(HAL_SerialPortHandle handle, int32_t mode,
+ int32_t* status);
+
+/**
+ * Sets the flow control mode of a serial port.
+ *
+ * Valid values are:
+ * 0: None (default)
+ * 1: XON-XOFF
+ * 2: RTS-CTS
+ * 3: DTR-DSR
+ *
+ * @param handle the serial port handle
+ * @param flow the mode to set (see remarks for valid values)
+ */
+void HAL_SetSerialFlowControl(HAL_SerialPortHandle handle, int32_t flow,
+ int32_t* status);
+
+/**
+ * Sets the minimum serial read timeout of a port.
+ *
+ * @param handle the serial port handle
+ * @param timeout the timeout in milliseconds
+ */
+void HAL_SetSerialTimeout(HAL_SerialPortHandle handle, double timeout,
+ int32_t* status);
+
+/**
+ * Sets the termination character that terminates a read.
+ *
+ * By default this is disabled.
+ *
+ * @param handle the serial port handle
+ * @param terminator the termination character to set
+ */
+void HAL_EnableSerialTermination(HAL_SerialPortHandle handle, char terminator,
+ int32_t* status);
+
+/**
+ * Disables a termination character for reads.
+ *
+ * @param handle the serial port handle
+ */
+void HAL_DisableSerialTermination(HAL_SerialPortHandle handle, int32_t* status);
+
+/**
+ * Sets the size of the read buffer.
+ *
+ * @param handle the serial port handle
+ * @param size the read buffer size
+ */
+void HAL_SetSerialReadBufferSize(HAL_SerialPortHandle handle, int32_t size,
+ int32_t* status);
+
+/**
+ * Sets the size of the write buffer.
+ *
+ * @param handle the serial port handle
+ * @param size the write buffer size
+ */
+void HAL_SetSerialWriteBufferSize(HAL_SerialPortHandle handle, int32_t size,
+ int32_t* status);
+
+/**
+ * Gets the number of bytes currently in the read buffer.
+ *
+ * @param handle the serial port handle
+ * @return the number of bytes in the read buffer
+ */
+int32_t HAL_GetSerialBytesReceived(HAL_SerialPortHandle handle,
+ int32_t* status);
+
+/**
+ * Reads data from the serial port.
+ *
+ * Will wait for either timeout (if set), the termination char (if set), or the
+ * count to be full. Whichever one comes first.
+ *
+ * @param handle the serial port handle
+ * @param count the number of bytes maximum to read
+ * @return the number of bytes actually read
+ */
+int32_t HAL_ReadSerial(HAL_SerialPortHandle handle, char* buffer, int32_t count,
+ int32_t* status);
+
+/**
+ * Writes data to the serial port.
+ *
+ * @param handle the serial port handle
+ * @param buffer the buffer to write
+ * @param count the number of bytes to write from the buffer
+ * @return the number of bytes actually written
+ */
+int32_t HAL_WriteSerial(HAL_SerialPortHandle handle, const char* buffer,
+ int32_t count, int32_t* status);
+
+/**
+ * Flushes the serial write buffer out to the port.
+ *
+ * @param handle the serial port handle
+ */
+void HAL_FlushSerial(HAL_SerialPortHandle handle, int32_t* status);
+
+/**
+ * Clears the receive buffer of the serial port.
+ *
+ * @param handle the serial port handle
+ */
+void HAL_ClearSerial(HAL_SerialPortHandle handle, int32_t* status);
+
+/**
+ * Closes a serial port.
+ *
+ * @param handle the serial port handle to close
+ */
+void HAL_CloseSerial(HAL_SerialPortHandle handle, int32_t* status);
+#ifdef __cplusplus
+} // extern "C"
+#endif
+/** @} */
diff --git a/hal/src/main/native/include/hal/SimDevice.h b/hal/src/main/native/include/hal/SimDevice.h
new file mode 100644
index 0000000..b05021e
--- /dev/null
+++ b/hal/src/main/native/include/hal/SimDevice.h
@@ -0,0 +1,610 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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 <stdint.h>
+
+#ifdef __cplusplus
+#include <initializer_list>
+
+#include <wpi/ArrayRef.h>
+#endif
+
+#include "hal/Types.h"
+#include "hal/Value.h"
+
+/**
+ * @defgroup hal_simdevice Simulator Device Framework
+ * @ingroup hal_capi
+ * HAL Simulator Device Framework. This enables creating simulation-only
+ * variables for higher level device access. For example, a device such as
+ * a SPI gyro can expose angle and rate variables to enable direct access
+ * from simulation extensions or user test code instead of requiring that
+ * the SPI bit-level protocol be implemented in simulation code.
+ *
+ * @{
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * Creates a simulated device.
+ *
+ * The device name must be unique. 0 is returned if the device name already
+ * exists. If multiple instances of the same device are desired, recommend
+ * appending the instance/unique identifer in brackets to the base name,
+ * e.g. "device[1]".
+ *
+ * 0 is returned if not in simulation.
+ *
+ * @param name device name
+ * @return simulated device handle
+ */
+HAL_SimDeviceHandle HAL_CreateSimDevice(const char* name);
+
+/**
+ * Frees a simulated device.
+ *
+ * This also allows the same device name to be used again.
+ * This also frees all the simulated values created on the device.
+ *
+ * @param handle simulated device handle
+ */
+void HAL_FreeSimDevice(HAL_SimDeviceHandle handle);
+
+/**
+ * Creates a value on a simulated device.
+ *
+ * Returns 0 if not in simulation; this can be used to avoid calls
+ * to Set/Get functions.
+ *
+ * @param device simulated device handle
+ * @param name value name
+ * @param readonly if the value should not be written from simulation side
+ * @param initialValue initial value
+ * @return simulated value handle
+ */
+HAL_SimValueHandle HAL_CreateSimValue(HAL_SimDeviceHandle device,
+ const char* name, HAL_Bool readonly,
+ const struct HAL_Value* initialValue);
+
+#ifdef __cplusplus
+extern "C++" {
+inline HAL_SimValueHandle HAL_CreateSimValue(HAL_SimDeviceHandle device,
+ const char* name,
+ HAL_Bool readonly,
+ const HAL_Value& initialValue) {
+ return HAL_CreateSimValue(device, name, readonly, &initialValue);
+}
+} // extern "C++"
+#endif
+
+/**
+ * Creates a double value on a simulated device.
+ *
+ * Returns 0 if not in simulation; this can be used to avoid calls
+ * to Set/Get functions.
+ *
+ * @param device simulated device handle
+ * @param name value name
+ * @param readonly if the value should not be written from simulation side
+ * @param initialValue initial value
+ * @return simulated value handle
+ */
+inline HAL_SimValueHandle HAL_CreateSimValueDouble(HAL_SimDeviceHandle device,
+ const char* name,
+ HAL_Bool readonly,
+ double initialValue) {
+ struct HAL_Value v = HAL_MakeDouble(initialValue);
+ return HAL_CreateSimValue(device, name, readonly, &v);
+}
+
+/**
+ * Creates an enumerated value on a simulated device.
+ *
+ * Enumerated values are always in the range 0 to numOptions-1.
+ *
+ * Returns 0 if not in simulation; this can be used to avoid calls
+ * to Set/Get functions.
+ *
+ * @param device simulated device handle
+ * @param name value name
+ * @param readonly if the value should not be written from simulation side
+ * @param numOptions number of enumerated value options (length of options)
+ * @param options array of option descriptions
+ * @param initialValue initial value (selection)
+ * @return simulated value handle
+ */
+HAL_SimValueHandle HAL_CreateSimValueEnum(HAL_SimDeviceHandle device,
+ const char* name, HAL_Bool readonly,
+ int32_t numOptions,
+ const char** options,
+ int32_t initialValue);
+
+/**
+ * Creates a boolean value on a simulated device.
+ *
+ * Returns 0 if not in simulation; this can be used to avoid calls
+ * to Set/Get functions.
+ *
+ * @param device simulated device handle
+ * @param name value name
+ * @param readonly if the value should not be written from simulation side
+ * @param initialValue initial value
+ * @return simulated value handle
+ */
+inline HAL_SimValueHandle HAL_CreateSimValueBoolean(HAL_SimDeviceHandle device,
+ const char* name,
+ HAL_Bool readonly,
+ HAL_Bool initialValue) {
+ struct HAL_Value v = HAL_MakeBoolean(initialValue);
+ return HAL_CreateSimValue(device, name, readonly, &v);
+}
+
+/**
+ * Gets a simulated value.
+ *
+ * @param handle simulated value handle
+ * @param value value (output parameter)
+ */
+void HAL_GetSimValue(HAL_SimValueHandle handle, struct HAL_Value* value);
+
+#ifdef __cplusplus
+extern "C++" {
+inline HAL_Value HAL_GetSimValue(HAL_SimValueHandle handle) {
+ HAL_Value v;
+ HAL_GetSimValue(handle, &v);
+ return v;
+}
+} // extern "C++"
+#endif
+
+/**
+ * Gets a simulated value (double).
+ *
+ * @param handle simulated value handle
+ * @return The current value
+ */
+inline double HAL_GetSimValueDouble(HAL_SimValueHandle handle) {
+ struct HAL_Value v;
+ HAL_GetSimValue(handle, &v);
+ return v.type == HAL_DOUBLE ? v.data.v_double : 0.0;
+}
+
+/**
+ * Gets a simulated value (enum).
+ *
+ * @param handle simulated value handle
+ * @return The current value
+ */
+inline int32_t HAL_GetSimValueEnum(HAL_SimValueHandle handle) {
+ struct HAL_Value v;
+ HAL_GetSimValue(handle, &v);
+ return v.type == HAL_ENUM ? v.data.v_enum : 0;
+}
+
+/**
+ * Gets a simulated value (boolean).
+ *
+ * @param handle simulated value handle
+ * @return The current value
+ */
+inline HAL_Bool HAL_GetSimValueBoolean(HAL_SimValueHandle handle) {
+ struct HAL_Value v;
+ HAL_GetSimValue(handle, &v);
+ return v.type == HAL_BOOLEAN ? v.data.v_boolean : 0;
+}
+
+/**
+ * Sets a simulated value.
+ *
+ * @param handle simulated value handle
+ * @param value the value to set
+ */
+void HAL_SetSimValue(HAL_SimValueHandle handle, const struct HAL_Value* value);
+
+#ifdef __cplusplus
+extern "C++" {
+inline void HAL_SetSimValue(HAL_SimValueHandle handle, const HAL_Value& value) {
+ HAL_SetSimValue(handle, &value);
+}
+} // extern "C++"
+#endif
+
+/**
+ * Sets a simulated value (double).
+ *
+ * @param handle simulated value handle
+ * @param value the value to set
+ */
+inline void HAL_SetSimValueDouble(HAL_SimValueHandle handle, double value) {
+ struct HAL_Value v = HAL_MakeDouble(value);
+ HAL_SetSimValue(handle, &v);
+}
+
+/**
+ * Sets a simulated value (enum).
+ *
+ * @param handle simulated value handle
+ * @param value the value to set
+ */
+inline void HAL_SetSimValueEnum(HAL_SimValueHandle handle, int32_t value) {
+ struct HAL_Value v = HAL_MakeEnum(value);
+ HAL_SetSimValue(handle, &v);
+}
+
+/**
+ * Sets a simulated value (boolean).
+ *
+ * @param handle simulated value handle
+ * @param value the value to set
+ */
+inline void HAL_SetSimValueBoolean(HAL_SimValueHandle handle, HAL_Bool value) {
+ struct HAL_Value v = HAL_MakeBoolean(value);
+ HAL_SetSimValue(handle, &v);
+}
+
+/** @} */
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
+
+#ifdef __cplusplus
+namespace hal {
+
+/**
+ * C++ wrapper around a HAL simulator value handle.
+ */
+class SimValue {
+ public:
+ /**
+ * Default constructor that results in an "empty" object that is false in
+ * a boolean context.
+ */
+ SimValue() = default;
+
+ /**
+ * Wraps a simulated value handle as returned by HAL_CreateSimValue().
+ *
+ * @param handle simulated value handle
+ */
+ /*implicit*/ SimValue(HAL_SimValueHandle val) // NOLINT(runtime/explicit)
+ : m_handle(val) {}
+
+ /**
+ * Determine if handle is empty. Should be used to optimize out code paths
+ * that are taken/not taken in simulation.
+ *
+ * @return False if handle is empty, true if handle is valid.
+ */
+ explicit operator bool() const { return m_handle != HAL_kInvalidHandle; }
+
+ /**
+ * Get the internal device handle.
+ *
+ * @return internal handle
+ */
+ operator HAL_SimValueHandle() const { return m_handle; }
+
+ /**
+ * Gets the simulated value.
+ *
+ * @return The current value
+ */
+ HAL_Value GetValue() const { return HAL_GetSimValue(m_handle); }
+
+ /**
+ * Sets the simulated value.
+ *
+ * @param value the value to set
+ */
+ void SetValue(const HAL_Value& value) { HAL_SetSimValue(m_handle, value); }
+
+ protected:
+ HAL_SimValueHandle m_handle = HAL_kInvalidHandle;
+};
+
+/**
+ * C++ wrapper around a HAL simulator double value handle.
+ */
+class SimDouble : public SimValue {
+ public:
+ /**
+ * Default constructor that results in an "empty" object that is false in
+ * a boolean context.
+ */
+ SimDouble() = default;
+
+ /**
+ * Wraps a simulated value handle as returned by HAL_CreateSimValueDouble().
+ *
+ * @param handle simulated value handle
+ */
+ /*implicit*/ SimDouble(HAL_SimValueHandle val) // NOLINT(runtime/explicit)
+ : SimValue(val) {}
+
+ /**
+ * Gets the simulated value.
+ *
+ * @return The current value
+ */
+ double Get() const { return HAL_GetSimValueDouble(m_handle); }
+
+ /**
+ * Sets the simulated value.
+ *
+ * @param value the value to set
+ */
+ void Set(double value) { HAL_SetSimValueDouble(m_handle, value); }
+};
+
+/**
+ * C++ wrapper around a HAL simulator enum value handle.
+ */
+class SimEnum : public SimValue {
+ public:
+ /**
+ * Default constructor that results in an "empty" object that is false in
+ * a boolean context.
+ */
+ SimEnum() = default;
+
+ /**
+ * Wraps a simulated value handle as returned by HAL_CreateSimValueEnum().
+ *
+ * @param handle simulated value handle
+ */
+ /*implicit*/ SimEnum(HAL_SimValueHandle val) // NOLINT(runtime/explicit)
+ : SimValue(val) {}
+
+ /**
+ * Gets the simulated value.
+ *
+ * @return The current value
+ */
+ int32_t Get() const { return HAL_GetSimValueEnum(m_handle); }
+
+ /**
+ * Sets the simulated value.
+ *
+ * @param value the value to set
+ */
+ void Set(int32_t value) { HAL_SetSimValueEnum(m_handle, value); }
+};
+
+/**
+ * C++ wrapper around a HAL simulator boolean value handle.
+ */
+class SimBoolean : public SimValue {
+ public:
+ /**
+ * Default constructor that results in an "empty" object that is false in
+ * a boolean context.
+ */
+ SimBoolean() = default;
+
+ /**
+ * Wraps a simulated value handle as returned by HAL_CreateSimValueBoolean().
+ *
+ * @param handle simulated value handle
+ */
+ /*implicit*/ SimBoolean(HAL_SimValueHandle val) // NOLINT(runtime/explicit)
+ : SimValue(val) {}
+
+ /**
+ * Gets the simulated value.
+ *
+ * @return The current value
+ */
+ bool Get() const { return HAL_GetSimValueBoolean(m_handle); }
+
+ /**
+ * Sets the simulated value.
+ *
+ * @param value the value to set
+ */
+ void Set(bool value) { HAL_SetSimValueBoolean(m_handle, value); }
+};
+
+/**
+ * A move-only C++ wrapper around a HAL simulator device handle.
+ */
+class SimDevice {
+ public:
+ /**
+ * Default constructor that results in an "empty" object that is false in
+ * a boolean context.
+ */
+ SimDevice() = default;
+
+ /**
+ * Creates a simulated device.
+ *
+ * The device name must be unique. Returns null if the device name
+ * already exists. If multiple instances of the same device are desired,
+ * recommend appending the instance/unique identifer in brackets to the base
+ * name, e.g. "device[1]".
+ *
+ * If not in simulation, results in an "empty" object that evaluates to false
+ * in a boolean context.
+ *
+ * @param name device name
+ */
+ explicit SimDevice(const char* name) : m_handle(HAL_CreateSimDevice(name)) {}
+
+ /**
+ * Creates a simulated device.
+ *
+ * The device name must be unique. Returns null if the device name
+ * already exists. This is a convenience method that appends index in
+ * brackets to the device name, e.g. passing index=1 results in "device[1]"
+ * for the device name.
+ *
+ * If not in simulation, results in an "empty" object that evaluates to false
+ * in a boolean context.
+ *
+ * @param name device name
+ * @param index device index number to append to name
+ */
+ SimDevice(const char* name, int index);
+
+ /**
+ * Creates a simulated device.
+ *
+ * The device name must be unique. Returns null if the device name
+ * already exists. This is a convenience method that appends index and
+ * channel in brackets to the device name, e.g. passing index=1 and channel=2
+ * results in "device[1,2]" for the device name.
+ *
+ * If not in simulation, results in an "empty" object that evaluates to false
+ * in a boolean context.
+ *
+ * @param name device name
+ * @param index device index number to append to name
+ * @param channel device channel number to append to name
+ */
+ SimDevice(const char* name, int index, int channel);
+
+ /**
+ * Wraps a simulated device handle as returned by HAL_CreateSimDevice().
+ *
+ * @param handle simulated device handle
+ */
+ /*implicit*/ SimDevice(HAL_SimDeviceHandle val) // NOLINT(runtime/explicit)
+ : m_handle(val) {}
+
+ ~SimDevice() {
+ if (m_handle != HAL_kInvalidHandle) HAL_FreeSimDevice(m_handle);
+ }
+
+ SimDevice(const SimDevice&) = delete;
+ SimDevice& operator=(const SimDevice&) = delete;
+
+ SimDevice(SimDevice&& rhs) : m_handle(rhs.m_handle) {
+ rhs.m_handle = HAL_kInvalidHandle;
+ }
+
+ SimDevice& operator=(SimDevice&& rhs) {
+ m_handle = rhs.m_handle;
+ rhs.m_handle = HAL_kInvalidHandle;
+ return *this;
+ }
+
+ /**
+ * Determine if handle is empty. Should be used to optimize out code paths
+ * that are taken/not taken in simulation.
+ *
+ * @return False if handle is empty, true if handle is valid.
+ */
+ explicit operator bool() const { return m_handle != HAL_kInvalidHandle; }
+
+ /**
+ * Get the internal device handle.
+ *
+ * @return internal handle
+ */
+ operator HAL_SimDeviceHandle() const { return m_handle; }
+
+ /**
+ * Creates a value on the simulated device.
+ *
+ * If not in simulation, results in an "empty" object that evaluates to false
+ * in a boolean context.
+ *
+ * @param name value name
+ * @param readonly if the value should not be written from simulation side
+ * @param initialValue initial value
+ * @return simulated value object
+ */
+ SimValue CreateValue(const char* name, bool readonly,
+ const HAL_Value& initialValue) {
+ return HAL_CreateSimValue(m_handle, name, readonly, &initialValue);
+ }
+
+ /**
+ * Creates a double value on the simulated device.
+ *
+ * If not in simulation, results in an "empty" object that evaluates to false
+ * in a boolean context.
+ *
+ * @param name value name
+ * @param readonly if the value should not be written from simulation side
+ * @param initialValue initial value
+ * @return simulated double value object
+ */
+ SimDouble CreateDouble(const char* name, bool readonly, double initialValue) {
+ return HAL_CreateSimValueDouble(m_handle, name, readonly, initialValue);
+ }
+
+ /**
+ * Creates an enumerated value on the simulated device.
+ *
+ * Enumerated values are always in the range 0 to numOptions-1.
+ *
+ * If not in simulation, results in an "empty" object that evaluates to false
+ * in a boolean context.
+ *
+ * @param name value name
+ * @param readonly if the value should not be written from simulation side
+ * @param options array of option descriptions
+ * @param initialValue initial value (selection)
+ * @return simulated enum value object
+ */
+ SimEnum CreateEnum(const char* name, bool readonly,
+ std::initializer_list<const char*> options,
+ int32_t initialValue) {
+ return HAL_CreateSimValueEnum(m_handle, name, readonly, options.size(),
+ const_cast<const char**>(options.begin()),
+ initialValue);
+ }
+
+ /**
+ * Creates an enumerated value on the simulated device.
+ *
+ * Enumerated values are always in the range 0 to numOptions-1.
+ *
+ * If not in simulation, results in an "empty" object that evaluates to false
+ * in a boolean context.
+ *
+ * @param name value name
+ * @param readonly if the value should not be written from simulation side
+ * @param options array of option descriptions
+ * @param initialValue initial value (selection)
+ * @return simulated enum value object
+ */
+ SimEnum CreateEnum(const char* name, bool readonly,
+ wpi::ArrayRef<const char*> options, int32_t initialValue) {
+ return HAL_CreateSimValueEnum(m_handle, name, readonly, options.size(),
+ const_cast<const char**>(options.data()),
+ initialValue);
+ }
+
+ /**
+ * Creates a boolean value on the simulated device.
+ *
+ * If not in simulation, results in an "empty" object that evaluates to false
+ * in a boolean context.
+ *
+ * @param name value name
+ * @param readonly if the value should not be written from simulation side
+ * @param initialValue initial value
+ * @return simulated boolean value object
+ */
+ SimBoolean CreateBoolean(const char* name, bool readonly, bool initialValue) {
+ return HAL_CreateSimValueBoolean(m_handle, name, readonly, initialValue);
+ }
+
+ protected:
+ HAL_SimDeviceHandle m_handle = HAL_kInvalidHandle;
+};
+
+} // namespace hal
+#endif // __cplusplus
diff --git a/hal/src/main/native/include/hal/Solenoid.h b/hal/src/main/native/include/hal/Solenoid.h
new file mode 100644
index 0000000..53257b2
--- /dev/null
+++ b/hal/src/main/native/include/hal/Solenoid.h
@@ -0,0 +1,141 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 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 <stdint.h>
+
+#include "hal/Types.h"
+
+/**
+ * @defgroup hal_solenoid Solenoid Output Functions
+ * @ingroup hal_capi
+ * @{
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * Initializes a solenoid port.
+ *
+ * @param portHandle the port handle of the module and channel to initialize
+ * @return the created solenoid handle
+ */
+HAL_SolenoidHandle HAL_InitializeSolenoidPort(HAL_PortHandle portHandle,
+ int32_t* status);
+
+/**
+ * Frees a solenoid port.
+ *
+ * @param solenoidPortHandle the solenoid handle
+ */
+void HAL_FreeSolenoidPort(HAL_SolenoidHandle solenoidPortHandle);
+
+/**
+ * Checks if a solenoid module is in the valid range.
+ *
+ * @param module the module number to check
+ * @return true if the module number is valid, otherwise false
+ */
+HAL_Bool HAL_CheckSolenoidModule(int32_t module);
+
+/**
+ * Checks if a solenoid channel is in the valid range.
+ *
+ * @param channel the channel number to check
+ * @return true if the channel number is valid, otherwise false
+ */
+HAL_Bool HAL_CheckSolenoidChannel(int32_t channel);
+
+/**
+ * Gets the current solenoid output value.
+ *
+ * @param solenoidPortHandle the solenoid handle
+ * @return true if the solenoid is on, otherwise false
+ */
+HAL_Bool HAL_GetSolenoid(HAL_SolenoidHandle solenoidPortHandle,
+ int32_t* status);
+
+/**
+ * Gets the status of all solenoids on a specific module.
+ *
+ * @param module the module to check
+ * @return bitmask of the channels, 1 for on 0 for off
+ */
+int32_t HAL_GetAllSolenoids(int32_t module, int32_t* status);
+
+/**
+ * Sets a solenoid output value.
+ *
+ * @param solenoidPortHandle the solenoid handle
+ * @param value true for on, false for off
+ */
+void HAL_SetSolenoid(HAL_SolenoidHandle solenoidPortHandle, HAL_Bool value,
+ int32_t* status);
+
+/**
+ * Sets all channels on a specific module.
+ *
+ * @param module the module to set the channels on
+ * @param state bitmask of the channels to set, 1 for on 0 for off
+ */
+void HAL_SetAllSolenoids(int32_t module, int32_t state, int32_t* status);
+
+/**
+ * Gets the channels blacklisted from being enabled on a module.
+ *
+ * @param module the module to check
+ * @retur bitmask of the blacklisted channels, 1 for true 0 for false
+ */
+int32_t HAL_GetPCMSolenoidBlackList(int32_t module, int32_t* status);
+
+/**
+ * Gets if a specific module has an over or under voltage sticky fault.
+ *
+ * @param module the module to check
+ * @return true if a stick fault is set, otherwise false
+ */
+HAL_Bool HAL_GetPCMSolenoidVoltageStickyFault(int32_t module, int32_t* status);
+
+/**
+ * Gets if a specific module has an over or under voltage fault.
+ *
+ * @param module the module to check
+ * @return true if faulted, otherwise false
+ */
+HAL_Bool HAL_GetPCMSolenoidVoltageFault(int32_t module, int32_t* status);
+
+/**
+ * Clears all faults on a module.
+ *
+ * @param module the module to clear
+ */
+void HAL_ClearAllPCMStickyFaults(int32_t module, int32_t* status);
+
+/**
+ * Sets the one shot duration on a solenoid channel.
+ *
+ * @param solenoidPortHandle the solenoid handle
+ * @param durMS the one shot duration in ms
+ */
+void HAL_SetOneShotDuration(HAL_SolenoidHandle solenoidPortHandle,
+ int32_t durMS, int32_t* status);
+
+/**
+ * Fires a single pulse on a solenoid channel.
+ *
+ * The pulse is the duration set by HAL_SetOneShotDuration().
+ *
+ * @param solenoidPortHandle the solenoid handle
+ */
+void HAL_FireOneShot(HAL_SolenoidHandle solenoidPortHandle, int32_t* status);
+#ifdef __cplusplus
+} // extern "C"
+#endif
+/** @} */
diff --git a/hal/src/main/native/include/hal/Threads.h b/hal/src/main/native/include/hal/Threads.h
new file mode 100644
index 0000000..aea4399
--- /dev/null
+++ b/hal/src/main/native/include/hal/Threads.h
@@ -0,0 +1,78 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 "hal/Types.h"
+
+/**
+ * @defgroup hal_threads Threads Functions
+ * @ingroup hal_capi
+ * @{
+ */
+
+typedef const void* NativeThreadHandle;
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * Gets the thread priority for the specified thread.
+ *
+ * @param handle Native handle pointer to the thread to get the priority for
+ * @param isRealTime Set to true if thread is realtime, otherwise false
+ * @param status Error status variable. 0 on success
+ * @return The current thread priority. Scaled 1-99, with 1 being
+ * highest.
+ */
+int32_t HAL_GetThreadPriority(NativeThreadHandle handle, HAL_Bool* isRealTime,
+ int32_t* status);
+
+/**
+ * Gets the thread priority for the current thread.
+ *
+ * @param handle Native handle pointer to the thread to get the priority for
+ * @param isRealTime Set to true if thread is realtime, otherwise false
+ * @param status Error status variable. 0 on success
+ * @return The current thread priority. Scaled 1-99, with 1 being
+ * highest.
+ */
+int32_t HAL_GetCurrentThreadPriority(HAL_Bool* isRealTime, int32_t* status);
+
+/**
+ * Sets the thread priority for the specified thread.
+ *
+ * @param thread Reference to the thread to set the priority of
+ * @param realTime Set to true to set a realtime priority, false for standard
+ * priority
+ * @param priority Priority to set the thread to. Scaled 1-99, with 1 being
+ * highest
+ * @param status Error status variable. 0 on success
+ * @return The success state of setting the priority
+ */
+HAL_Bool HAL_SetThreadPriority(NativeThreadHandle handle, HAL_Bool realTime,
+ int32_t priority, int32_t* status);
+
+/**
+ * Sets the thread priority for the current thread.
+ *
+ * @param thread Reference to the thread to set the priority of
+ * @param realTime Set to true to set a realtime priority, false for standard
+ * priority
+ * @param priority Priority to set the thread to. Scaled 1-99, with 1 being
+ * highest
+ * @param status Error status variable. 0 on success
+ * @return The success state of setting the priority
+ */
+HAL_Bool HAL_SetCurrentThreadPriority(HAL_Bool realTime, int32_t priority,
+ int32_t* status);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
+/** @} */
diff --git a/hal/src/main/native/include/hal/Types.h b/hal/src/main/native/include/hal/Types.h
new file mode 100644
index 0000000..1ebbe2d
--- /dev/null
+++ b/hal/src/main/native/include/hal/Types.h
@@ -0,0 +1,110 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 <stdint.h>
+
+/**
+ * @defgroup hal_types Type Definitions
+ * @ingroup hal_capi
+ * @{
+ */
+
+#define HAL_kInvalidHandle 0
+
+typedef int32_t HAL_Handle;
+
+typedef HAL_Handle HAL_PortHandle;
+
+typedef HAL_Handle HAL_AnalogInputHandle;
+
+typedef HAL_Handle HAL_AnalogOutputHandle;
+
+typedef HAL_Handle HAL_AnalogTriggerHandle;
+
+typedef HAL_Handle HAL_CompressorHandle;
+
+typedef HAL_Handle HAL_CounterHandle;
+
+typedef HAL_Handle HAL_DigitalHandle;
+
+typedef HAL_Handle HAL_DigitalPWMHandle;
+
+typedef HAL_Handle HAL_EncoderHandle;
+
+typedef HAL_Handle HAL_FPGAEncoderHandle;
+
+typedef HAL_Handle HAL_GyroHandle;
+
+typedef HAL_Handle HAL_InterruptHandle;
+
+typedef HAL_Handle HAL_NotifierHandle;
+
+typedef HAL_Handle HAL_RelayHandle;
+
+typedef HAL_Handle HAL_SolenoidHandle;
+
+typedef HAL_Handle HAL_SerialPortHandle;
+
+typedef HAL_Handle HAL_CANHandle;
+
+typedef HAL_Handle HAL_SimDeviceHandle;
+
+typedef HAL_Handle HAL_SimValueHandle;
+
+typedef HAL_Handle HAL_DMAHandle;
+
+typedef HAL_Handle HAL_DutyCycleHandle;
+
+typedef HAL_Handle HAL_AddressableLEDHandle;
+
+typedef HAL_CANHandle HAL_PDPHandle;
+
+typedef int32_t HAL_Bool;
+
+#ifdef __cplusplus
+#define HAL_ENUM(name) enum name : int32_t
+#else
+#define HAL_ENUM(name) \
+ typedef int32_t name; \
+ enum name
+#endif
+
+#ifdef __cplusplus
+namespace hal {
+
+/**
+ * A move-only C++ wrapper around a HAL handle.
+ * Does not ensure destruction.
+ */
+template <typename CType, int32_t CInvalid = HAL_kInvalidHandle>
+class Handle {
+ public:
+ Handle() = default;
+ /*implicit*/ Handle(CType val) : m_handle(val) {} // NOLINT(runtime/explicit)
+
+ Handle(const Handle&) = delete;
+ Handle& operator=(const Handle&) = delete;
+
+ Handle(Handle&& rhs) : m_handle(rhs.m_handle) { rhs.m_handle = CInvalid; }
+
+ Handle& operator=(Handle&& rhs) {
+ m_handle = rhs.m_handle;
+ rhs.m_handle = CInvalid;
+ return *this;
+ }
+
+ operator CType() const { return m_handle; }
+
+ private:
+ CType m_handle = CInvalid;
+};
+
+} // namespace hal
+#endif
+/** @} */
diff --git a/hal/src/main/native/include/hal/Value.h b/hal/src/main/native/include/hal/Value.h
new file mode 100644
index 0000000..578d989
--- /dev/null
+++ b/hal/src/main/native/include/hal/Value.h
@@ -0,0 +1,75 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "hal/Types.h"
+
+/** HAL data types. */
+enum HAL_Type {
+ HAL_UNASSIGNED = 0,
+ HAL_BOOLEAN = 0x01,
+ HAL_DOUBLE = 0x02,
+ HAL_ENUM = 0x04,
+ HAL_INT = 0x08,
+ HAL_LONG = 0x10,
+};
+
+/** HAL Entry Value. Note this is a typed union. */
+struct HAL_Value {
+ union {
+ HAL_Bool v_boolean;
+ int32_t v_enum;
+ int32_t v_int;
+ int64_t v_long;
+ double v_double;
+ } data;
+ enum HAL_Type type;
+};
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+inline struct HAL_Value HAL_MakeBoolean(HAL_Bool v) {
+ struct HAL_Value value;
+ value.type = HAL_BOOLEAN;
+ value.data.v_boolean = v;
+ return value;
+}
+
+inline struct HAL_Value HAL_MakeEnum(int v) {
+ struct HAL_Value value;
+ value.type = HAL_ENUM;
+ value.data.v_enum = v;
+ return value;
+}
+
+inline struct HAL_Value HAL_MakeInt(int v) {
+ struct HAL_Value value;
+ value.type = HAL_INT;
+ value.data.v_int = v;
+ return value;
+}
+
+inline struct HAL_Value HAL_MakeLong(int64_t v) {
+ struct HAL_Value value;
+ value.type = HAL_LONG;
+ value.data.v_long = v;
+ return value;
+}
+
+inline struct HAL_Value HAL_MakeDouble(double v) {
+ struct HAL_Value value;
+ value.type = HAL_DOUBLE;
+ value.data.v_double = v;
+ return value;
+}
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
diff --git a/hal/src/main/native/include/hal/cpp/SerialHelper.h b/hal/src/main/native/include/hal/cpp/SerialHelper.h
new file mode 100644
index 0000000..9f4d6a0
--- /dev/null
+++ b/hal/src/main/native/include/hal/cpp/SerialHelper.h
@@ -0,0 +1,83 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 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 <stdint.h>
+
+#include <string>
+#include <vector>
+
+#include <wpi/SmallString.h>
+#include <wpi/SmallVector.h>
+#include <wpi/mutex.h>
+
+#include "hal/SerialPort.h"
+
+namespace hal {
+/**
+ * A class for deterministically getting information about Serial Ports.
+ */
+class SerialHelper {
+ public:
+ SerialHelper();
+
+ /**
+ * Get the VISA name of a serial port.
+ *
+ * @param port the serial port index
+ * @param status status check
+ * @return the VISA name
+ */
+ std::string GetVISASerialPortName(HAL_SerialPort port, int32_t* status);
+
+ /**
+ * Get the OS name of a serial port.
+ *
+ * @param port the serial port index
+ * @param status status check
+ * @return the OS name
+ */
+ std::string GetOSSerialPortName(HAL_SerialPort port, int32_t* status);
+
+ /**
+ * Get a vector of all serial port VISA names.
+ *
+ * @param status status check
+ * @return vector of serial port VISA names
+ */
+ std::vector<std::string> GetVISASerialPortList(int32_t* status);
+
+ /**
+ * Get a vector of all serial port OS names.
+ *
+ * @param status status check
+ * @return vector of serial port OS names
+ */
+ std::vector<std::string> GetOSSerialPortList(int32_t* status);
+
+ private:
+ void SortHubPathVector();
+ void CoiteratedSort(wpi::SmallVectorImpl<wpi::SmallString<16>>& vec);
+ void QueryHubPaths(int32_t* status);
+
+ int32_t GetIndexForPort(HAL_SerialPort port, int32_t* status);
+
+ // Vectors to hold data before sorting.
+ // Note we will most likely have at max 2 instances, and the longest string
+ // is around 12, so these should never touch the heap;
+ wpi::SmallVector<wpi::SmallString<16>, 4> m_visaResource;
+ wpi::SmallVector<wpi::SmallString<16>, 4> m_osResource;
+ wpi::SmallVector<wpi::SmallString<16>, 4> m_unsortedHubPath;
+ wpi::SmallVector<wpi::SmallString<16>, 4> m_sortedHubPath;
+
+ int32_t m_resourceHandle;
+
+ static wpi::mutex m_nameMutex;
+ static std::string m_usbNames[2];
+};
+} // namespace hal
diff --git a/hal/src/main/native/include/hal/cpp/UnsafeDIO.h b/hal/src/main/native/include/hal/cpp/UnsafeDIO.h
new file mode 100644
index 0000000..dad5eb7
--- /dev/null
+++ b/hal/src/main/native/include/hal/cpp/UnsafeDIO.h
@@ -0,0 +1,95 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 <wpi/mutex.h>
+
+#include "hal/ChipObject.h"
+#include "hal/Types.h"
+
+namespace hal {
+
+/**
+ * Proxy class for directly manipulating the DIO pins.
+ *
+ * This class is not copyable or movable, and should never be used
+ * outside of the UnsafeManipulateDIO callback.
+ */
+struct DIOSetProxy {
+ DIOSetProxy(const DIOSetProxy&) = delete;
+ DIOSetProxy(DIOSetProxy&&) = delete;
+ DIOSetProxy& operator=(const DIOSetProxy&) = delete;
+ DIOSetProxy& operator=(DIOSetProxy&&) = delete;
+
+ void SetOutputMode(int32_t* status) {
+ m_dio->writeOutputEnable(m_setOutputDirReg, status);
+ }
+
+ void SetInputMode(int32_t* status) {
+ m_dio->writeOutputEnable(m_unsetOutputDirReg, status);
+ }
+
+ void SetOutputTrue(int32_t* status) {
+ m_dio->writeDO(m_setOutputStateReg, status);
+ }
+
+ void SetOutputFalse(int32_t* status) {
+ m_dio->writeDO(m_unsetOutputStateReg, status);
+ }
+
+ tDIO::tOutputEnable m_setOutputDirReg;
+ tDIO::tOutputEnable m_unsetOutputDirReg;
+ tDIO::tDO m_setOutputStateReg;
+ tDIO::tDO m_unsetOutputStateReg;
+ tDIO* m_dio;
+};
+namespace detail {
+wpi::mutex& UnsafeGetDIOMutex();
+tDIO* UnsafeGetDigialSystem();
+int32_t ComputeDigitalMask(HAL_DigitalHandle handle, int32_t* status);
+} // namespace detail
+
+/**
+ * Unsafe digital output set function
+ * This function can be used to perform fast and determinstically set digital
+ * outputs. This function holds the DIO lock, so calling anyting other then
+ * functions on the Proxy object passed as a parameter can deadlock your
+ * program.
+ *
+ * @param handle the HAL digital handle of the pin to toggle.
+ * @param status status check
+ * @param func A functor taking a ref to a DIOSetProxy object.
+ */
+template <typename Functor>
+void UnsafeManipulateDIO(HAL_DigitalHandle handle, int32_t* status,
+ Functor func) {
+ auto port = digitalChannelHandles->Get(handle, HAL_HandleEnum::DIO);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ wpi::mutex& dioMutex = detail::UnsafeGetDIOMutex();
+ tDIO* dSys = detail::UnsafeGetDigialSystem();
+ auto mask = detail::ComputeDigitalMask(handle, status);
+ if (status != 0) return;
+ std::scoped_lock lock(dioMutex);
+
+ tDIO::tOutputEnable enableOE = dSys->readOutputEnable(status);
+ enableOE.value |= mask;
+ auto disableOE = enableOE;
+ disableOE.value &= ~mask;
+ tDIO::tDO enableDO = dSys->readDO(status);
+ enableDO.value |= mask;
+ auto disableDO = enableDO;
+ disableDO.value &= ~mask;
+
+ DIOSetProxy dioData{enableOE, disableOE, enableDO, disableDO, dSys};
+ func(dioData);
+}
+
+} // namespace hal
diff --git a/hal/src/main/native/include/hal/cpp/fpga_clock.h b/hal/src/main/native/include/hal/cpp/fpga_clock.h
new file mode 100644
index 0000000..94031b1
--- /dev/null
+++ b/hal/src/main/native/include/hal/cpp/fpga_clock.h
@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 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 <chrono>
+#include <limits>
+
+/** WPILib Hardware Abstraction Layer (HAL) namespace */
+namespace hal {
+
+/**
+ * A std::chrono compatible wrapper around the FPGA Timer.
+ */
+class fpga_clock {
+ public:
+ using rep = std::chrono::microseconds::rep;
+ using period = std::chrono::microseconds::period;
+ using duration = std::chrono::microseconds;
+ using time_point = std::chrono::time_point<fpga_clock>;
+
+ static fpga_clock::time_point now() noexcept;
+ static constexpr bool is_steady = true;
+
+ static fpga_clock::time_point epoch() noexcept { return time_point(zero()); }
+
+ static fpga_clock::duration zero() noexcept { return duration(0); }
+
+ static const time_point min_time;
+};
+} // namespace hal
diff --git a/hal/src/main/native/include/hal/handles/DigitalHandleResource.h b/hal/src/main/native/include/hal/handles/DigitalHandleResource.h
new file mode 100644
index 0000000..dcd4b97
--- /dev/null
+++ b/hal/src/main/native/include/hal/handles/DigitalHandleResource.h
@@ -0,0 +1,105 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 <stdint.h>
+
+#include <array>
+#include <memory>
+
+#include <wpi/mutex.h>
+
+#include "hal/Errors.h"
+#include "hal/Types.h"
+#include "hal/handles/HandlesInternal.h"
+
+namespace hal {
+
+/**
+ * The DigitalHandleResource class is a way to track handles. This version
+ * allows a limited number of handles that are allocated by index.
+ * The enum value is seperate, as 2 enum values are allowed per handle
+ * Because they are allocated by index, each individual index holds its own
+ * mutex, which reduces contention heavily.]
+ *
+ * @tparam THandle The Handle Type (Must be typedefed from HAL_Handle)
+ * @tparam TStruct The struct type held by this resource
+ * @tparam size The number of resources allowed to be allocated
+ *
+ */
+template <typename THandle, typename TStruct, int16_t size>
+class DigitalHandleResource : public HandleBase {
+ friend class DigitalHandleResourceTest;
+
+ public:
+ DigitalHandleResource() = default;
+ DigitalHandleResource(const DigitalHandleResource&) = delete;
+ DigitalHandleResource& operator=(const DigitalHandleResource&) = delete;
+
+ THandle Allocate(int16_t index, HAL_HandleEnum enumValue, int32_t* status);
+ std::shared_ptr<TStruct> Get(THandle handle, HAL_HandleEnum enumValue);
+ void Free(THandle handle, HAL_HandleEnum enumValue);
+ void ResetHandles() override;
+
+ private:
+ std::array<std::shared_ptr<TStruct>, size> m_structures;
+ std::array<wpi::mutex, size> m_handleMutexes;
+};
+
+template <typename THandle, typename TStruct, int16_t size>
+THandle DigitalHandleResource<THandle, TStruct, size>::Allocate(
+ int16_t index, HAL_HandleEnum enumValue, int32_t* status) {
+ // don't aquire the lock if we can fail early.
+ if (index < 0 || index >= size) {
+ *status = RESOURCE_OUT_OF_RANGE;
+ return HAL_kInvalidHandle;
+ }
+ std::scoped_lock lock(m_handleMutexes[index]);
+ // check for allocation, otherwise allocate and return a valid handle
+ if (m_structures[index] != nullptr) {
+ *status = RESOURCE_IS_ALLOCATED;
+ return HAL_kInvalidHandle;
+ }
+ m_structures[index] = std::make_shared<TStruct>();
+ return static_cast<THandle>(hal::createHandle(index, enumValue, m_version));
+}
+
+template <typename THandle, typename TStruct, int16_t size>
+std::shared_ptr<TStruct> DigitalHandleResource<THandle, TStruct, size>::Get(
+ THandle handle, HAL_HandleEnum enumValue) {
+ // get handle index, and fail early if index out of range or wrong handle
+ int16_t index = getHandleTypedIndex(handle, enumValue, m_version);
+ if (index < 0 || index >= size) {
+ return nullptr;
+ }
+ std::scoped_lock lock(m_handleMutexes[index]);
+ // return structure. Null will propogate correctly, so no need to manually
+ // check.
+ return m_structures[index];
+}
+
+template <typename THandle, typename TStruct, int16_t size>
+void DigitalHandleResource<THandle, TStruct, size>::Free(
+ THandle handle, HAL_HandleEnum enumValue) {
+ // get handle index, and fail early if index out of range or wrong handle
+ int16_t index = getHandleTypedIndex(handle, enumValue, m_version);
+ if (index < 0 || index >= size) return;
+ // lock and deallocated handle
+ std::scoped_lock lock(m_handleMutexes[index]);
+ m_structures[index].reset();
+}
+
+template <typename THandle, typename TStruct, int16_t size>
+void DigitalHandleResource<THandle, TStruct, size>::ResetHandles() {
+ for (int i = 0; i < size; i++) {
+ std::scoped_lock lock(m_handleMutexes[i]);
+ m_structures[i].reset();
+ }
+ HandleBase::ResetHandles();
+}
+} // namespace hal
diff --git a/hal/src/main/native/include/hal/handles/HandlesInternal.h b/hal/src/main/native/include/hal/handles/HandlesInternal.h
new file mode 100644
index 0000000..511433e
--- /dev/null
+++ b/hal/src/main/native/include/hal/handles/HandlesInternal.h
@@ -0,0 +1,215 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 <stdint.h>
+
+#include "hal/Types.h"
+
+/* General Handle Data Layout
+ * Bits 0-15: Handle Index
+ * Bits 16-23: 8 bit rolling reset detection
+ * Bits 24-30: Handle Type
+ * Bit 31: 1 if handle error, 0 if no error
+ *
+ * Other specialized handles will use different formats, however Bits 24-31 are
+ * always reserved for type and error handling.
+ */
+
+namespace hal {
+
+/**
+ * Base for all HAL Handles.
+ */
+class HandleBase {
+ public:
+ HandleBase();
+ ~HandleBase();
+ HandleBase(const HandleBase&) = delete;
+ HandleBase& operator=(const HandleBase&) = delete;
+ virtual void ResetHandles();
+ static void ResetGlobalHandles();
+
+ protected:
+ int16_t m_version;
+};
+
+constexpr int16_t InvalidHandleIndex = -1;
+
+/**
+ * Enum of HAL handle types. Vendors/Teams should use Vendor (17).
+ */
+enum class HAL_HandleEnum {
+ Undefined = 0,
+ DIO = 1,
+ Port = 2,
+ Notifier = 3,
+ Interrupt = 4,
+ AnalogOutput = 5,
+ AnalogInput = 6,
+ AnalogTrigger = 7,
+ Relay = 8,
+ PWM = 9,
+ DigitalPWM = 10,
+ Counter = 11,
+ FPGAEncoder = 12,
+ Encoder = 13,
+ Compressor = 14,
+ Solenoid = 15,
+ AnalogGyro = 16,
+ Vendor = 17,
+ SimulationJni = 18,
+ CAN = 19,
+ SerialPort = 20,
+ DutyCycle = 21,
+ DMA = 22,
+ AddressableLED = 23,
+};
+
+/**
+ * Get the handle index from a handle.
+ *
+ * @param handle the handle
+ * @return the index
+ */
+static inline int16_t getHandleIndex(HAL_Handle handle) {
+ // mask and return last 16 bits
+ return static_cast<int16_t>(handle & 0xffff);
+}
+
+/**
+ * Get the handle type from a handle.
+ *
+ * @param handle the handle
+ * @return the type
+ */
+static inline HAL_HandleEnum getHandleType(HAL_Handle handle) {
+ // mask first 8 bits and cast to enum
+ return static_cast<HAL_HandleEnum>((handle >> 24) & 0xff);
+}
+
+/**
+ * Get if the handle is a specific type.
+ *
+ * @param handle the handle
+ * @param handleType the type to check
+ * @return true if the type is correct, otherwise false
+ */
+static inline bool isHandleType(HAL_Handle handle, HAL_HandleEnum handleType) {
+ return handleType == getHandleType(handle);
+}
+
+/**
+ * Get if the version of the handle is correct.
+ *
+ * Do not use on the roboRIO, used specifically for the sim to handle resets.
+ *
+ * @param handle the handle
+ * @param version the handle version to check
+ * @return true if the handle is the right version, otherwise false
+ */
+static inline bool isHandleCorrectVersion(HAL_Handle handle, int16_t version) {
+ return (((handle & 0xFF0000) >> 16) & version) == version;
+}
+
+/**
+ * Get if the handle is a correct type and version.
+ *
+ * Note the version is not checked on the roboRIO.
+ *
+ * @param handle the handle
+ * @param handleType the type to check
+ * @param version the handle version to check
+ * @return true if the handle is proper version and type, otherwise
+ * false.
+ */
+static inline int16_t getHandleTypedIndex(HAL_Handle handle,
+ HAL_HandleEnum enumType,
+ int16_t version) {
+ if (!isHandleType(handle, enumType)) return InvalidHandleIndex;
+#if !defined(__FRC_ROBORIO__)
+ if (!isHandleCorrectVersion(handle, version)) return InvalidHandleIndex;
+#endif
+ return getHandleIndex(handle);
+}
+
+/* specialized functions for Port handle
+ * Port Handle Data Layout
+ * Bits 0-7: Channel Number
+ * Bits 8-15: Module Number
+ * Bits 16-23: Unused
+ * Bits 24-30: Handle Type
+ * Bit 31: 1 if handle error, 0 if no error
+ */
+
+// using a 16 bit value so we can store 0-255 and still report error
+/**
+ * Gets the port channel of a port handle.
+ *
+ * @param handle the port handle
+ * @return the port channel
+ */
+static inline int16_t getPortHandleChannel(HAL_PortHandle handle) {
+ if (!isHandleType(handle, HAL_HandleEnum::Port)) return InvalidHandleIndex;
+ return static_cast<uint8_t>(handle & 0xff);
+}
+
+// using a 16 bit value so we can store 0-255 and still report error
+/**
+ * Gets the port module of a port handle.
+ *
+ * @param handle the port handle
+ * @return the port module
+ */
+static inline int16_t getPortHandleModule(HAL_PortHandle handle) {
+ if (!isHandleType(handle, HAL_HandleEnum::Port)) return InvalidHandleIndex;
+ return static_cast<uint8_t>((handle >> 8) & 0xff);
+}
+
+// using a 16 bit value so we can store 0-255 and still report error
+/**
+ * Gets the SPI channel of a port handle.
+ *
+ * @param handle the port handle
+ * @return the port SPI channel
+ */
+static inline int16_t getPortHandleSPIEnable(HAL_PortHandle handle) {
+ if (!isHandleType(handle, HAL_HandleEnum::Port)) return InvalidHandleIndex;
+ return static_cast<uint8_t>((handle >> 16) & 0xff);
+}
+
+/**
+ * Create a port handle.
+ *
+ * @param channel the channel
+ * @param module the module
+ * @return port handle for the module and channel
+ */
+HAL_PortHandle createPortHandle(uint8_t channel, uint8_t module);
+
+/**
+ * Create a port handle for SPI.
+ *
+ * @param channel the SPI channel
+ * @return port handle for the channel
+ */
+HAL_PortHandle createPortHandleForSPI(uint8_t channel);
+
+/**
+ * Create a handle for a specific index, type and version.
+ *
+ * Note the version is not checked on the roboRIO.
+ *
+ * @param index the index
+ * @param handleType the handle type
+ * @param version the handle version
+ * @return the created handle
+ */
+HAL_Handle createHandle(int16_t index, HAL_HandleEnum handleType,
+ int16_t version);
+} // namespace hal
diff --git a/hal/src/main/native/include/hal/handles/IndexedClassedHandleResource.h b/hal/src/main/native/include/hal/handles/IndexedClassedHandleResource.h
new file mode 100644
index 0000000..2725573
--- /dev/null
+++ b/hal/src/main/native/include/hal/handles/IndexedClassedHandleResource.h
@@ -0,0 +1,117 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 <stdint.h>
+
+#include <array>
+#include <memory>
+#include <vector>
+
+#include <wpi/mutex.h>
+
+#include "hal/Errors.h"
+#include "hal/Types.h"
+#include "hal/handles/HandlesInternal.h"
+
+namespace hal {
+
+/**
+ * The IndexedClassedHandleResource class is a way to track handles. This
+ * version
+ * allows a limited number of handles that are allocated by index.
+ * Because they are allocated by index, each individual index holds its own
+ * mutex, which reduces contention heavily.]
+ *
+ * @tparam THandle The Handle Type (Must be typedefed from HAL_Handle)
+ * @tparam TStruct The struct type held by this resource
+ * @tparam size The number of resources allowed to be allocated
+ * @tparam enumValue The type value stored in the handle
+ *
+ */
+template <typename THandle, typename TStruct, int16_t size,
+ HAL_HandleEnum enumValue>
+class IndexedClassedHandleResource : public HandleBase {
+ friend class IndexedClassedHandleResourceTest;
+
+ public:
+ IndexedClassedHandleResource() = default;
+ IndexedClassedHandleResource(const IndexedClassedHandleResource&) = delete;
+ IndexedClassedHandleResource& operator=(const IndexedClassedHandleResource&) =
+ delete;
+
+ THandle Allocate(int16_t index, std::shared_ptr<TStruct> toSet,
+ int32_t* status);
+ std::shared_ptr<TStruct> Get(THandle handle);
+ void Free(THandle handle);
+ void ResetHandles();
+
+ private:
+ std::array<std::shared_ptr<TStruct>, size> m_structures;
+ std::array<wpi::mutex, size> m_handleMutexes;
+};
+
+template <typename THandle, typename TStruct, int16_t size,
+ HAL_HandleEnum enumValue>
+THandle
+IndexedClassedHandleResource<THandle, TStruct, size, enumValue>::Allocate(
+ int16_t index, std::shared_ptr<TStruct> toSet, int32_t* status) {
+ // don't aquire the lock if we can fail early.
+ if (index < 0 || index >= size) {
+ *status = RESOURCE_OUT_OF_RANGE;
+ return HAL_kInvalidHandle;
+ }
+ std::scoped_lock lock(m_handleMutexes[index]);
+ // check for allocation, otherwise allocate and return a valid handle
+ if (m_structures[index] != nullptr) {
+ *status = RESOURCE_IS_ALLOCATED;
+ return HAL_kInvalidHandle;
+ }
+ m_structures[index] = toSet;
+ return static_cast<THandle>(hal::createHandle(index, enumValue, m_version));
+}
+
+template <typename THandle, typename TStruct, int16_t size,
+ HAL_HandleEnum enumValue>
+std::shared_ptr<TStruct>
+IndexedClassedHandleResource<THandle, TStruct, size, enumValue>::Get(
+ THandle handle) {
+ // get handle index, and fail early if index out of range or wrong handle
+ int16_t index = getHandleTypedIndex(handle, enumValue, m_version);
+ if (index < 0 || index >= size) {
+ return nullptr;
+ }
+ std::scoped_lock lock(m_handleMutexes[index]);
+ // return structure. Null will propogate correctly, so no need to manually
+ // check.
+ return m_structures[index];
+}
+
+template <typename THandle, typename TStruct, int16_t size,
+ HAL_HandleEnum enumValue>
+void IndexedClassedHandleResource<THandle, TStruct, size, enumValue>::Free(
+ THandle handle) {
+ // get handle index, and fail early if index out of range or wrong handle
+ int16_t index = getHandleTypedIndex(handle, enumValue, m_version);
+ if (index < 0 || index >= size) return;
+ // lock and deallocated handle
+ std::scoped_lock lock(m_handleMutexes[index]);
+ m_structures[index].reset();
+}
+
+template <typename THandle, typename TStruct, int16_t size,
+ HAL_HandleEnum enumValue>
+void IndexedClassedHandleResource<THandle, TStruct, size,
+ enumValue>::ResetHandles() {
+ for (int i = 0; i < size; i++) {
+ std::scoped_lock lock(m_handleMutexes[i]);
+ m_structures[i].reset();
+ }
+ HandleBase::ResetHandles();
+}
+} // namespace hal
diff --git a/hal/src/main/native/include/hal/handles/IndexedHandleResource.h b/hal/src/main/native/include/hal/handles/IndexedHandleResource.h
new file mode 100644
index 0000000..2bca4ce
--- /dev/null
+++ b/hal/src/main/native/include/hal/handles/IndexedHandleResource.h
@@ -0,0 +1,110 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 <stdint.h>
+
+#include <array>
+#include <memory>
+
+#include <wpi/mutex.h>
+
+#include "hal/Errors.h"
+#include "hal/Types.h"
+#include "hal/handles/HandlesInternal.h"
+
+namespace hal {
+
+/**
+ * The IndexedHandleResource class is a way to track handles. This version
+ * allows a limited number of handles that are allocated by index.
+ * Because they are allocated by index, each individual index holds its own
+ * mutex, which reduces contention heavily.]
+ *
+ * @tparam THandle The Handle Type (Must be typedefed from HAL_Handle)
+ * @tparam TStruct The struct type held by this resource
+ * @tparam size The number of resources allowed to be allocated
+ * @tparam enumValue The type value stored in the handle
+ *
+ */
+template <typename THandle, typename TStruct, int16_t size,
+ HAL_HandleEnum enumValue>
+class IndexedHandleResource : public HandleBase {
+ friend class IndexedHandleResourceTest;
+
+ public:
+ IndexedHandleResource() = default;
+ IndexedHandleResource(const IndexedHandleResource&) = delete;
+ IndexedHandleResource& operator=(const IndexedHandleResource&) = delete;
+
+ THandle Allocate(int16_t index, int32_t* status);
+ std::shared_ptr<TStruct> Get(THandle handle);
+ void Free(THandle handle);
+ void ResetHandles() override;
+
+ private:
+ std::array<std::shared_ptr<TStruct>, size> m_structures;
+ std::array<wpi::mutex, size> m_handleMutexes;
+};
+
+template <typename THandle, typename TStruct, int16_t size,
+ HAL_HandleEnum enumValue>
+THandle IndexedHandleResource<THandle, TStruct, size, enumValue>::Allocate(
+ int16_t index, int32_t* status) {
+ // don't aquire the lock if we can fail early.
+ if (index < 0 || index >= size) {
+ *status = RESOURCE_OUT_OF_RANGE;
+ return HAL_kInvalidHandle;
+ }
+ std::scoped_lock lock(m_handleMutexes[index]);
+ // check for allocation, otherwise allocate and return a valid handle
+ if (m_structures[index] != nullptr) {
+ *status = RESOURCE_IS_ALLOCATED;
+ return HAL_kInvalidHandle;
+ }
+ m_structures[index] = std::make_shared<TStruct>();
+ return static_cast<THandle>(hal::createHandle(index, enumValue, m_version));
+}
+
+template <typename THandle, typename TStruct, int16_t size,
+ HAL_HandleEnum enumValue>
+std::shared_ptr<TStruct>
+IndexedHandleResource<THandle, TStruct, size, enumValue>::Get(THandle handle) {
+ // get handle index, and fail early if index out of range or wrong handle
+ int16_t index = getHandleTypedIndex(handle, enumValue, m_version);
+ if (index < 0 || index >= size) {
+ return nullptr;
+ }
+ std::scoped_lock lock(m_handleMutexes[index]);
+ // return structure. Null will propogate correctly, so no need to manually
+ // check.
+ return m_structures[index];
+}
+
+template <typename THandle, typename TStruct, int16_t size,
+ HAL_HandleEnum enumValue>
+void IndexedHandleResource<THandle, TStruct, size, enumValue>::Free(
+ THandle handle) {
+ // get handle index, and fail early if index out of range or wrong handle
+ int16_t index = getHandleTypedIndex(handle, enumValue, m_version);
+ if (index < 0 || index >= size) return;
+ // lock and deallocated handle
+ std::scoped_lock lock(m_handleMutexes[index]);
+ m_structures[index].reset();
+}
+
+template <typename THandle, typename TStruct, int16_t size,
+ HAL_HandleEnum enumValue>
+void IndexedHandleResource<THandle, TStruct, size, enumValue>::ResetHandles() {
+ for (int i = 0; i < size; i++) {
+ std::scoped_lock lock(m_handleMutexes[i]);
+ m_structures[i].reset();
+ }
+ HandleBase::ResetHandles();
+}
+} // namespace hal
diff --git a/hal/src/main/native/include/hal/handles/LimitedClassedHandleResource.h b/hal/src/main/native/include/hal/handles/LimitedClassedHandleResource.h
new file mode 100644
index 0000000..a991fc3
--- /dev/null
+++ b/hal/src/main/native/include/hal/handles/LimitedClassedHandleResource.h
@@ -0,0 +1,116 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 <stdint.h>
+
+#include <array>
+#include <memory>
+
+#include <wpi/mutex.h>
+
+#include "hal/Types.h"
+#include "hal/handles/HandlesInternal.h"
+
+namespace hal {
+
+/**
+ * The LimitedClassedHandleResource class is a way to track handles. This
+ * version
+ * allows a limited number of handles that are allocated sequentially.
+ *
+ * @tparam THandle The Handle Type (Must be typedefed from HAL_Handle)
+ * @tparam TStruct The struct type held by this resource
+ * @tparam size The number of resources allowed to be allocated
+ * @tparam enumValue The type value stored in the handle
+ *
+ */
+template <typename THandle, typename TStruct, int16_t size,
+ HAL_HandleEnum enumValue>
+class LimitedClassedHandleResource : public HandleBase {
+ friend class LimitedClassedHandleResourceTest;
+
+ public:
+ LimitedClassedHandleResource() = default;
+ LimitedClassedHandleResource(const LimitedClassedHandleResource&) = delete;
+ LimitedClassedHandleResource& operator=(const LimitedClassedHandleResource&) =
+ delete;
+
+ THandle Allocate(std::shared_ptr<TStruct> toSet);
+ std::shared_ptr<TStruct> Get(THandle handle);
+ void Free(THandle handle);
+ void ResetHandles() override;
+
+ private:
+ std::array<std::shared_ptr<TStruct>, size> m_structures;
+ std::array<wpi::mutex, size> m_handleMutexes;
+ wpi::mutex m_allocateMutex;
+};
+
+template <typename THandle, typename TStruct, int16_t size,
+ HAL_HandleEnum enumValue>
+THandle
+LimitedClassedHandleResource<THandle, TStruct, size, enumValue>::Allocate(
+ std::shared_ptr<TStruct> toSet) {
+ // globally lock to loop through indices
+ std::scoped_lock lock(m_allocateMutex);
+ for (int16_t i = 0; i < size; i++) {
+ if (m_structures[i] == nullptr) {
+ // if a false index is found, grab its specific mutex
+ // and allocate it.
+ std::scoped_lock lock(m_handleMutexes[i]);
+ m_structures[i] = toSet;
+ return static_cast<THandle>(createHandle(i, enumValue, m_version));
+ }
+ }
+ return HAL_kInvalidHandle;
+}
+
+template <typename THandle, typename TStruct, int16_t size,
+ HAL_HandleEnum enumValue>
+std::shared_ptr<TStruct>
+LimitedClassedHandleResource<THandle, TStruct, size, enumValue>::Get(
+ THandle handle) {
+ // get handle index, and fail early if index out of range or wrong handle
+ int16_t index = getHandleTypedIndex(handle, enumValue, m_version);
+ if (index < 0 || index >= size) {
+ return nullptr;
+ }
+ std::scoped_lock lock(m_handleMutexes[index]);
+ // return structure. Null will propogate correctly, so no need to manually
+ // check.
+ return m_structures[index];
+}
+
+template <typename THandle, typename TStruct, int16_t size,
+ HAL_HandleEnum enumValue>
+void LimitedClassedHandleResource<THandle, TStruct, size, enumValue>::Free(
+ THandle handle) {
+ // get handle index, and fail early if index out of range or wrong handle
+ int16_t index = getHandleTypedIndex(handle, enumValue, m_version);
+ if (index < 0 || index >= size) return;
+ // lock and deallocated handle
+ std::scoped_lock allocateLock(m_allocateMutex);
+ std::scoped_lock handleLock(m_handleMutexes[index]);
+ m_structures[index].reset();
+}
+
+template <typename THandle, typename TStruct, int16_t size,
+ HAL_HandleEnum enumValue>
+void LimitedClassedHandleResource<THandle, TStruct, size,
+ enumValue>::ResetHandles() {
+ {
+ std::scoped_lock allocateLock(m_allocateMutex);
+ for (int i = 0; i < size; i++) {
+ std::scoped_lock handleLock(m_handleMutexes[i]);
+ m_structures[i].reset();
+ }
+ }
+ HandleBase::ResetHandles();
+}
+} // namespace hal
diff --git a/hal/src/main/native/include/hal/handles/LimitedHandleResource.h b/hal/src/main/native/include/hal/handles/LimitedHandleResource.h
new file mode 100644
index 0000000..0756634
--- /dev/null
+++ b/hal/src/main/native/include/hal/handles/LimitedHandleResource.h
@@ -0,0 +1,110 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 <stdint.h>
+
+#include <array>
+#include <memory>
+
+#include <wpi/mutex.h>
+
+#include "HandlesInternal.h"
+#include "hal/Types.h"
+
+namespace hal {
+
+/**
+ * The LimitedHandleResource class is a way to track handles. This version
+ * allows a limited number of handles that are allocated sequentially.
+ *
+ * @tparam THandle The Handle Type (Must be typedefed from HAL_Handle)
+ * @tparam TStruct The struct type held by this resource
+ * @tparam size The number of resources allowed to be allocated
+ * @tparam enumValue The type value stored in the handle
+ *
+ */
+template <typename THandle, typename TStruct, int16_t size,
+ HAL_HandleEnum enumValue>
+class LimitedHandleResource : public HandleBase {
+ friend class LimitedHandleResourceTest;
+
+ public:
+ LimitedHandleResource() = default;
+ LimitedHandleResource(const LimitedHandleResource&) = delete;
+ LimitedHandleResource& operator=(const LimitedHandleResource&) = delete;
+
+ THandle Allocate();
+ std::shared_ptr<TStruct> Get(THandle handle);
+ void Free(THandle handle);
+ void ResetHandles() override;
+
+ private:
+ std::array<std::shared_ptr<TStruct>, size> m_structures;
+ std::array<wpi::mutex, size> m_handleMutexes;
+ wpi::mutex m_allocateMutex;
+};
+
+template <typename THandle, typename TStruct, int16_t size,
+ HAL_HandleEnum enumValue>
+THandle LimitedHandleResource<THandle, TStruct, size, enumValue>::Allocate() {
+ // globally lock to loop through indices
+ std::scoped_lock lock(m_allocateMutex);
+ for (int16_t i = 0; i < size; i++) {
+ if (m_structures[i] == nullptr) {
+ // if a false index is found, grab its specific mutex
+ // and allocate it.
+ std::scoped_lock lock(m_handleMutexes[i]);
+ m_structures[i] = std::make_shared<TStruct>();
+ return static_cast<THandle>(createHandle(i, enumValue, m_version));
+ }
+ }
+ return HAL_kInvalidHandle;
+}
+
+template <typename THandle, typename TStruct, int16_t size,
+ HAL_HandleEnum enumValue>
+std::shared_ptr<TStruct>
+LimitedHandleResource<THandle, TStruct, size, enumValue>::Get(THandle handle) {
+ // get handle index, and fail early if index out of range or wrong handle
+ int16_t index = getHandleTypedIndex(handle, enumValue, m_version);
+ if (index < 0 || index >= size) {
+ return nullptr;
+ }
+ std::scoped_lock lock(m_handleMutexes[index]);
+ // return structure. Null will propogate correctly, so no need to manually
+ // check.
+ return m_structures[index];
+}
+
+template <typename THandle, typename TStruct, int16_t size,
+ HAL_HandleEnum enumValue>
+void LimitedHandleResource<THandle, TStruct, size, enumValue>::Free(
+ THandle handle) {
+ // get handle index, and fail early if index out of range or wrong handle
+ int16_t index = getHandleTypedIndex(handle, enumValue, m_version);
+ if (index < 0 || index >= size) return;
+ // lock and deallocated handle
+ std::scoped_lock allocateLock(m_allocateMutex);
+ std::scoped_lock handleLock(m_handleMutexes[index]);
+ m_structures[index].reset();
+}
+
+template <typename THandle, typename TStruct, int16_t size,
+ HAL_HandleEnum enumValue>
+void LimitedHandleResource<THandle, TStruct, size, enumValue>::ResetHandles() {
+ {
+ std::scoped_lock allocateLock(m_allocateMutex);
+ for (int i = 0; i < size; i++) {
+ std::scoped_lock handleLock(m_handleMutexes[i]);
+ m_structures[i].reset();
+ }
+ }
+ HandleBase::ResetHandles();
+}
+} // namespace hal
diff --git a/hal/src/main/native/include/hal/handles/UnlimitedHandleResource.h b/hal/src/main/native/include/hal/handles/UnlimitedHandleResource.h
new file mode 100644
index 0000000..96a91f8
--- /dev/null
+++ b/hal/src/main/native/include/hal/handles/UnlimitedHandleResource.h
@@ -0,0 +1,126 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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 <stdint.h>
+
+#include <memory>
+#include <utility>
+#include <vector>
+
+#include <wpi/mutex.h>
+
+#include "hal/Types.h"
+#include "hal/handles/HandlesInternal.h"
+
+namespace hal {
+
+/**
+ * The UnlimitedHandleResource class is a way to track handles. This version
+ * allows an unlimted number of handles that are allocated sequentially. When
+ * possible, indices are reused to save memory usage and keep the array length
+ * down.
+ * However, automatic array management has not been implemented, but might be in
+ * the future.
+ * Because we have to loop through the allocator, we must use a global mutex.
+
+ * @tparam THandle The Handle Type (Must be typedefed from HAL_Handle)
+ * @tparam TStruct The struct type held by this resource
+ * @tparam enumValue The type value stored in the handle
+ *
+ */
+template <typename THandle, typename TStruct, HAL_HandleEnum enumValue>
+class UnlimitedHandleResource : public HandleBase {
+ friend class UnlimitedHandleResourceTest;
+
+ public:
+ UnlimitedHandleResource() = default;
+ UnlimitedHandleResource(const UnlimitedHandleResource&) = delete;
+ UnlimitedHandleResource& operator=(const UnlimitedHandleResource&) = delete;
+
+ THandle Allocate(std::shared_ptr<TStruct> structure);
+ std::shared_ptr<TStruct> Get(THandle handle);
+ /* Returns structure previously at that handle (or nullptr if none) */
+ std::shared_ptr<TStruct> Free(THandle handle);
+ void ResetHandles() override;
+
+ /* Calls func(THandle, TStruct*) for each handle. Note this holds the
+ * global lock for the entirety of execution.
+ */
+ template <typename Functor>
+ void ForEach(Functor func);
+
+ private:
+ std::vector<std::shared_ptr<TStruct>> m_structures;
+ wpi::mutex m_handleMutex;
+};
+
+template <typename THandle, typename TStruct, HAL_HandleEnum enumValue>
+THandle UnlimitedHandleResource<THandle, TStruct, enumValue>::Allocate(
+ std::shared_ptr<TStruct> structure) {
+ std::scoped_lock lock(m_handleMutex);
+ size_t i;
+ for (i = 0; i < m_structures.size(); i++) {
+ if (m_structures[i] == nullptr) {
+ m_structures[i] = structure;
+ return static_cast<THandle>(createHandle(i, enumValue, m_version));
+ }
+ }
+ if (i >= INT16_MAX) return HAL_kInvalidHandle;
+
+ m_structures.push_back(structure);
+ return static_cast<THandle>(
+ createHandle(static_cast<int16_t>(i), enumValue, m_version));
+}
+
+template <typename THandle, typename TStruct, HAL_HandleEnum enumValue>
+std::shared_ptr<TStruct>
+UnlimitedHandleResource<THandle, TStruct, enumValue>::Get(THandle handle) {
+ int16_t index = getHandleTypedIndex(handle, enumValue, m_version);
+ std::scoped_lock lock(m_handleMutex);
+ if (index < 0 || index >= static_cast<int16_t>(m_structures.size()))
+ return nullptr;
+ return m_structures[index];
+}
+
+template <typename THandle, typename TStruct, HAL_HandleEnum enumValue>
+std::shared_ptr<TStruct>
+UnlimitedHandleResource<THandle, TStruct, enumValue>::Free(THandle handle) {
+ int16_t index = getHandleTypedIndex(handle, enumValue, m_version);
+ std::scoped_lock lock(m_handleMutex);
+ if (index < 0 || index >= static_cast<int16_t>(m_structures.size()))
+ return nullptr;
+ return std::move(m_structures[index]);
+}
+
+template <typename THandle, typename TStruct, HAL_HandleEnum enumValue>
+void UnlimitedHandleResource<THandle, TStruct, enumValue>::ResetHandles() {
+ {
+ std::scoped_lock lock(m_handleMutex);
+ for (size_t i = 0; i < m_structures.size(); i++) {
+ m_structures[i].reset();
+ }
+ }
+ HandleBase::ResetHandles();
+}
+
+template <typename THandle, typename TStruct, HAL_HandleEnum enumValue>
+template <typename Functor>
+void UnlimitedHandleResource<THandle, TStruct, enumValue>::ForEach(
+ Functor func) {
+ std::scoped_lock lock(m_handleMutex);
+ size_t i;
+ for (i = 0; i < m_structures.size(); i++) {
+ if (m_structures[i] != nullptr) {
+ func(static_cast<THandle>(createHandle(i, enumValue, m_version)),
+ m_structures[i].get());
+ }
+ }
+}
+
+} // namespace hal
diff --git a/hal/src/main/native/include/mockdata/AccelerometerData.h b/hal/src/main/native/include/mockdata/AccelerometerData.h
new file mode 100644
index 0000000..aa89f6e
--- /dev/null
+++ b/hal/src/main/native/include/mockdata/AccelerometerData.h
@@ -0,0 +1,66 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "NotifyListener.h"
+#include "hal/Accelerometer.h"
+#include "hal/Types.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void HALSIM_ResetAccelerometerData(int32_t index);
+int32_t HALSIM_RegisterAccelerometerActiveCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelAccelerometerActiveCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetAccelerometerActive(int32_t index);
+void HALSIM_SetAccelerometerActive(int32_t index, HAL_Bool active);
+
+int32_t HALSIM_RegisterAccelerometerRangeCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelAccelerometerRangeCallback(int32_t index, int32_t uid);
+HAL_AccelerometerRange HALSIM_GetAccelerometerRange(int32_t index);
+void HALSIM_SetAccelerometerRange(int32_t index, HAL_AccelerometerRange range);
+
+int32_t HALSIM_RegisterAccelerometerXCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelAccelerometerXCallback(int32_t index, int32_t uid);
+double HALSIM_GetAccelerometerX(int32_t index);
+void HALSIM_SetAccelerometerX(int32_t index, double x);
+
+int32_t HALSIM_RegisterAccelerometerYCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelAccelerometerYCallback(int32_t index, int32_t uid);
+double HALSIM_GetAccelerometerY(int32_t index);
+void HALSIM_SetAccelerometerY(int32_t index, double y);
+
+int32_t HALSIM_RegisterAccelerometerZCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelAccelerometerZCallback(int32_t index, int32_t uid);
+double HALSIM_GetAccelerometerZ(int32_t index);
+void HALSIM_SetAccelerometerZ(int32_t index, double z);
+
+void HALSIM_RegisterAccelerometerAllCallbacks(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
diff --git a/hal/src/main/native/include/mockdata/AddressableLEDData.h b/hal/src/main/native/include/mockdata/AddressableLEDData.h
new file mode 100644
index 0000000..0d8f3f3
--- /dev/null
+++ b/hal/src/main/native/include/mockdata/AddressableLEDData.h
@@ -0,0 +1,65 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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 "NotifyListener.h"
+#include "hal/AddressableLEDTypes.h"
+#include "hal/Types.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void HALSIM_ResetAddressableLEDData(int32_t index);
+
+int32_t HALSIM_RegisterAddressableLEDInitializedCallback(
+ int32_t index, HAL_NotifyCallback callback, void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelAddressableLEDInitializedCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetAddressableLEDInitialized(int32_t index);
+void HALSIM_SetAddressableLEDInitialized(int32_t index, HAL_Bool initialized);
+
+int32_t HALSIM_RegisterAddressableLEDOutputPortCallback(
+ int32_t index, HAL_NotifyCallback callback, void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelAddressableLEDOutputPortCallback(int32_t index, int32_t uid);
+int32_t HALSIM_GetAddressableLEDOutputPort(int32_t index);
+void HALSIM_SetAddressableLEDOutputPort(int32_t index, int32_t outputPort);
+
+int32_t HALSIM_RegisterAddressableLEDLengthCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelAddressableLEDLengthCallback(int32_t index, int32_t uid);
+int32_t HALSIM_GetAddressableLEDLength(int32_t index);
+void HALSIM_SetAddressableLEDLength(int32_t index, int32_t length);
+
+int32_t HALSIM_RegisterAddressableLEDRunningCallback(
+ int32_t index, HAL_NotifyCallback callback, void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelAddressableLEDRunningCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetAddressableLEDRunning(int32_t index);
+void HALSIM_SetAddressableLEDRunning(int32_t index, HAL_Bool running);
+
+int32_t HALSIM_RegisterAddressableLEDDataCallback(
+ int32_t index, HAL_ConstBufferCallback callback, void* param);
+void HALSIM_CancelAddressableLEDDataCallback(int32_t index, int32_t uid);
+int32_t HALSIM_GetAddressableLEDData(int32_t index,
+ struct HAL_AddressableLEDData* data);
+void HALSIM_SetAddressableLEDData(int32_t index,
+ const struct HAL_AddressableLEDData* data,
+ int32_t length);
+
+void HALSIM_RegisterAddressableLEDAllCallbacks(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
diff --git a/hal/src/main/native/include/mockdata/AnalogGyroData.h b/hal/src/main/native/include/mockdata/AnalogGyroData.h
new file mode 100644
index 0000000..56c739d
--- /dev/null
+++ b/hal/src/main/native/include/mockdata/AnalogGyroData.h
@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "NotifyListener.h"
+#include "hal/Types.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void HALSIM_ResetAnalogGyroData(int32_t index);
+int32_t HALSIM_RegisterAnalogGyroAngleCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelAnalogGyroAngleCallback(int32_t index, int32_t uid);
+double HALSIM_GetAnalogGyroAngle(int32_t index);
+void HALSIM_SetAnalogGyroAngle(int32_t index, double angle);
+
+int32_t HALSIM_RegisterAnalogGyroRateCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelAnalogGyroRateCallback(int32_t index, int32_t uid);
+double HALSIM_GetAnalogGyroRate(int32_t index);
+void HALSIM_SetAnalogGyroRate(int32_t index, double rate);
+
+int32_t HALSIM_RegisterAnalogGyroInitializedCallback(
+ int32_t index, HAL_NotifyCallback callback, void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelAnalogGyroInitializedCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetAnalogGyroInitialized(int32_t index);
+void HALSIM_SetAnalogGyroInitialized(int32_t index, HAL_Bool initialized);
+
+void HALSIM_RegisterAnalogGyroAllCallbacks(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param, HAL_Bool initialNotify);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
diff --git a/hal/src/main/native/include/mockdata/AnalogInData.h b/hal/src/main/native/include/mockdata/AnalogInData.h
new file mode 100644
index 0000000..e571563
--- /dev/null
+++ b/hal/src/main/native/include/mockdata/AnalogInData.h
@@ -0,0 +1,99 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "NotifyListener.h"
+#include "hal/Types.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void HALSIM_ResetAnalogInData(int32_t index);
+int32_t HALSIM_RegisterAnalogInInitializedCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelAnalogInInitializedCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetAnalogInInitialized(int32_t index);
+void HALSIM_SetAnalogInInitialized(int32_t index, HAL_Bool initialized);
+
+HAL_SimDeviceHandle HALSIM_GetAnalogInSimDevice(int32_t index);
+
+int32_t HALSIM_RegisterAnalogInAverageBitsCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelAnalogInAverageBitsCallback(int32_t index, int32_t uid);
+int32_t HALSIM_GetAnalogInAverageBits(int32_t index);
+void HALSIM_SetAnalogInAverageBits(int32_t index, int32_t averageBits);
+
+int32_t HALSIM_RegisterAnalogInOversampleBitsCallback(
+ int32_t index, HAL_NotifyCallback callback, void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelAnalogInOversampleBitsCallback(int32_t index, int32_t uid);
+int32_t HALSIM_GetAnalogInOversampleBits(int32_t index);
+void HALSIM_SetAnalogInOversampleBits(int32_t index, int32_t oversampleBits);
+
+int32_t HALSIM_RegisterAnalogInVoltageCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelAnalogInVoltageCallback(int32_t index, int32_t uid);
+double HALSIM_GetAnalogInVoltage(int32_t index);
+void HALSIM_SetAnalogInVoltage(int32_t index, double voltage);
+
+int32_t HALSIM_RegisterAnalogInAccumulatorInitializedCallback(
+ int32_t index, HAL_NotifyCallback callback, void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelAnalogInAccumulatorInitializedCallback(int32_t index,
+ int32_t uid);
+HAL_Bool HALSIM_GetAnalogInAccumulatorInitialized(int32_t index);
+void HALSIM_SetAnalogInAccumulatorInitialized(int32_t index,
+ HAL_Bool accumulatorInitialized);
+
+int32_t HALSIM_RegisterAnalogInAccumulatorValueCallback(
+ int32_t index, HAL_NotifyCallback callback, void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelAnalogInAccumulatorValueCallback(int32_t index, int32_t uid);
+int64_t HALSIM_GetAnalogInAccumulatorValue(int32_t index);
+void HALSIM_SetAnalogInAccumulatorValue(int32_t index,
+ int64_t accumulatorValue);
+
+int32_t HALSIM_RegisterAnalogInAccumulatorCountCallback(
+ int32_t index, HAL_NotifyCallback callback, void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelAnalogInAccumulatorCountCallback(int32_t index, int32_t uid);
+int64_t HALSIM_GetAnalogInAccumulatorCount(int32_t index);
+void HALSIM_SetAnalogInAccumulatorCount(int32_t index,
+ int64_t accumulatorCount);
+
+int32_t HALSIM_RegisterAnalogInAccumulatorCenterCallback(
+ int32_t index, HAL_NotifyCallback callback, void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelAnalogInAccumulatorCenterCallback(int32_t index, int32_t uid);
+int32_t HALSIM_GetAnalogInAccumulatorCenter(int32_t index);
+void HALSIM_SetAnalogInAccumulatorCenter(int32_t index,
+ int32_t accumulatorCenter);
+
+int32_t HALSIM_RegisterAnalogInAccumulatorDeadbandCallback(
+ int32_t index, HAL_NotifyCallback callback, void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelAnalogInAccumulatorDeadbandCallback(int32_t index,
+ int32_t uid);
+int32_t HALSIM_GetAnalogInAccumulatorDeadband(int32_t index);
+void HALSIM_SetAnalogInAccumulatorDeadband(int32_t index,
+ int32_t accumulatorDeadband);
+
+void HALSIM_RegisterAnalogInAllCallbacks(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param, HAL_Bool initialNotify);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
diff --git a/hal/src/main/native/include/mockdata/AnalogOutData.h b/hal/src/main/native/include/mockdata/AnalogOutData.h
new file mode 100644
index 0000000..e8f55f6
--- /dev/null
+++ b/hal/src/main/native/include/mockdata/AnalogOutData.h
@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "NotifyListener.h"
+#include "hal/Types.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void HALSIM_ResetAnalogOutData(int32_t index);
+int32_t HALSIM_RegisterAnalogOutVoltageCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelAnalogOutVoltageCallback(int32_t index, int32_t uid);
+double HALSIM_GetAnalogOutVoltage(int32_t index);
+void HALSIM_SetAnalogOutVoltage(int32_t index, double voltage);
+
+int32_t HALSIM_RegisterAnalogOutInitializedCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelAnalogOutInitializedCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetAnalogOutInitialized(int32_t index);
+void HALSIM_SetAnalogOutInitialized(int32_t index, HAL_Bool initialized);
+
+void HALSIM_RegisterAnalogOutAllCallbacks(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param, HAL_Bool initialNotify);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
diff --git a/hal/src/main/native/include/mockdata/AnalogTriggerData.h b/hal/src/main/native/include/mockdata/AnalogTriggerData.h
new file mode 100644
index 0000000..566f2f5
--- /dev/null
+++ b/hal/src/main/native/include/mockdata/AnalogTriggerData.h
@@ -0,0 +1,65 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "NotifyListener.h"
+#include "hal/Types.h"
+
+enum HALSIM_AnalogTriggerMode : int32_t {
+ HALSIM_AnalogTriggerUnassigned,
+ HALSIM_AnalogTriggerFiltered,
+ HALSIM_AnalogTriggerDutyCycle,
+ HALSIM_AnalogTriggerAveraged
+};
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void HALSIM_ResetAnalogTriggerData(int32_t index);
+int32_t HALSIM_RegisterAnalogTriggerInitializedCallback(
+ int32_t index, HAL_NotifyCallback callback, void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelAnalogTriggerInitializedCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetAnalogTriggerInitialized(int32_t index);
+void HALSIM_SetAnalogTriggerInitialized(int32_t index, HAL_Bool initialized);
+
+int32_t HALSIM_RegisterAnalogTriggerTriggerLowerBoundCallback(
+ int32_t index, HAL_NotifyCallback callback, void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelAnalogTriggerTriggerLowerBoundCallback(int32_t index,
+ int32_t uid);
+double HALSIM_GetAnalogTriggerTriggerLowerBound(int32_t index);
+void HALSIM_SetAnalogTriggerTriggerLowerBound(int32_t index,
+ double triggerLowerBound);
+
+int32_t HALSIM_RegisterAnalogTriggerTriggerUpperBoundCallback(
+ int32_t index, HAL_NotifyCallback callback, void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelAnalogTriggerTriggerUpperBoundCallback(int32_t index,
+ int32_t uid);
+double HALSIM_GetAnalogTriggerTriggerUpperBound(int32_t index);
+void HALSIM_SetAnalogTriggerTriggerUpperBound(int32_t index,
+ double triggerUpperBound);
+
+int32_t HALSIM_RegisterAnalogTriggerTriggerModeCallback(
+ int32_t index, HAL_NotifyCallback callback, void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelAnalogTriggerTriggerModeCallback(int32_t index, int32_t uid);
+HALSIM_AnalogTriggerMode HALSIM_GetAnalogTriggerTriggerMode(int32_t index);
+void HALSIM_SetAnalogTriggerTriggerMode(int32_t index,
+ HALSIM_AnalogTriggerMode triggerMode);
+
+void HALSIM_RegisterAnalogTriggerAllCallbacks(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
diff --git a/hal/src/main/native/include/mockdata/CanData.h b/hal/src/main/native/include/mockdata/CanData.h
new file mode 100644
index 0000000..0d48290
--- /dev/null
+++ b/hal/src/main/native/include/mockdata/CanData.h
@@ -0,0 +1,74 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "NotifyListener.h"
+#include "hal/Types.h"
+#include "hal/Value.h"
+
+typedef void (*HAL_CAN_SendMessageCallback)(const char* name, void* param,
+ uint32_t messageID,
+ const uint8_t* data,
+ uint8_t dataSize, int32_t periodMs,
+ int32_t* status);
+
+typedef void (*HAL_CAN_ReceiveMessageCallback)(
+ const char* name, void* param, uint32_t* messageID, uint32_t messageIDMask,
+ uint8_t* data, uint8_t* dataSize, uint32_t* timeStamp, int32_t* status);
+
+typedef void (*HAL_CAN_OpenStreamSessionCallback)(
+ const char* name, void* param, uint32_t* sessionHandle, uint32_t messageID,
+ uint32_t messageIDMask, uint32_t maxMessages, int32_t* status);
+
+typedef void (*HAL_CAN_CloseStreamSessionCallback)(const char* name,
+ void* param,
+ uint32_t sessionHandle);
+
+typedef void (*HAL_CAN_ReadStreamSessionCallback)(
+ const char* name, void* param, uint32_t sessionHandle,
+ struct HAL_CANStreamMessage* messages, uint32_t messagesToRead,
+ uint32_t* messagesRead, int32_t* status);
+
+typedef void (*HAL_CAN_GetCANStatusCallback)(
+ const char* name, void* param, float* percentBusUtilization,
+ uint32_t* busOffCount, uint32_t* txFullCount, uint32_t* receiveErrorCount,
+ uint32_t* transmitErrorCount, int32_t* status);
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void HALSIM_ResetCanData(void);
+
+int32_t HALSIM_RegisterCanSendMessageCallback(
+ HAL_CAN_SendMessageCallback callback, void* param);
+void HALSIM_CancelCanSendMessageCallback(int32_t uid);
+
+int32_t HALSIM_RegisterCanReceiveMessageCallback(
+ HAL_CAN_ReceiveMessageCallback callback, void* param);
+void HALSIM_CancelCanReceiveMessageCallback(int32_t uid);
+
+int32_t HALSIM_RegisterCanOpenStreamCallback(
+ HAL_CAN_OpenStreamSessionCallback callback, void* param);
+void HALSIM_CancelCanOpenStreamCallback(int32_t uid);
+
+int32_t HALSIM_RegisterCanCloseStreamCallback(
+ HAL_CAN_CloseStreamSessionCallback callback, void* param);
+void HALSIM_CancelCanCloseStreamCallback(int32_t uid);
+
+int32_t HALSIM_RegisterCanReadStreamCallback(
+ HAL_CAN_ReadStreamSessionCallback callback, void* param);
+void HALSIM_CancelCanReadStreamCallback(int32_t uid);
+
+int32_t HALSIM_RegisterCanGetCANStatusCallback(
+ HAL_CAN_GetCANStatusCallback callback, void* param);
+void HALSIM_CancelCanGetCANStatusCallback(int32_t uid);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
diff --git a/hal/src/main/native/include/mockdata/DIOData.h b/hal/src/main/native/include/mockdata/DIOData.h
new file mode 100644
index 0000000..d13eee1
--- /dev/null
+++ b/hal/src/main/native/include/mockdata/DIOData.h
@@ -0,0 +1,63 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "NotifyListener.h"
+#include "hal/Types.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void HALSIM_ResetDIOData(int32_t index);
+int32_t HALSIM_RegisterDIOInitializedCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelDIOInitializedCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetDIOInitialized(int32_t index);
+void HALSIM_SetDIOInitialized(int32_t index, HAL_Bool initialized);
+
+HAL_SimDeviceHandle HALSIM_GetDIOSimDevice(int32_t index);
+
+int32_t HALSIM_RegisterDIOValueCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param, HAL_Bool initialNotify);
+void HALSIM_CancelDIOValueCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetDIOValue(int32_t index);
+void HALSIM_SetDIOValue(int32_t index, HAL_Bool value);
+
+int32_t HALSIM_RegisterDIOPulseLengthCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelDIOPulseLengthCallback(int32_t index, int32_t uid);
+double HALSIM_GetDIOPulseLength(int32_t index);
+void HALSIM_SetDIOPulseLength(int32_t index, double pulseLength);
+
+int32_t HALSIM_RegisterDIOIsInputCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param, HAL_Bool initialNotify);
+void HALSIM_CancelDIOIsInputCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetDIOIsInput(int32_t index);
+void HALSIM_SetDIOIsInput(int32_t index, HAL_Bool isInput);
+
+int32_t HALSIM_RegisterDIOFilterIndexCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelDIOFilterIndexCallback(int32_t index, int32_t uid);
+int32_t HALSIM_GetDIOFilterIndex(int32_t index);
+void HALSIM_SetDIOFilterIndex(int32_t index, int32_t filterIndex);
+
+void HALSIM_RegisterDIOAllCallbacks(int32_t index, HAL_NotifyCallback callback,
+ void* param, HAL_Bool initialNotify);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
diff --git a/hal/src/main/native/include/mockdata/DigitalPWMData.h b/hal/src/main/native/include/mockdata/DigitalPWMData.h
new file mode 100644
index 0000000..5af17e3
--- /dev/null
+++ b/hal/src/main/native/include/mockdata/DigitalPWMData.h
@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "NotifyListener.h"
+#include "hal/Types.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void HALSIM_ResetDigitalPWMData(int32_t index);
+int32_t HALSIM_RegisterDigitalPWMInitializedCallback(
+ int32_t index, HAL_NotifyCallback callback, void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelDigitalPWMInitializedCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetDigitalPWMInitialized(int32_t index);
+void HALSIM_SetDigitalPWMInitialized(int32_t index, HAL_Bool initialized);
+
+int32_t HALSIM_RegisterDigitalPWMDutyCycleCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelDigitalPWMDutyCycleCallback(int32_t index, int32_t uid);
+double HALSIM_GetDigitalPWMDutyCycle(int32_t index);
+void HALSIM_SetDigitalPWMDutyCycle(int32_t index, double dutyCycle);
+
+int32_t HALSIM_RegisterDigitalPWMPinCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelDigitalPWMPinCallback(int32_t index, int32_t uid);
+int32_t HALSIM_GetDigitalPWMPin(int32_t index);
+void HALSIM_SetDigitalPWMPin(int32_t index, int32_t pin);
+
+void HALSIM_RegisterDigitalPWMAllCallbacks(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param, HAL_Bool initialNotify);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
diff --git a/hal/src/main/native/include/mockdata/DriverStationData.h b/hal/src/main/native/include/mockdata/DriverStationData.h
new file mode 100644
index 0000000..1f69664
--- /dev/null
+++ b/hal/src/main/native/include/mockdata/DriverStationData.h
@@ -0,0 +1,91 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "NotifyListener.h"
+#include "hal/DriverStationTypes.h"
+#include "hal/Types.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void HALSIM_ResetDriverStationData(void);
+int32_t HALSIM_RegisterDriverStationEnabledCallback(HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelDriverStationEnabledCallback(int32_t uid);
+HAL_Bool HALSIM_GetDriverStationEnabled(void);
+void HALSIM_SetDriverStationEnabled(HAL_Bool enabled);
+
+int32_t HALSIM_RegisterDriverStationAutonomousCallback(
+ HAL_NotifyCallback callback, void* param, HAL_Bool initialNotify);
+void HALSIM_CancelDriverStationAutonomousCallback(int32_t uid);
+HAL_Bool HALSIM_GetDriverStationAutonomous(void);
+void HALSIM_SetDriverStationAutonomous(HAL_Bool autonomous);
+
+int32_t HALSIM_RegisterDriverStationTestCallback(HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelDriverStationTestCallback(int32_t uid);
+HAL_Bool HALSIM_GetDriverStationTest(void);
+void HALSIM_SetDriverStationTest(HAL_Bool test);
+
+int32_t HALSIM_RegisterDriverStationEStopCallback(HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelDriverStationEStopCallback(int32_t uid);
+HAL_Bool HALSIM_GetDriverStationEStop(void);
+void HALSIM_SetDriverStationEStop(HAL_Bool eStop);
+
+int32_t HALSIM_RegisterDriverStationFmsAttachedCallback(
+ HAL_NotifyCallback callback, void* param, HAL_Bool initialNotify);
+void HALSIM_CancelDriverStationFmsAttachedCallback(int32_t uid);
+HAL_Bool HALSIM_GetDriverStationFmsAttached(void);
+void HALSIM_SetDriverStationFmsAttached(HAL_Bool fmsAttached);
+
+int32_t HALSIM_RegisterDriverStationDsAttachedCallback(
+ HAL_NotifyCallback callback, void* param, HAL_Bool initialNotify);
+void HALSIM_CancelDriverStationDsAttachedCallback(int32_t uid);
+HAL_Bool HALSIM_GetDriverStationDsAttached(void);
+void HALSIM_SetDriverStationDsAttached(HAL_Bool dsAttached);
+
+int32_t HALSIM_RegisterDriverStationAllianceStationIdCallback(
+ HAL_NotifyCallback callback, void* param, HAL_Bool initialNotify);
+void HALSIM_CancelDriverStationAllianceStationIdCallback(int32_t uid);
+HAL_AllianceStationID HALSIM_GetDriverStationAllianceStationId(void);
+void HALSIM_SetDriverStationAllianceStationId(
+ HAL_AllianceStationID allianceStationId);
+
+int32_t HALSIM_RegisterDriverStationMatchTimeCallback(
+ HAL_NotifyCallback callback, void* param, HAL_Bool initialNotify);
+void HALSIM_CancelDriverStationMatchTimeCallback(int32_t uid);
+double HALSIM_GetDriverStationMatchTime(void);
+void HALSIM_SetDriverStationMatchTime(double matchTime);
+
+void HALSIM_SetJoystickAxes(int32_t joystickNum, const HAL_JoystickAxes* axes);
+void HALSIM_SetJoystickPOVs(int32_t joystickNum, const HAL_JoystickPOVs* povs);
+void HALSIM_SetJoystickButtons(int32_t joystickNum,
+ const HAL_JoystickButtons* buttons);
+void HALSIM_SetJoystickDescriptor(int32_t joystickNum,
+ const HAL_JoystickDescriptor* descriptor);
+
+void HALSIM_GetJoystickOutputs(int32_t joystickNum, int64_t* outputs,
+ int32_t* leftRumble, int32_t* rightRumble);
+
+void HALSIM_SetMatchInfo(const HAL_MatchInfo* info);
+
+void HALSIM_RegisterDriverStationAllCallbacks(HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+
+void HALSIM_NotifyDriverStationNewData(void);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
diff --git a/hal/src/main/native/include/mockdata/DutyCycleData.h b/hal/src/main/native/include/mockdata/DutyCycleData.h
new file mode 100644
index 0000000..b97b355
--- /dev/null
+++ b/hal/src/main/native/include/mockdata/DutyCycleData.h
@@ -0,0 +1,52 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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 "NotifyListener.h"
+#include "hal/Types.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void HALSIM_ResetDutyCycleData(int32_t index);
+int32_t HALSIM_GetDutyCycleDigitalChannel(int32_t index);
+
+int32_t HALSIM_RegisterDutyCycleInitializedCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelDutyCycleInitializedCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetDutyCycleInitialized(int32_t index);
+void HALSIM_SetDutyCycleInitialized(int32_t index, HAL_Bool initialized);
+
+HAL_SimDeviceHandle HALSIM_GetDutyCycleSimDevice(int32_t index);
+
+int32_t HALSIM_RegisterDutyCycleOutputCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelDutyCycleOutputCallback(int32_t index, int32_t uid);
+double HALSIM_GetDutyCycleOutput(int32_t index);
+void HALSIM_SetDutyCycleOutput(int32_t index, double output);
+
+int32_t HALSIM_RegisterDutyCycleFrequencyCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelDutyCycleFrequencyCallback(int32_t index, int32_t uid);
+int32_t HALSIM_GetDutyCycleFrequency(int32_t index);
+void HALSIM_SetDutyCycleFrequency(int32_t index, int32_t frequency);
+
+void HALSIM_RegisterDutyCycleAllCallbacks(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param, HAL_Bool initialNotify);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
diff --git a/hal/src/main/native/include/mockdata/EncoderData.h b/hal/src/main/native/include/mockdata/EncoderData.h
new file mode 100644
index 0000000..d5d12be
--- /dev/null
+++ b/hal/src/main/native/include/mockdata/EncoderData.h
@@ -0,0 +1,98 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "NotifyListener.h"
+#include "hal/Types.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void HALSIM_ResetEncoderData(int32_t index);
+int32_t HALSIM_GetEncoderDigitalChannelA(int32_t index);
+int32_t HALSIM_GetEncoderDigitalChannelB(int32_t index);
+int32_t HALSIM_RegisterEncoderInitializedCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelEncoderInitializedCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetEncoderInitialized(int32_t index);
+void HALSIM_SetEncoderInitialized(int32_t index, HAL_Bool initialized);
+
+HAL_SimDeviceHandle HALSIM_GetEncoderSimDevice(int32_t index);
+
+int32_t HALSIM_RegisterEncoderCountCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelEncoderCountCallback(int32_t index, int32_t uid);
+int32_t HALSIM_GetEncoderCount(int32_t index);
+void HALSIM_SetEncoderCount(int32_t index, int32_t count);
+
+int32_t HALSIM_RegisterEncoderPeriodCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelEncoderPeriodCallback(int32_t index, int32_t uid);
+double HALSIM_GetEncoderPeriod(int32_t index);
+void HALSIM_SetEncoderPeriod(int32_t index, double period);
+
+int32_t HALSIM_RegisterEncoderResetCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelEncoderResetCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetEncoderReset(int32_t index);
+void HALSIM_SetEncoderReset(int32_t index, HAL_Bool reset);
+
+int32_t HALSIM_RegisterEncoderMaxPeriodCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelEncoderMaxPeriodCallback(int32_t index, int32_t uid);
+double HALSIM_GetEncoderMaxPeriod(int32_t index);
+void HALSIM_SetEncoderMaxPeriod(int32_t index, double maxPeriod);
+
+int32_t HALSIM_RegisterEncoderDirectionCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelEncoderDirectionCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetEncoderDirection(int32_t index);
+void HALSIM_SetEncoderDirection(int32_t index, HAL_Bool direction);
+
+int32_t HALSIM_RegisterEncoderReverseDirectionCallback(
+ int32_t index, HAL_NotifyCallback callback, void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelEncoderReverseDirectionCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetEncoderReverseDirection(int32_t index);
+void HALSIM_SetEncoderReverseDirection(int32_t index,
+ HAL_Bool reverseDirection);
+
+int32_t HALSIM_RegisterEncoderSamplesToAverageCallback(
+ int32_t index, HAL_NotifyCallback callback, void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelEncoderSamplesToAverageCallback(int32_t index, int32_t uid);
+int32_t HALSIM_GetEncoderSamplesToAverage(int32_t index);
+void HALSIM_SetEncoderSamplesToAverage(int32_t index, int32_t samplesToAverage);
+
+int32_t HALSIM_RegisterEncoderDistancePerPulseCallback(
+ int32_t index, HAL_NotifyCallback callback, void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelEncoderDistancePerPulseCallback(int32_t index, int32_t uid);
+double HALSIM_GetEncoderDistancePerPulse(int32_t index);
+void HALSIM_SetEncoderDistancePerPulse(int32_t index, double distancePerPulse);
+
+void HALSIM_RegisterEncoderAllCallbacks(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param, HAL_Bool initialNotify);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
diff --git a/hal/src/main/native/include/mockdata/I2CData.h b/hal/src/main/native/include/mockdata/I2CData.h
new file mode 100644
index 0000000..32ae3a8
--- /dev/null
+++ b/hal/src/main/native/include/mockdata/I2CData.h
@@ -0,0 +1,39 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "NotifyListener.h"
+#include "hal/Types.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void HALSIM_ResetI2CData(int32_t index);
+
+int32_t HALSIM_RegisterI2CInitializedCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelI2CInitializedCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetI2CInitialized(int32_t index);
+void HALSIM_SetI2CInitialized(int32_t index, HAL_Bool initialized);
+
+int32_t HALSIM_RegisterI2CReadCallback(int32_t index,
+ HAL_BufferCallback callback,
+ void* param);
+void HALSIM_CancelI2CReadCallback(int32_t index, int32_t uid);
+
+int32_t HALSIM_RegisterI2CWriteCallback(int32_t index,
+ HAL_ConstBufferCallback callback,
+ void* param);
+void HALSIM_CancelI2CWriteCallback(int32_t index, int32_t uid);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
diff --git a/hal/src/main/native/include/mockdata/MockHooks.h b/hal/src/main/native/include/mockdata/MockHooks.h
new file mode 100644
index 0000000..318ff21
--- /dev/null
+++ b/hal/src/main/native/include/mockdata/MockHooks.h
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "hal/Types.h"
+
+extern "C" {
+void HALSIM_WaitForProgramStart(void);
+void HALSIM_SetProgramStarted(void);
+HAL_Bool HALSIM_GetProgramStarted(void);
+void HALSIM_RestartTiming(void);
+void HALSIM_PauseTiming(void);
+void HALSIM_ResumeTiming(void);
+HAL_Bool HALSIM_IsTimingPaused(void);
+void HALSIM_StepTiming(uint64_t delta);
+
+typedef int32_t (*HALSIM_SendErrorHandler)(
+ HAL_Bool isError, int32_t errorCode, HAL_Bool isLVCode, const char* details,
+ const char* location, const char* callStack, HAL_Bool printMsg);
+void HALSIM_SetSendError(HALSIM_SendErrorHandler handler);
+
+} // extern "C"
diff --git a/hal/src/main/native/include/mockdata/NotifierData.h b/hal/src/main/native/include/mockdata/NotifierData.h
new file mode 100644
index 0000000..b1ed50f
--- /dev/null
+++ b/hal/src/main/native/include/mockdata/NotifierData.h
@@ -0,0 +1,38 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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 "hal/Types.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct HALSIM_NotifierInfo {
+ HAL_NotifierHandle handle;
+ char name[64];
+ uint64_t timeout;
+ HAL_Bool running;
+};
+
+uint64_t HALSIM_GetNextNotifierTimeout(void);
+
+int32_t HALSIM_GetNumNotifiers(void);
+
+/**
+ * Gets detailed information about each notifier.
+ *
+ * @param arr array of information to be filled
+ * @param size size of arr
+ * @return Number of notifiers; note: may be larger than passed-in size
+ */
+int32_t HALSIM_GetNotifierInfo(struct HALSIM_NotifierInfo* arr, int32_t size);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
diff --git a/hal/src/main/native/include/mockdata/NotifyListener.h b/hal/src/main/native/include/mockdata/NotifyListener.h
new file mode 100644
index 0000000..a455803
--- /dev/null
+++ b/hal/src/main/native/include/mockdata/NotifyListener.h
@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "hal/Value.h"
+
+typedef void (*HAL_NotifyCallback)(const char* name, void* param,
+ const struct HAL_Value* value);
+
+typedef void (*HAL_BufferCallback)(const char* name, void* param,
+ unsigned char* buffer, unsigned int count);
+
+typedef void (*HAL_ConstBufferCallback)(const char* name, void* param,
+ const unsigned char* buffer,
+ unsigned int count);
+
+#ifdef __cplusplus
+
+namespace hal {
+
+template <typename CallbackFunction>
+struct HalCallbackListener {
+ HalCallbackListener() = default;
+ HalCallbackListener(void* param_, CallbackFunction callback_)
+ : callback(callback_), param(param_) {}
+
+ explicit operator bool() const { return callback != nullptr; }
+
+ CallbackFunction callback;
+ void* param;
+};
+
+} // namespace hal
+
+#endif
diff --git a/hal/src/main/native/include/mockdata/PCMData.h b/hal/src/main/native/include/mockdata/PCMData.h
new file mode 100644
index 0000000..66b1ec2
--- /dev/null
+++ b/hal/src/main/native/include/mockdata/PCMData.h
@@ -0,0 +1,92 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "NotifyListener.h"
+#include "hal/Types.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void HALSIM_ResetPCMData(int32_t index);
+int32_t HALSIM_RegisterPCMSolenoidInitializedCallback(
+ int32_t index, int32_t channel, HAL_NotifyCallback callback, void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelPCMSolenoidInitializedCallback(int32_t index, int32_t channel,
+ int32_t uid);
+HAL_Bool HALSIM_GetPCMSolenoidInitialized(int32_t index, int32_t channel);
+void HALSIM_SetPCMSolenoidInitialized(int32_t index, int32_t channel,
+ HAL_Bool solenoidInitialized);
+
+int32_t HALSIM_RegisterPCMSolenoidOutputCallback(int32_t index, int32_t channel,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelPCMSolenoidOutputCallback(int32_t index, int32_t channel,
+ int32_t uid);
+HAL_Bool HALSIM_GetPCMSolenoidOutput(int32_t index, int32_t channel);
+void HALSIM_SetPCMSolenoidOutput(int32_t index, int32_t channel,
+ HAL_Bool solenoidOutput);
+
+int32_t HALSIM_RegisterPCMCompressorInitializedCallback(
+ int32_t index, HAL_NotifyCallback callback, void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelPCMCompressorInitializedCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetPCMCompressorInitialized(int32_t index);
+void HALSIM_SetPCMCompressorInitialized(int32_t index,
+ HAL_Bool compressorInitialized);
+
+int32_t HALSIM_RegisterPCMCompressorOnCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelPCMCompressorOnCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetPCMCompressorOn(int32_t index);
+void HALSIM_SetPCMCompressorOn(int32_t index, HAL_Bool compressorOn);
+
+int32_t HALSIM_RegisterPCMClosedLoopEnabledCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelPCMClosedLoopEnabledCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetPCMClosedLoopEnabled(int32_t index);
+void HALSIM_SetPCMClosedLoopEnabled(int32_t index, HAL_Bool closedLoopEnabled);
+
+int32_t HALSIM_RegisterPCMPressureSwitchCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelPCMPressureSwitchCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetPCMPressureSwitch(int32_t index);
+void HALSIM_SetPCMPressureSwitch(int32_t index, HAL_Bool pressureSwitch);
+
+int32_t HALSIM_RegisterPCMCompressorCurrentCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelPCMCompressorCurrentCallback(int32_t index, int32_t uid);
+double HALSIM_GetPCMCompressorCurrent(int32_t index);
+void HALSIM_SetPCMCompressorCurrent(int32_t index, double compressorCurrent);
+
+void HALSIM_GetPCMAllSolenoids(int32_t index, uint8_t* values);
+void HALSIM_SetPCMAllSolenoids(int32_t index, uint8_t values);
+
+void HALSIM_RegisterPCMAllNonSolenoidCallbacks(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+
+void HALSIM_RegisterPCMAllSolenoidCallbacks(int32_t index, int32_t channel,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
diff --git a/hal/src/main/native/include/mockdata/PDPData.h b/hal/src/main/native/include/mockdata/PDPData.h
new file mode 100644
index 0000000..8315e3c
--- /dev/null
+++ b/hal/src/main/native/include/mockdata/PDPData.h
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "NotifyListener.h"
+#include "hal/Types.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void HALSIM_ResetPDPData(int32_t index);
+int32_t HALSIM_RegisterPDPInitializedCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelPDPInitializedCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetPDPInitialized(int32_t index);
+void HALSIM_SetPDPInitialized(int32_t index, HAL_Bool initialized);
+
+int32_t HALSIM_RegisterPDPTemperatureCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelPDPTemperatureCallback(int32_t index, int32_t uid);
+double HALSIM_GetPDPTemperature(int32_t index);
+void HALSIM_SetPDPTemperature(int32_t index, double temperature);
+
+int32_t HALSIM_RegisterPDPVoltageCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param, HAL_Bool initialNotify);
+void HALSIM_CancelPDPVoltageCallback(int32_t index, int32_t uid);
+double HALSIM_GetPDPVoltage(int32_t index);
+void HALSIM_SetPDPVoltage(int32_t index, double voltage);
+
+int32_t HALSIM_RegisterPDPCurrentCallback(int32_t index, int32_t channel,
+ HAL_NotifyCallback callback,
+ void* param, HAL_Bool initialNotify);
+void HALSIM_CancelPDPCurrentCallback(int32_t index, int32_t channel,
+ int32_t uid);
+double HALSIM_GetPDPCurrent(int32_t index, int32_t channel);
+void HALSIM_SetPDPCurrent(int32_t index, int32_t channel, double current);
+
+void HALSIM_GetPDPAllCurrents(int32_t index, double* currents);
+void HALSIM_SetPDPAllCurrents(int32_t index, const double* currents);
+
+void HALSIM_RegisterPDPAllNonCurrentCallbacks(int32_t index, int32_t channel,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
diff --git a/hal/src/main/native/include/mockdata/PWMData.h b/hal/src/main/native/include/mockdata/PWMData.h
new file mode 100644
index 0000000..2a8c63d
--- /dev/null
+++ b/hal/src/main/native/include/mockdata/PWMData.h
@@ -0,0 +1,68 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "NotifyListener.h"
+#include "hal/Types.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void HALSIM_ResetPWMData(int32_t index);
+int32_t HALSIM_RegisterPWMInitializedCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelPWMInitializedCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetPWMInitialized(int32_t index);
+void HALSIM_SetPWMInitialized(int32_t index, HAL_Bool initialized);
+
+int32_t HALSIM_RegisterPWMRawValueCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param, HAL_Bool initialNotify);
+void HALSIM_CancelPWMRawValueCallback(int32_t index, int32_t uid);
+int32_t HALSIM_GetPWMRawValue(int32_t index);
+void HALSIM_SetPWMRawValue(int32_t index, int32_t rawValue);
+
+int32_t HALSIM_RegisterPWMSpeedCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param, HAL_Bool initialNotify);
+void HALSIM_CancelPWMSpeedCallback(int32_t index, int32_t uid);
+double HALSIM_GetPWMSpeed(int32_t index);
+void HALSIM_SetPWMSpeed(int32_t index, double speed);
+
+int32_t HALSIM_RegisterPWMPositionCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param, HAL_Bool initialNotify);
+void HALSIM_CancelPWMPositionCallback(int32_t index, int32_t uid);
+double HALSIM_GetPWMPosition(int32_t index);
+void HALSIM_SetPWMPosition(int32_t index, double position);
+
+int32_t HALSIM_RegisterPWMPeriodScaleCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelPWMPeriodScaleCallback(int32_t index, int32_t uid);
+int32_t HALSIM_GetPWMPeriodScale(int32_t index);
+void HALSIM_SetPWMPeriodScale(int32_t index, int32_t periodScale);
+
+int32_t HALSIM_RegisterPWMZeroLatchCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelPWMZeroLatchCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetPWMZeroLatch(int32_t index);
+void HALSIM_SetPWMZeroLatch(int32_t index, HAL_Bool zeroLatch);
+
+void HALSIM_RegisterPWMAllCallbacks(int32_t index, HAL_NotifyCallback callback,
+ void* param, HAL_Bool initialNotify);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
diff --git a/hal/src/main/native/include/mockdata/RelayData.h b/hal/src/main/native/include/mockdata/RelayData.h
new file mode 100644
index 0000000..c0b853d
--- /dev/null
+++ b/hal/src/main/native/include/mockdata/RelayData.h
@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "NotifyListener.h"
+#include "hal/Types.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void HALSIM_ResetRelayData(int32_t index);
+int32_t HALSIM_RegisterRelayInitializedForwardCallback(
+ int32_t index, HAL_NotifyCallback callback, void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelRelayInitializedForwardCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetRelayInitializedForward(int32_t index);
+void HALSIM_SetRelayInitializedForward(int32_t index,
+ HAL_Bool initializedForward);
+
+int32_t HALSIM_RegisterRelayInitializedReverseCallback(
+ int32_t index, HAL_NotifyCallback callback, void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelRelayInitializedReverseCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetRelayInitializedReverse(int32_t index);
+void HALSIM_SetRelayInitializedReverse(int32_t index,
+ HAL_Bool initializedReverse);
+
+int32_t HALSIM_RegisterRelayForwardCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelRelayForwardCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetRelayForward(int32_t index);
+void HALSIM_SetRelayForward(int32_t index, HAL_Bool forward);
+
+int32_t HALSIM_RegisterRelayReverseCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelRelayReverseCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetRelayReverse(int32_t index);
+void HALSIM_SetRelayReverse(int32_t index, HAL_Bool reverse);
+
+void HALSIM_RegisterRelayAllCallbacks(int32_t index,
+ HAL_NotifyCallback callback, void* param,
+ HAL_Bool initialNotify);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
diff --git a/hal/src/main/native/include/mockdata/RoboRioData.h b/hal/src/main/native/include/mockdata/RoboRioData.h
new file mode 100644
index 0000000..3acb0ec
--- /dev/null
+++ b/hal/src/main/native/include/mockdata/RoboRioData.h
@@ -0,0 +1,142 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "NotifyListener.h"
+#include "hal/Types.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void HALSIM_ResetRoboRioData(int32_t index);
+int32_t HALSIM_RegisterRoboRioFPGAButtonCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelRoboRioFPGAButtonCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetRoboRioFPGAButton(int32_t index);
+void HALSIM_SetRoboRioFPGAButton(int32_t index, HAL_Bool fPGAButton);
+
+int32_t HALSIM_RegisterRoboRioVInVoltageCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelRoboRioVInVoltageCallback(int32_t index, int32_t uid);
+double HALSIM_GetRoboRioVInVoltage(int32_t index);
+void HALSIM_SetRoboRioVInVoltage(int32_t index, double vInVoltage);
+
+int32_t HALSIM_RegisterRoboRioVInCurrentCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelRoboRioVInCurrentCallback(int32_t index, int32_t uid);
+double HALSIM_GetRoboRioVInCurrent(int32_t index);
+void HALSIM_SetRoboRioVInCurrent(int32_t index, double vInCurrent);
+
+int32_t HALSIM_RegisterRoboRioUserVoltage6VCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelRoboRioUserVoltage6VCallback(int32_t index, int32_t uid);
+double HALSIM_GetRoboRioUserVoltage6V(int32_t index);
+void HALSIM_SetRoboRioUserVoltage6V(int32_t index, double userVoltage6V);
+
+int32_t HALSIM_RegisterRoboRioUserCurrent6VCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelRoboRioUserCurrent6VCallback(int32_t index, int32_t uid);
+double HALSIM_GetRoboRioUserCurrent6V(int32_t index);
+void HALSIM_SetRoboRioUserCurrent6V(int32_t index, double userCurrent6V);
+
+int32_t HALSIM_RegisterRoboRioUserActive6VCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelRoboRioUserActive6VCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetRoboRioUserActive6V(int32_t index);
+void HALSIM_SetRoboRioUserActive6V(int32_t index, HAL_Bool userActive6V);
+
+int32_t HALSIM_RegisterRoboRioUserVoltage5VCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelRoboRioUserVoltage5VCallback(int32_t index, int32_t uid);
+double HALSIM_GetRoboRioUserVoltage5V(int32_t index);
+void HALSIM_SetRoboRioUserVoltage5V(int32_t index, double userVoltage5V);
+
+int32_t HALSIM_RegisterRoboRioUserCurrent5VCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelRoboRioUserCurrent5VCallback(int32_t index, int32_t uid);
+double HALSIM_GetRoboRioUserCurrent5V(int32_t index);
+void HALSIM_SetRoboRioUserCurrent5V(int32_t index, double userCurrent5V);
+
+int32_t HALSIM_RegisterRoboRioUserActive5VCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelRoboRioUserActive5VCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetRoboRioUserActive5V(int32_t index);
+void HALSIM_SetRoboRioUserActive5V(int32_t index, HAL_Bool userActive5V);
+
+int32_t HALSIM_RegisterRoboRioUserVoltage3V3Callback(
+ int32_t index, HAL_NotifyCallback callback, void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelRoboRioUserVoltage3V3Callback(int32_t index, int32_t uid);
+double HALSIM_GetRoboRioUserVoltage3V3(int32_t index);
+void HALSIM_SetRoboRioUserVoltage3V3(int32_t index, double userVoltage3V3);
+
+int32_t HALSIM_RegisterRoboRioUserCurrent3V3Callback(
+ int32_t index, HAL_NotifyCallback callback, void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelRoboRioUserCurrent3V3Callback(int32_t index, int32_t uid);
+double HALSIM_GetRoboRioUserCurrent3V3(int32_t index);
+void HALSIM_SetRoboRioUserCurrent3V3(int32_t index, double userCurrent3V3);
+
+int32_t HALSIM_RegisterRoboRioUserActive3V3Callback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelRoboRioUserActive3V3Callback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetRoboRioUserActive3V3(int32_t index);
+void HALSIM_SetRoboRioUserActive3V3(int32_t index, HAL_Bool userActive3V3);
+
+int32_t HALSIM_RegisterRoboRioUserFaults6VCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelRoboRioUserFaults6VCallback(int32_t index, int32_t uid);
+int32_t HALSIM_GetRoboRioUserFaults6V(int32_t index);
+void HALSIM_SetRoboRioUserFaults6V(int32_t index, int32_t userFaults6V);
+
+int32_t HALSIM_RegisterRoboRioUserFaults5VCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelRoboRioUserFaults5VCallback(int32_t index, int32_t uid);
+int32_t HALSIM_GetRoboRioUserFaults5V(int32_t index);
+void HALSIM_SetRoboRioUserFaults5V(int32_t index, int32_t userFaults5V);
+
+int32_t HALSIM_RegisterRoboRioUserFaults3V3Callback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelRoboRioUserFaults3V3Callback(int32_t index, int32_t uid);
+int32_t HALSIM_GetRoboRioUserFaults3V3(int32_t index);
+void HALSIM_SetRoboRioUserFaults3V3(int32_t index, int32_t userFaults3V3);
+
+void HALSIM_RegisterRoboRioAllCallbacks(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param, HAL_Bool initialNotify);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
diff --git a/hal/src/main/native/include/mockdata/SPIAccelerometerData.h b/hal/src/main/native/include/mockdata/SPIAccelerometerData.h
new file mode 100644
index 0000000..c68da45
--- /dev/null
+++ b/hal/src/main/native/include/mockdata/SPIAccelerometerData.h
@@ -0,0 +1,63 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "NotifyListener.h"
+#include "hal/Types.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void HALSIM_ResetSPIAccelerometerData(int32_t index);
+int32_t HALSIM_RegisterSPIAccelerometerActiveCallback(
+ int32_t index, HAL_NotifyCallback callback, void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelSPIAccelerometerActiveCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetSPIAccelerometerActive(int32_t index);
+void HALSIM_SetSPIAccelerometerActive(int32_t index, HAL_Bool active);
+
+int32_t HALSIM_RegisterSPIAccelerometerRangeCallback(
+ int32_t index, HAL_NotifyCallback callback, void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelSPIAccelerometerRangeCallback(int32_t index, int32_t uid);
+int32_t HALSIM_GetSPIAccelerometerRange(int32_t index);
+void HALSIM_SetSPIAccelerometerRange(int32_t index, int32_t range);
+
+int32_t HALSIM_RegisterSPIAccelerometerXCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelSPIAccelerometerXCallback(int32_t index, int32_t uid);
+double HALSIM_GetSPIAccelerometerX(int32_t index);
+void HALSIM_SetSPIAccelerometerX(int32_t index, double x);
+
+int32_t HALSIM_RegisterSPIAccelerometerYCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelSPIAccelerometerYCallback(int32_t index, int32_t uid);
+double HALSIM_GetSPIAccelerometerY(int32_t index);
+void HALSIM_SetSPIAccelerometerY(int32_t index, double y);
+
+int32_t HALSIM_RegisterSPIAccelerometerZCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelSPIAccelerometerZCallback(int32_t index, int32_t uid);
+double HALSIM_GetSPIAccelerometerZ(int32_t index);
+void HALSIM_SetSPIAccelerometerZ(int32_t index, double z);
+
+void HALSIM_RegisterSPIAccelerometerAllCallbcaks(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
diff --git a/hal/src/main/native/include/mockdata/SPIData.h b/hal/src/main/native/include/mockdata/SPIData.h
new file mode 100644
index 0000000..da21643
--- /dev/null
+++ b/hal/src/main/native/include/mockdata/SPIData.h
@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "NotifyListener.h"
+#include "hal/Types.h"
+
+typedef void (*HAL_SpiReadAutoReceiveBufferCallback)(const char* name,
+ void* param,
+ uint32_t* buffer,
+ int32_t numToRead,
+ int32_t* outputCount);
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void HALSIM_ResetSPIData(int32_t index);
+
+int32_t HALSIM_RegisterSPIInitializedCallback(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+void HALSIM_CancelSPIInitializedCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetSPIInitialized(int32_t index);
+void HALSIM_SetSPIInitialized(int32_t index, HAL_Bool initialized);
+
+int32_t HALSIM_RegisterSPIReadCallback(int32_t index,
+ HAL_BufferCallback callback,
+ void* param);
+void HALSIM_CancelSPIReadCallback(int32_t index, int32_t uid);
+
+int32_t HALSIM_RegisterSPIWriteCallback(int32_t index,
+ HAL_ConstBufferCallback callback,
+ void* param);
+void HALSIM_CancelSPIWriteCallback(int32_t index, int32_t uid);
+
+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"
+#endif
diff --git a/hal/src/main/native/include/mockdata/SimCallbackRegistry.h b/hal/src/main/native/include/mockdata/SimCallbackRegistry.h
new file mode 100644
index 0000000..3e1aeb0
--- /dev/null
+++ b/hal/src/main/native/include/mockdata/SimCallbackRegistry.h
@@ -0,0 +1,155 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 <memory>
+#include <utility>
+
+#include <wpi/Compiler.h>
+#include <wpi/UidVector.h>
+#include <wpi/spinlock.h>
+
+#include "mockdata/NotifyListener.h"
+
+namespace hal {
+
+namespace impl {
+
+class SimCallbackRegistryBase {
+ public:
+ using RawFunctor = void (*)();
+
+ protected:
+ using CallbackVector = wpi::UidVector<HalCallbackListener<RawFunctor>, 4>;
+
+ public:
+ void Cancel(int32_t uid) {
+ std::scoped_lock lock(m_mutex);
+ if (m_callbacks) m_callbacks->erase(uid - 1);
+ }
+
+ void Reset() {
+ std::scoped_lock lock(m_mutex);
+ DoReset();
+ }
+
+ wpi::recursive_spinlock& GetMutex() { return m_mutex; }
+
+ protected:
+ int32_t DoRegister(RawFunctor callback, void* param) {
+ // Must return -1 on a null callback for error handling
+ if (callback == nullptr) return -1;
+ if (!m_callbacks) m_callbacks = std::make_unique<CallbackVector>();
+ return m_callbacks->emplace_back(param, callback) + 1;
+ }
+
+ LLVM_ATTRIBUTE_ALWAYS_INLINE void DoReset() {
+ if (m_callbacks) m_callbacks->clear();
+ }
+
+ mutable wpi::recursive_spinlock m_mutex;
+ std::unique_ptr<CallbackVector> m_callbacks;
+};
+
+} // namespace impl
+
+/**
+ * Simulation callback registry. Provides callback functionality.
+ *
+ * @tparam CallbackFunction callback function type (e.g. HAL_BufferCallback)
+ * @tparam GetName function that returns a const char* for the name
+ */
+template <typename CallbackFunction, const char* (*GetName)()>
+class SimCallbackRegistry : public impl::SimCallbackRegistryBase {
+ public:
+ int32_t Register(CallbackFunction callback, void* param) {
+ std::scoped_lock lock(m_mutex);
+ return DoRegister(reinterpret_cast<RawFunctor>(callback), param);
+ }
+
+ template <typename... U>
+ void Invoke(U&&... u) const {
+#ifdef _MSC_VER // work around VS2019 16.4.0 bug
+ std::scoped_lock<wpi::recursive_spinlock> lock(m_mutex);
+#else
+ std::scoped_lock lock(m_mutex);
+#endif
+ if (m_callbacks) {
+ const char* name = GetName();
+ for (auto&& cb : *m_callbacks)
+ reinterpret_cast<CallbackFunction>(cb.callback)(name, cb.param,
+ std::forward<U>(u)...);
+ }
+ }
+
+ template <typename... U>
+ LLVM_ATTRIBUTE_ALWAYS_INLINE void operator()(U&&... u) const {
+ return Invoke(std::forward<U>(u)...);
+ }
+};
+
+/**
+ * Define a name functor for use with SimCallbackRegistry.
+ * This creates a function named GetNAMEName() that returns "NAME".
+ * @param NAME the name to return
+ */
+#define HAL_SIMCALLBACKREGISTRY_DEFINE_NAME(NAME) \
+ static LLVM_ATTRIBUTE_ALWAYS_INLINE constexpr const char* \
+ Get##NAME##Name() { \
+ return #NAME; \
+ }
+
+/**
+ * Define a standard C API for SimCallbackRegistry.
+ *
+ * Functions defined:
+ * - int32 NS_RegisterCAPINAMECallback(
+ * int32_t index, TYPE callback, void* param)
+ * - void NS_CancelCAPINAMECallback(int32_t index, int32_t uid)
+ *
+ * @param TYPE the underlying callback type (e.g. HAL_BufferCallback)
+ * @param NS the "namespace" (e.g. HALSIM)
+ * @param CAPINAME the C API name (usually first letter capitalized)
+ * @param DATA the backing data array
+ * @param LOWERNAME the lowercase name of the backing data registry
+ */
+#define HAL_SIMCALLBACKREGISTRY_DEFINE_CAPI(TYPE, NS, CAPINAME, DATA, \
+ LOWERNAME) \
+ int32_t NS##_Register##CAPINAME##Callback(int32_t index, TYPE callback, \
+ void* param) { \
+ return DATA[index].LOWERNAME.Register(callback, param); \
+ } \
+ \
+ void NS##_Cancel##CAPINAME##Callback(int32_t index, int32_t uid) { \
+ DATA[index].LOWERNAME.Cancel(uid); \
+ }
+
+/**
+ * Define a standard C API for SimCallbackRegistry (no index variant).
+ *
+ * Functions defined:
+ * - int32 NS_RegisterCAPINAMECallback(TYPE callback, void* param)
+ * - void NS_CancelCAPINAMECallback(int32_t uid)
+ *
+ * @param TYPE the underlying callback type (e.g. HAL_BufferCallback)
+ * @param NS the "namespace" (e.g. HALSIM)
+ * @param CAPINAME the C API name (usually first letter capitalized)
+ * @param DATA the backing data pointer
+ * @param LOWERNAME the lowercase name of the backing data registry
+ */
+#define HAL_SIMCALLBACKREGISTRY_DEFINE_CAPI_NOINDEX(TYPE, NS, CAPINAME, DATA, \
+ LOWERNAME) \
+ int32_t NS##_Register##CAPINAME##Callback(TYPE callback, void* param) { \
+ return DATA->LOWERNAME.Register(callback, param); \
+ } \
+ \
+ void NS##_Cancel##CAPINAME##Callback(int32_t uid) { \
+ DATA->LOWERNAME.Cancel(uid); \
+ }
+
+} // namespace hal
diff --git a/hal/src/main/native/include/mockdata/SimDataValue.h b/hal/src/main/native/include/mockdata/SimDataValue.h
new file mode 100644
index 0000000..b6723bb
--- /dev/null
+++ b/hal/src/main/native/include/mockdata/SimDataValue.h
@@ -0,0 +1,227 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 <memory>
+
+#include <wpi/Compiler.h>
+#include <wpi/UidVector.h>
+#include <wpi/spinlock.h>
+
+#include "mockdata/NotifyListener.h"
+#include "mockdata/SimCallbackRegistry.h"
+
+namespace hal {
+
+namespace impl {
+template <typename T, HAL_Value (*MakeValue)(T)>
+class SimDataValueBase : protected SimCallbackRegistryBase {
+ public:
+ explicit SimDataValueBase(T value) : m_value(value) {}
+
+ LLVM_ATTRIBUTE_ALWAYS_INLINE void CancelCallback(int32_t uid) { Cancel(uid); }
+
+ T Get() const {
+ std::scoped_lock lock(m_mutex);
+ return m_value;
+ }
+
+ LLVM_ATTRIBUTE_ALWAYS_INLINE operator T() const { return Get(); }
+
+ void Reset(T value) {
+ std::scoped_lock lock(m_mutex);
+ DoReset();
+ m_value = value;
+ }
+
+ wpi::recursive_spinlock& GetMutex() { return m_mutex; }
+
+ protected:
+ int32_t DoRegisterCallback(HAL_NotifyCallback callback, void* param,
+ HAL_Bool initialNotify, const char* name) {
+ std::unique_lock lock(m_mutex);
+ int32_t newUid = DoRegister(reinterpret_cast<RawFunctor>(callback), param);
+ if (newUid == -1) return -1;
+ if (initialNotify) {
+ // We know that the callback is not null because of earlier null check
+ HAL_Value value = MakeValue(m_value);
+ lock.unlock();
+ callback(name, param, &value);
+ }
+ return newUid + 1;
+ }
+
+ void DoSet(T value, const char* name) {
+ std::scoped_lock lock(this->m_mutex);
+ if (m_value != value) {
+ m_value = value;
+ if (m_callbacks) {
+ HAL_Value halValue = MakeValue(value);
+ for (auto&& cb : *m_callbacks)
+ reinterpret_cast<HAL_NotifyCallback>(cb.callback)(name, cb.param,
+ &halValue);
+ }
+ }
+ }
+
+ T m_value;
+};
+} // namespace impl
+
+/**
+ * Simulation data value wrapper. Provides callback functionality when the
+ * data value is changed.
+ *
+ * @tparam T value type (e.g. double)
+ * @tparam MakeValue function that takes a T and returns a HAL_Value
+ * @tparam GetName function that returns a const char* for the name
+ * @tparam GetDefault function that returns the default T (used only for
+ * default constructor)
+ */
+template <typename T, HAL_Value (*MakeValue)(T), const char* (*GetName)(),
+ T (*GetDefault)() = nullptr>
+class SimDataValue final : public impl::SimDataValueBase<T, MakeValue> {
+ public:
+ SimDataValue()
+ : impl::SimDataValueBase<T, MakeValue>(
+ GetDefault != nullptr ? GetDefault() : T()) {}
+ explicit SimDataValue(T value)
+ : impl::SimDataValueBase<T, MakeValue>(value) {}
+
+ LLVM_ATTRIBUTE_ALWAYS_INLINE int32_t RegisterCallback(
+ HAL_NotifyCallback callback, void* param, HAL_Bool initialNotify) {
+ return this->DoRegisterCallback(callback, param, initialNotify, GetName());
+ }
+
+ LLVM_ATTRIBUTE_ALWAYS_INLINE void Set(T value) {
+ this->DoSet(value, GetName());
+ }
+
+ LLVM_ATTRIBUTE_ALWAYS_INLINE SimDataValue& operator=(T value) {
+ Set(value);
+ return *this;
+ }
+};
+
+/**
+ * Define a name functor for use with SimDataValue.
+ * This creates a function named GetNAMEName() that returns "NAME".
+ * @param NAME the name to return
+ */
+#define HAL_SIMDATAVALUE_DEFINE_NAME(NAME) \
+ static LLVM_ATTRIBUTE_ALWAYS_INLINE constexpr const char* \
+ Get##NAME##Name() { \
+ return #NAME; \
+ }
+
+/**
+ * Define a standard C API for simulation data.
+ *
+ * Functions defined:
+ * - int32 NS_RegisterCAPINAMECallback(
+ * int32_t index, HAL_NotifyCallback callback, void* param,
+ * HAL_Bool initialNotify)
+ * - void NS_CancelCAPINAMECallback(int32_t index, int32_t uid)
+ * - TYPE NS_GetCAPINAME(int32_t index)
+ * - void NS_SetCAPINAME(int32_t index, TYPE value)
+ *
+ * @param TYPE the underlying value type (e.g. double)
+ * @param NS the "namespace" (e.g. HALSIM)
+ * @param CAPINAME the C API name (usually first letter capitalized)
+ * @param DATA the backing data array
+ * @param LOWERNAME the lowercase name of the backing data variable
+ */
+#define HAL_SIMDATAVALUE_DEFINE_CAPI(TYPE, NS, CAPINAME, DATA, LOWERNAME) \
+ int32_t NS##_Register##CAPINAME##Callback( \
+ int32_t index, HAL_NotifyCallback callback, void* param, \
+ HAL_Bool initialNotify) { \
+ return DATA[index].LOWERNAME.RegisterCallback(callback, param, \
+ initialNotify); \
+ } \
+ \
+ void NS##_Cancel##CAPINAME##Callback(int32_t index, int32_t uid) { \
+ DATA[index].LOWERNAME.CancelCallback(uid); \
+ } \
+ \
+ TYPE NS##_Get##CAPINAME(int32_t index) { return DATA[index].LOWERNAME; } \
+ \
+ void NS##_Set##CAPINAME(int32_t index, TYPE LOWERNAME) { \
+ DATA[index].LOWERNAME = LOWERNAME; \
+ }
+
+/**
+ * Define a standard C API for simulation data (channel variant).
+ *
+ * Functions defined:
+ * - int32 NS_RegisterCAPINAMECallback(
+ * int32_t index, int32_t channel, HAL_NotifyCallback callback,
+ * void* param, HAL_Bool initialNotify)
+ * - void NS_CancelCAPINAMECallback(int32_t index, int32_t channel, int32_t uid)
+ * - TYPE NS_GetCAPINAME(int32_t index, int32_t channel)
+ * - void NS_SetCAPINAME(int32_t index, int32_t channel, TYPE value)
+ *
+ * @param TYPE the underlying value type (e.g. double)
+ * @param NS the "namespace" (e.g. HALSIM)
+ * @param CAPINAME the C API name (usually first letter capitalized)
+ * @param DATA the backing data array
+ * @param LOWERNAME the lowercase name of the backing data variable array
+ */
+#define HAL_SIMDATAVALUE_DEFINE_CAPI_CHANNEL(TYPE, NS, CAPINAME, DATA, \
+ LOWERNAME) \
+ int32_t NS##_Register##CAPINAME##Callback( \
+ int32_t index, int32_t channel, HAL_NotifyCallback callback, \
+ void* param, HAL_Bool initialNotify) { \
+ return DATA[index].LOWERNAME[channel].RegisterCallback(callback, param, \
+ initialNotify); \
+ } \
+ \
+ void NS##_Cancel##CAPINAME##Callback(int32_t index, int32_t channel, \
+ int32_t uid) { \
+ DATA[index].LOWERNAME[channel].CancelCallback(uid); \
+ } \
+ \
+ TYPE NS##_Get##CAPINAME(int32_t index, int32_t channel) { \
+ return DATA[index].LOWERNAME[channel]; \
+ } \
+ \
+ void NS##_Set##CAPINAME(int32_t index, int32_t channel, TYPE LOWERNAME) { \
+ DATA[index].LOWERNAME[channel] = LOWERNAME; \
+ }
+
+/**
+ * Define a standard C API for simulation data (no index variant).
+ *
+ * Functions defined:
+ * - int32 NS_RegisterCAPINAMECallback(
+ * HAL_NotifyCallback callback, void* param, HAL_Bool initialNotify)
+ * - void NS_CancelCAPINAMECallback(int32_t uid)
+ * - TYPE NS_GetCAPINAME(void)
+ * - void NS_SetCAPINAME(TYPE value)
+ *
+ * @param TYPE the underlying value type (e.g. double)
+ * @param NS the "namespace" (e.g. HALSIM)
+ * @param CAPINAME the C API name (usually first letter capitalized)
+ * @param DATA the backing data pointer
+ * @param LOWERNAME the lowercase name of the backing data variable
+ */
+#define HAL_SIMDATAVALUE_DEFINE_CAPI_NOINDEX(TYPE, NS, CAPINAME, DATA, \
+ LOWERNAME) \
+ int32_t NS##_Register##CAPINAME##Callback( \
+ HAL_NotifyCallback callback, void* param, HAL_Bool initialNotify) { \
+ return DATA->LOWERNAME.RegisterCallback(callback, param, initialNotify); \
+ } \
+ \
+ void NS##_Cancel##CAPINAME##Callback(int32_t uid) { \
+ DATA->LOWERNAME.CancelCallback(uid); \
+ } \
+ \
+ TYPE NS##_Get##CAPINAME(void) { return DATA->LOWERNAME; } \
+ \
+ void NS##_Set##CAPINAME(TYPE LOWERNAME) { DATA->LOWERNAME = LOWERNAME; }
+
+} // namespace hal
diff --git a/hal/src/main/native/include/mockdata/SimDeviceData.h b/hal/src/main/native/include/mockdata/SimDeviceData.h
new file mode 100644
index 0000000..44398d7
--- /dev/null
+++ b/hal/src/main/native/include/mockdata/SimDeviceData.h
@@ -0,0 +1,73 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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 "NotifyListener.h"
+#include "hal/Types.h"
+#include "hal/Value.h"
+
+typedef void (*HALSIM_SimDeviceCallback)(const char* name, void* param,
+ HAL_SimDeviceHandle handle);
+
+typedef void (*HALSIM_SimValueCallback)(const char* name, void* param,
+ HAL_SimValueHandle handle,
+ HAL_Bool readonly,
+ const struct HAL_Value* value);
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+int32_t HALSIM_RegisterSimDeviceCreatedCallback(
+ const char* prefix, void* param, HALSIM_SimDeviceCallback callback,
+ HAL_Bool initialNotify);
+
+void HALSIM_CancelSimDeviceCreatedCallback(int32_t uid);
+
+int32_t HALSIM_RegisterSimDeviceFreedCallback(
+ const char* prefix, void* param, HALSIM_SimDeviceCallback callback);
+
+void HALSIM_CancelSimDeviceFreedCallback(int32_t uid);
+
+HAL_SimDeviceHandle HALSIM_GetSimDeviceHandle(const char* name);
+
+const char* HALSIM_GetSimDeviceName(HAL_SimDeviceHandle handle);
+
+HAL_SimDeviceHandle HALSIM_GetSimValueDeviceHandle(HAL_SimValueHandle handle);
+
+void HALSIM_EnumerateSimDevices(const char* prefix, void* param,
+ HALSIM_SimDeviceCallback callback);
+
+int32_t HALSIM_RegisterSimValueCreatedCallback(HAL_SimDeviceHandle device,
+ void* param,
+ HALSIM_SimValueCallback callback,
+ HAL_Bool initialNotify);
+
+void HALSIM_CancelSimValueCreatedCallback(int32_t uid);
+
+int32_t HALSIM_RegisterSimValueChangedCallback(HAL_SimValueHandle handle,
+ void* param,
+ HALSIM_SimValueCallback callback,
+ HAL_Bool initialNotify);
+
+void HALSIM_CancelSimValueChangedCallback(int32_t uid);
+
+HAL_SimValueHandle HALSIM_GetSimValueHandle(HAL_SimDeviceHandle device,
+ const char* name);
+
+void HALSIM_EnumerateSimValues(HAL_SimDeviceHandle device, void* param,
+ HALSIM_SimValueCallback callback);
+
+const char** HALSIM_GetSimValueEnumOptions(HAL_SimValueHandle handle,
+ int32_t* numOptions);
+
+void HALSIM_ResetSimDeviceData(void);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
diff --git a/hal/src/main/native/include/simulation/AccelerometerSim.h b/hal/src/main/native/include/simulation/AccelerometerSim.h
new file mode 100644
index 0000000..07860f0
--- /dev/null
+++ b/hal/src/main/native/include/simulation/AccelerometerSim.h
@@ -0,0 +1,99 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 <memory>
+#include <utility>
+
+#include "CallbackStore.h"
+#include "mockdata/AccelerometerData.h"
+
+namespace frc {
+namespace sim {
+class AccelerometerSim {
+ public:
+ explicit AccelerometerSim(int index) { m_index = index; }
+
+ std::unique_ptr<CallbackStore> RegisterActiveCallback(NotifyCallback callback,
+ bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelAccelerometerActiveCallback);
+ store->SetUid(HALSIM_RegisterAccelerometerActiveCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ bool GetActive() const { return HALSIM_GetAccelerometerActive(m_index); }
+
+ void SetActive(bool active) {
+ HALSIM_SetAccelerometerActive(m_index, active);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterRangeCallback(NotifyCallback callback,
+ bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelAccelerometerRangeCallback);
+ store->SetUid(HALSIM_RegisterAccelerometerRangeCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ HAL_AccelerometerRange GetRange() const {
+ return HALSIM_GetAccelerometerRange(m_index);
+ }
+
+ void SetRange(HAL_AccelerometerRange range) {
+ HALSIM_SetAccelerometerRange(m_index, range);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterXCallback(NotifyCallback callback,
+ bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelAccelerometerXCallback);
+ store->SetUid(HALSIM_RegisterAccelerometerXCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ double GetX() const { return HALSIM_GetAccelerometerX(m_index); }
+
+ void SetX(double x) { HALSIM_SetAccelerometerX(m_index, x); }
+
+ std::unique_ptr<CallbackStore> RegisterYCallback(NotifyCallback callback,
+ bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelAccelerometerYCallback);
+ store->SetUid(HALSIM_RegisterAccelerometerYCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ double GetY() const { return HALSIM_GetAccelerometerY(m_index); }
+
+ void SetY(double y) { HALSIM_SetAccelerometerY(m_index, y); }
+
+ std::unique_ptr<CallbackStore> RegisterZCallback(NotifyCallback callback,
+ bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelAccelerometerZCallback);
+ store->SetUid(HALSIM_RegisterAccelerometerZCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ double GetZ() const { return HALSIM_GetAccelerometerZ(m_index); }
+
+ void SetZ(double z) { HALSIM_SetAccelerometerZ(m_index, z); }
+
+ void ResetData() { HALSIM_ResetAccelerometerData(m_index); }
+
+ private:
+ int m_index;
+};
+} // namespace sim
+} // namespace frc
diff --git a/hal/src/main/native/include/simulation/AnalogGyroSim.h b/hal/src/main/native/include/simulation/AnalogGyroSim.h
new file mode 100644
index 0000000..b2cdb85
--- /dev/null
+++ b/hal/src/main/native/include/simulation/AnalogGyroSim.h
@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 <memory>
+#include <utility>
+
+#include "CallbackStore.h"
+#include "mockdata/AnalogGyroData.h"
+
+namespace frc {
+namespace sim {
+class AnalogGyroSim {
+ public:
+ explicit AnalogGyroSim(int index) { m_index = index; }
+
+ std::unique_ptr<CallbackStore> RegisterAngleCallback(NotifyCallback callback,
+ bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelAnalogGyroAngleCallback);
+ store->SetUid(HALSIM_RegisterAnalogGyroAngleCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ double GetAngle() const { return HALSIM_GetAnalogGyroAngle(m_index); }
+
+ void SetAngle(double angle) { HALSIM_SetAnalogGyroAngle(m_index, angle); }
+
+ std::unique_ptr<CallbackStore> RegisterRateCallback(NotifyCallback callback,
+ bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelAnalogGyroRateCallback);
+ store->SetUid(HALSIM_RegisterAnalogGyroRateCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ double GetRate() const { return HALSIM_GetAnalogGyroRate(m_index); }
+
+ void SetRate(double rate) { HALSIM_SetAnalogGyroRate(m_index, rate); }
+
+ std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelAnalogGyroInitializedCallback);
+ store->SetUid(HALSIM_RegisterAnalogGyroInitializedCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ bool GetInitialized() const {
+ return HALSIM_GetAnalogGyroInitialized(m_index);
+ }
+
+ void SetInitialized(bool initialized) {
+ HALSIM_SetAnalogGyroInitialized(m_index, initialized);
+ }
+
+ void ResetData() { HALSIM_ResetAnalogGyroData(m_index); }
+
+ private:
+ int m_index;
+};
+} // namespace sim
+} // namespace frc
diff --git a/hal/src/main/native/include/simulation/AnalogInSim.h b/hal/src/main/native/include/simulation/AnalogInSim.h
new file mode 100644
index 0000000..2a1bbea
--- /dev/null
+++ b/hal/src/main/native/include/simulation/AnalogInSim.h
@@ -0,0 +1,177 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 <memory>
+#include <utility>
+
+#include "CallbackStore.h"
+#include "mockdata/AnalogInData.h"
+
+namespace frc {
+namespace sim {
+class AnalogInSim {
+ public:
+ explicit AnalogInSim(int index) { m_index = index; }
+
+ std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelAnalogInInitializedCallback);
+ store->SetUid(HALSIM_RegisterAnalogInInitializedCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ bool GetInitialized() const { return HALSIM_GetAnalogInInitialized(m_index); }
+
+ void SetInitialized(bool initialized) {
+ HALSIM_SetAnalogInInitialized(m_index, initialized);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterAverageBitsCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelAnalogInAverageBitsCallback);
+ store->SetUid(HALSIM_RegisterAnalogInAverageBitsCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ int GetAverageBits() const { return HALSIM_GetAnalogInAverageBits(m_index); }
+
+ void SetAverageBits(int averageBits) {
+ HALSIM_SetAnalogInAverageBits(m_index, averageBits);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterOversampleBitsCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelAnalogInOversampleBitsCallback);
+ store->SetUid(HALSIM_RegisterAnalogInOversampleBitsCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ int GetOversampleBits() const {
+ return HALSIM_GetAnalogInOversampleBits(m_index);
+ }
+
+ void SetOversampleBits(int oversampleBits) {
+ HALSIM_SetAnalogInOversampleBits(m_index, oversampleBits);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterVoltageCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelAnalogInVoltageCallback);
+ store->SetUid(HALSIM_RegisterAnalogInVoltageCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ double GetVoltage() const { return HALSIM_GetAnalogInVoltage(m_index); }
+
+ void SetVoltage(double voltage) {
+ HALSIM_SetAnalogInVoltage(m_index, voltage);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterAccumulatorInitializedCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback,
+ &HALSIM_CancelAnalogInAccumulatorInitializedCallback);
+ store->SetUid(HALSIM_RegisterAnalogInAccumulatorInitializedCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ bool GetAccumulatorInitialized() const {
+ return HALSIM_GetAnalogInAccumulatorInitialized(m_index);
+ }
+
+ void SetAccumulatorInitialized(bool accumulatorInitialized) {
+ HALSIM_SetAnalogInAccumulatorInitialized(m_index, accumulatorInitialized);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterAccumulatorValueCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelAnalogInAccumulatorValueCallback);
+ store->SetUid(HALSIM_RegisterAnalogInAccumulatorValueCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ int64_t GetAccumulatorValue() const {
+ return HALSIM_GetAnalogInAccumulatorValue(m_index);
+ }
+
+ void SetAccumulatorValue(int64_t accumulatorValue) {
+ HALSIM_SetAnalogInAccumulatorValue(m_index, accumulatorValue);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterAccumulatorCountCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelAnalogInAccumulatorCountCallback);
+ store->SetUid(HALSIM_RegisterAnalogInAccumulatorCountCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ int64_t GetAccumulatorCount() const {
+ return HALSIM_GetAnalogInAccumulatorCount(m_index);
+ }
+
+ void SetAccumulatorCount(int64_t accumulatorCount) {
+ HALSIM_SetAnalogInAccumulatorCount(m_index, accumulatorCount);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterAccumulatorCenterCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelAnalogInAccumulatorCenterCallback);
+ store->SetUid(HALSIM_RegisterAnalogInAccumulatorCenterCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ int GetAccumulatorCenter() const {
+ return HALSIM_GetAnalogInAccumulatorCenter(m_index);
+ }
+
+ void SetAccumulatorCenter(int accumulatorCenter) {
+ HALSIM_SetAnalogInAccumulatorCenter(m_index, accumulatorCenter);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterAccumulatorDeadbandCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback,
+ &HALSIM_CancelAnalogInAccumulatorDeadbandCallback);
+ store->SetUid(HALSIM_RegisterAnalogInAccumulatorDeadbandCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ int GetAccumulatorDeadband() const {
+ return HALSIM_GetAnalogInAccumulatorDeadband(m_index);
+ }
+
+ void SetAccumulatorDeadband(int accumulatorDeadband) {
+ HALSIM_SetAnalogInAccumulatorDeadband(m_index, accumulatorDeadband);
+ }
+
+ void ResetData() { HALSIM_ResetAnalogInData(m_index); }
+
+ private:
+ int m_index;
+};
+} // namespace sim
+} // namespace frc
diff --git a/hal/src/main/native/include/simulation/AnalogOutSim.h b/hal/src/main/native/include/simulation/AnalogOutSim.h
new file mode 100644
index 0000000..6bb6e5a
--- /dev/null
+++ b/hal/src/main/native/include/simulation/AnalogOutSim.h
@@ -0,0 +1,60 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 <memory>
+#include <utility>
+
+#include "CallbackStore.h"
+#include "mockdata/AnalogOutData.h"
+
+namespace frc {
+namespace sim {
+class AnalogOutSim {
+ public:
+ explicit AnalogOutSim(int index) { m_index = index; }
+
+ std::unique_ptr<CallbackStore> RegisterVoltageCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelAnalogOutVoltageCallback);
+ store->SetUid(HALSIM_RegisterAnalogOutVoltageCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ double GetVoltage() const { return HALSIM_GetAnalogOutVoltage(m_index); }
+
+ void SetVoltage(double voltage) {
+ HALSIM_SetAnalogOutVoltage(m_index, voltage);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelAnalogOutInitializedCallback);
+ store->SetUid(HALSIM_RegisterAnalogOutInitializedCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ bool GetInitialized() const {
+ return HALSIM_GetAnalogOutInitialized(m_index);
+ }
+
+ void SetInitialized(bool initialized) {
+ HALSIM_SetAnalogOutInitialized(m_index, initialized);
+ }
+
+ void ResetData() { HALSIM_ResetAnalogOutData(m_index); }
+
+ private:
+ int m_index;
+};
+} // namespace sim
+} // namespace frc
diff --git a/hal/src/main/native/include/simulation/AnalogTriggerSim.h b/hal/src/main/native/include/simulation/AnalogTriggerSim.h
new file mode 100644
index 0000000..2c41be1
--- /dev/null
+++ b/hal/src/main/native/include/simulation/AnalogTriggerSim.h
@@ -0,0 +1,81 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 <memory>
+#include <utility>
+
+#include "CallbackStore.h"
+#include "mockdata/AnalogTriggerData.h"
+
+namespace frc {
+namespace sim {
+class AnalogTriggerSim {
+ public:
+ explicit AnalogTriggerSim(int index) { m_index = index; }
+
+ std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelAnalogTriggerInitializedCallback);
+ store->SetUid(HALSIM_RegisterAnalogTriggerInitializedCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ bool GetInitialized() const {
+ return HALSIM_GetAnalogTriggerInitialized(m_index);
+ }
+
+ void SetInitialized(bool initialized) {
+ HALSIM_SetAnalogTriggerInitialized(m_index, initialized);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterTriggerLowerBoundCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback,
+ &HALSIM_CancelAnalogTriggerTriggerLowerBoundCallback);
+ store->SetUid(HALSIM_RegisterAnalogTriggerTriggerLowerBoundCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ double GetTriggerLowerBound() const {
+ return HALSIM_GetAnalogTriggerTriggerLowerBound(m_index);
+ }
+
+ void SetTriggerLowerBound(double triggerLowerBound) {
+ HALSIM_SetAnalogTriggerTriggerLowerBound(m_index, triggerLowerBound);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterTriggerUpperBoundCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback,
+ &HALSIM_CancelAnalogTriggerTriggerUpperBoundCallback);
+ store->SetUid(HALSIM_RegisterAnalogTriggerTriggerUpperBoundCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ double GetTriggerUpperBound() const {
+ return HALSIM_GetAnalogTriggerTriggerUpperBound(m_index);
+ }
+
+ void SetTriggerUpperBound(double triggerUpperBound) {
+ HALSIM_SetAnalogTriggerTriggerUpperBound(m_index, triggerUpperBound);
+ }
+
+ void ResetData() { HALSIM_ResetAnalogTriggerData(m_index); }
+
+ private:
+ int m_index;
+};
+} // namespace sim
+} // namespace frc
diff --git a/hal/src/main/native/include/simulation/CallbackStore.h b/hal/src/main/native/include/simulation/CallbackStore.h
new file mode 100644
index 0000000..b2d4bdf
--- /dev/null
+++ b/hal/src/main/native/include/simulation/CallbackStore.h
@@ -0,0 +1,89 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 <functional>
+
+#include <wpi/StringRef.h>
+
+#include "hal/Value.h"
+
+namespace frc {
+namespace sim {
+
+using NotifyCallback = std::function<void(wpi::StringRef, const HAL_Value*)>;
+typedef void (*CancelCallbackFunc)(int32_t index, int32_t uid);
+typedef void (*CancelCallbackNoIndexFunc)(int32_t uid);
+typedef void (*CancelCallbackChannelFunc)(int32_t index, int32_t channel,
+ int32_t uid);
+
+void CallbackStoreThunk(const char* name, void* param, const HAL_Value* value);
+
+class CallbackStore {
+ public:
+ CallbackStore(int32_t i, NotifyCallback cb, CancelCallbackNoIndexFunc ccf) {
+ index = i;
+ callback = cb;
+ this->ccnif = ccf;
+ cancelType = NoIndex;
+ }
+
+ CallbackStore(int32_t i, int32_t u, NotifyCallback cb,
+ CancelCallbackFunc ccf) {
+ index = i;
+ uid = u;
+ callback = cb;
+ this->ccf = ccf;
+ cancelType = Normal;
+ }
+
+ CallbackStore(int32_t i, int32_t c, int32_t u, NotifyCallback cb,
+ CancelCallbackChannelFunc ccf) {
+ index = i;
+ channel = c;
+ uid = u;
+ callback = cb;
+ this->cccf = ccf;
+ cancelType = Channel;
+ }
+
+ ~CallbackStore() {
+ switch (cancelType) {
+ case Normal:
+ ccf(index, uid);
+ break;
+ case Channel:
+ cccf(index, channel, uid);
+ break;
+ case NoIndex:
+ ccnif(uid);
+ break;
+ }
+ }
+
+ void SetUid(int32_t uid) { this->uid = uid; }
+
+ friend void CallbackStoreThunk(const char* name, void* param,
+ const HAL_Value* value);
+
+ private:
+ int32_t index;
+ int32_t channel;
+ int32_t uid;
+
+ NotifyCallback callback;
+ union {
+ CancelCallbackFunc ccf;
+ CancelCallbackChannelFunc cccf;
+ CancelCallbackNoIndexFunc ccnif;
+ };
+ enum CancelType { Normal, Channel, NoIndex };
+ CancelType cancelType;
+};
+} // namespace sim
+} // namespace frc
diff --git a/hal/src/main/native/include/simulation/DIOSim.h b/hal/src/main/native/include/simulation/DIOSim.h
new file mode 100644
index 0000000..57079b9
--- /dev/null
+++ b/hal/src/main/native/include/simulation/DIOSim.h
@@ -0,0 +1,99 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 <memory>
+#include <utility>
+
+#include "CallbackStore.h"
+#include "mockdata/DIOData.h"
+
+namespace frc {
+namespace sim {
+class DIOSim {
+ public:
+ explicit DIOSim(int index) { m_index = index; }
+
+ std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelDIOInitializedCallback);
+ store->SetUid(HALSIM_RegisterDIOInitializedCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ bool GetInitialized() const { return HALSIM_GetDIOInitialized(m_index); }
+
+ void SetInitialized(bool initialized) {
+ HALSIM_SetDIOInitialized(m_index, initialized);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterValueCallback(NotifyCallback callback,
+ bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelDIOValueCallback);
+ store->SetUid(HALSIM_RegisterDIOValueCallback(m_index, &CallbackStoreThunk,
+ store.get(), initialNotify));
+ return store;
+ }
+
+ bool GetValue() const { return HALSIM_GetDIOValue(m_index); }
+
+ void SetValue(bool value) { HALSIM_SetDIOValue(m_index, value); }
+
+ std::unique_ptr<CallbackStore> RegisterPulseLengthCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelDIOPulseLengthCallback);
+ store->SetUid(HALSIM_RegisterDIOPulseLengthCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ double GetPulseLength() const { return HALSIM_GetDIOPulseLength(m_index); }
+
+ void SetPulseLength(double pulseLength) {
+ HALSIM_SetDIOPulseLength(m_index, pulseLength);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterIsInputCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelDIOIsInputCallback);
+ store->SetUid(HALSIM_RegisterDIOIsInputCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ bool GetIsInput() const { return HALSIM_GetDIOIsInput(m_index); }
+
+ void SetIsInput(bool isInput) { HALSIM_SetDIOIsInput(m_index, isInput); }
+
+ std::unique_ptr<CallbackStore> RegisterFilterIndexCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelDIOFilterIndexCallback);
+ store->SetUid(HALSIM_RegisterDIOFilterIndexCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ int GetFilterIndex() const { return HALSIM_GetDIOFilterIndex(m_index); }
+
+ void SetFilterIndex(int filterIndex) {
+ HALSIM_SetDIOFilterIndex(m_index, filterIndex);
+ }
+
+ void ResetData() { HALSIM_ResetDIOData(m_index); }
+
+ private:
+ int m_index;
+};
+} // namespace sim
+} // namespace frc
diff --git a/hal/src/main/native/include/simulation/DigitalPWMSim.h b/hal/src/main/native/include/simulation/DigitalPWMSim.h
new file mode 100644
index 0000000..5f5769d
--- /dev/null
+++ b/hal/src/main/native/include/simulation/DigitalPWMSim.h
@@ -0,0 +1,73 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 <memory>
+#include <utility>
+
+#include "CallbackStore.h"
+#include "mockdata/DigitalPWMData.h"
+
+namespace frc {
+namespace sim {
+class DigitalPWMSim {
+ public:
+ explicit DigitalPWMSim(int index) { m_index = index; }
+
+ std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelDigitalPWMInitializedCallback);
+ store->SetUid(HALSIM_RegisterDigitalPWMInitializedCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ bool GetInitialized() const {
+ return HALSIM_GetDigitalPWMInitialized(m_index);
+ }
+
+ void SetInitialized(bool initialized) {
+ HALSIM_SetDigitalPWMInitialized(m_index, initialized);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterDutyCycleCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelDigitalPWMDutyCycleCallback);
+ store->SetUid(HALSIM_RegisterDigitalPWMDutyCycleCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ double GetDutyCycle() const { return HALSIM_GetDigitalPWMDutyCycle(m_index); }
+
+ void SetDutyCycle(double dutyCycle) {
+ HALSIM_SetDigitalPWMDutyCycle(m_index, dutyCycle);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterPinCallback(NotifyCallback callback,
+ bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelDigitalPWMPinCallback);
+ store->SetUid(HALSIM_RegisterDigitalPWMPinCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ int GetPin() const { return HALSIM_GetDigitalPWMPin(m_index); }
+
+ void SetPin(int pin) { HALSIM_SetDigitalPWMPin(m_index, pin); }
+
+ void ResetData() { HALSIM_ResetDigitalPWMData(m_index); }
+
+ private:
+ int m_index;
+};
+} // namespace sim
+} // namespace frc
diff --git a/hal/src/main/native/include/simulation/DriverStationSim.h b/hal/src/main/native/include/simulation/DriverStationSim.h
new file mode 100644
index 0000000..e5071cc
--- /dev/null
+++ b/hal/src/main/native/include/simulation/DriverStationSim.h
@@ -0,0 +1,109 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 <memory>
+#include <utility>
+
+#include "CallbackStore.h"
+#include "mockdata/DriverStationData.h"
+
+namespace frc {
+namespace sim {
+class DriverStationSim {
+ public:
+ std::unique_ptr<CallbackStore> RegisterEnabledCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ -1, callback, &HALSIM_CancelDriverStationEnabledCallback);
+ store->SetUid(HALSIM_RegisterDriverStationEnabledCallback(
+ &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ bool GetEnabled() const { return HALSIM_GetDriverStationEnabled(); }
+
+ void SetEnabled(bool enabled) { HALSIM_SetDriverStationEnabled(enabled); }
+
+ std::unique_ptr<CallbackStore> RegisterAutonomousCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ -1, callback, &HALSIM_CancelDriverStationAutonomousCallback);
+ store->SetUid(HALSIM_RegisterDriverStationAutonomousCallback(
+ &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ bool GetAutonomous() const { return HALSIM_GetDriverStationAutonomous(); }
+
+ void SetAutonomous(bool autonomous) {
+ HALSIM_SetDriverStationAutonomous(autonomous);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterTestCallback(NotifyCallback callback,
+ bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ -1, callback, &HALSIM_CancelDriverStationTestCallback);
+ store->SetUid(HALSIM_RegisterDriverStationTestCallback(
+ &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ bool GetTest() const { return HALSIM_GetDriverStationTest(); }
+
+ void SetTest(bool test) { HALSIM_SetDriverStationTest(test); }
+
+ std::unique_ptr<CallbackStore> RegisterEStopCallback(NotifyCallback callback,
+ bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ -1, callback, &HALSIM_CancelDriverStationEStopCallback);
+ store->SetUid(HALSIM_RegisterDriverStationEStopCallback(
+ &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ bool GetEStop() const { return HALSIM_GetDriverStationEStop(); }
+
+ void SetEStop(bool eStop) { HALSIM_SetDriverStationEStop(eStop); }
+
+ std::unique_ptr<CallbackStore> RegisterFmsAttachedCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ -1, callback, &HALSIM_CancelDriverStationFmsAttachedCallback);
+ store->SetUid(HALSIM_RegisterDriverStationFmsAttachedCallback(
+ &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ bool GetFmsAttached() const { return HALSIM_GetDriverStationFmsAttached(); }
+
+ void SetFmsAttached(bool fmsAttached) {
+ HALSIM_SetDriverStationFmsAttached(fmsAttached);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterDsAttachedCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ -1, callback, &HALSIM_CancelDriverStationDsAttachedCallback);
+ store->SetUid(HALSIM_RegisterDriverStationDsAttachedCallback(
+ &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ bool GetDsAttached() const { return HALSIM_GetDriverStationDsAttached(); }
+
+ void SetDsAttached(bool dsAttached) {
+ HALSIM_SetDriverStationDsAttached(dsAttached);
+ }
+
+ void NotifyNewData() { HALSIM_NotifyDriverStationNewData(); }
+
+ void ResetData() { HALSIM_ResetDriverStationData(); }
+};
+} // namespace sim
+} // namespace frc
diff --git a/hal/src/main/native/include/simulation/DutyCycleSim.h b/hal/src/main/native/include/simulation/DutyCycleSim.h
new file mode 100644
index 0000000..f55dfc9
--- /dev/null
+++ b/hal/src/main/native/include/simulation/DutyCycleSim.h
@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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 <memory>
+#include <utility>
+
+#include "CallbackStore.h"
+#include "mockdata/DutyCycleData.h"
+
+namespace frc {
+namespace sim {
+class DutyCycleSim {
+ public:
+ explicit DutyCycleSim(int index) { m_index = index; }
+
+ std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelDutyCycleInitializedCallback);
+ store->SetUid(HALSIM_RegisterDutyCycleInitializedCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ bool GetInitialized() const {
+ return HALSIM_GetDutyCycleInitialized(m_index);
+ }
+
+ void SetInitialized(bool initialized) {
+ HALSIM_SetDutyCycleInitialized(m_index, initialized);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterFrequencyCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelDutyCycleFrequencyCallback);
+ store->SetUid(HALSIM_RegisterDutyCycleFrequencyCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ int GetFrequency() const { return HALSIM_GetDutyCycleFrequency(m_index); }
+
+ void SetFrequency(int count) { HALSIM_SetDutyCycleFrequency(m_index, count); }
+
+ std::unique_ptr<CallbackStore> RegisterOutputCallback(NotifyCallback callback,
+ bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelDutyCycleOutputCallback);
+ store->SetUid(HALSIM_RegisterDutyCycleOutputCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ double GetOutput() const { return HALSIM_GetDutyCycleOutput(m_index); }
+
+ void SetOutput(double period) { HALSIM_SetDutyCycleOutput(m_index, period); }
+
+ void ResetData() { HALSIM_ResetDutyCycleData(m_index); }
+
+ private:
+ int m_index;
+};
+} // namespace sim
+} // namespace frc
diff --git a/hal/src/main/native/include/simulation/EncoderSim.h b/hal/src/main/native/include/simulation/EncoderSim.h
new file mode 100644
index 0000000..f80e8a8
--- /dev/null
+++ b/hal/src/main/native/include/simulation/EncoderSim.h
@@ -0,0 +1,163 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 <memory>
+#include <utility>
+
+#include "CallbackStore.h"
+#include "mockdata/EncoderData.h"
+
+namespace frc {
+namespace sim {
+class EncoderSim {
+ public:
+ explicit EncoderSim(int index) { m_index = index; }
+
+ std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelEncoderInitializedCallback);
+ store->SetUid(HALSIM_RegisterEncoderInitializedCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ bool GetInitialized() const { return HALSIM_GetEncoderInitialized(m_index); }
+
+ void SetInitialized(bool initialized) {
+ HALSIM_SetEncoderInitialized(m_index, initialized);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterCountCallback(NotifyCallback callback,
+ bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelEncoderCountCallback);
+ store->SetUid(HALSIM_RegisterEncoderCountCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ int GetCount() const { return HALSIM_GetEncoderCount(m_index); }
+
+ void SetCount(int count) { HALSIM_SetEncoderCount(m_index, count); }
+
+ std::unique_ptr<CallbackStore> RegisterPeriodCallback(NotifyCallback callback,
+ bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelEncoderPeriodCallback);
+ store->SetUid(HALSIM_RegisterEncoderPeriodCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ double GetPeriod() const { return HALSIM_GetEncoderPeriod(m_index); }
+
+ void SetPeriod(double period) { HALSIM_SetEncoderPeriod(m_index, period); }
+
+ std::unique_ptr<CallbackStore> RegisterResetCallback(NotifyCallback callback,
+ bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelEncoderResetCallback);
+ store->SetUid(HALSIM_RegisterEncoderResetCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ bool GetReset() const { return HALSIM_GetEncoderReset(m_index); }
+
+ void SetReset(bool reset) { HALSIM_SetEncoderReset(m_index, reset); }
+
+ std::unique_ptr<CallbackStore> RegisterMaxPeriodCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelEncoderMaxPeriodCallback);
+ store->SetUid(HALSIM_RegisterEncoderMaxPeriodCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ double GetMaxPeriod() const { return HALSIM_GetEncoderMaxPeriod(m_index); }
+
+ void SetMaxPeriod(double maxPeriod) {
+ HALSIM_SetEncoderMaxPeriod(m_index, maxPeriod);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterDirectionCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelEncoderDirectionCallback);
+ store->SetUid(HALSIM_RegisterEncoderDirectionCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ bool GetDirection() const { return HALSIM_GetEncoderDirection(m_index); }
+
+ void SetDirection(bool direction) {
+ HALSIM_SetEncoderDirection(m_index, direction);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterReverseDirectionCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelEncoderReverseDirectionCallback);
+ store->SetUid(HALSIM_RegisterEncoderReverseDirectionCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ bool GetReverseDirection() const {
+ return HALSIM_GetEncoderReverseDirection(m_index);
+ }
+
+ void SetReverseDirection(bool reverseDirection) {
+ HALSIM_SetEncoderReverseDirection(m_index, reverseDirection);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterSamplesToAverageCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelEncoderSamplesToAverageCallback);
+ store->SetUid(HALSIM_RegisterEncoderSamplesToAverageCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ int GetSamplesToAverage() const {
+ return HALSIM_GetEncoderSamplesToAverage(m_index);
+ }
+
+ void SetSamplesToAverage(int samplesToAverage) {
+ HALSIM_SetEncoderSamplesToAverage(m_index, samplesToAverage);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterDistancePerPulseCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelEncoderDistancePerPulseCallback);
+ store->SetUid(HALSIM_RegisterEncoderDistancePerPulseCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ double GetDistancePerPulse() const {
+ return HALSIM_GetEncoderDistancePerPulse(m_index);
+ }
+
+ void SetDistancePerPulse(double distancePerPulse) {
+ HALSIM_SetEncoderDistancePerPulse(m_index, distancePerPulse);
+ }
+
+ void ResetData() { HALSIM_ResetEncoderData(m_index); }
+
+ private:
+ int m_index;
+};
+} // namespace sim
+} // namespace frc
diff --git a/hal/src/main/native/include/simulation/PCMSim.h b/hal/src/main/native/include/simulation/PCMSim.h
new file mode 100644
index 0000000..b6fcd5a
--- /dev/null
+++ b/hal/src/main/native/include/simulation/PCMSim.h
@@ -0,0 +1,157 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 <memory>
+#include <utility>
+
+#include "CallbackStore.h"
+#include "mockdata/PCMData.h"
+
+namespace frc {
+namespace sim {
+class PCMSim {
+ public:
+ explicit PCMSim(int index) { m_index = index; }
+
+ std::unique_ptr<CallbackStore> RegisterSolenoidInitializedCallback(
+ int channel, NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, channel, -1, callback,
+ &HALSIM_CancelPCMSolenoidInitializedCallback);
+ store->SetUid(HALSIM_RegisterPCMSolenoidInitializedCallback(
+ m_index, channel, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ bool GetSolenoidInitialized(int channel) const {
+ return HALSIM_GetPCMSolenoidInitialized(m_index, channel);
+ }
+
+ void SetSolenoidInitialized(int channel, bool solenoidInitialized) {
+ HALSIM_SetPCMSolenoidInitialized(m_index, channel, solenoidInitialized);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterSolenoidOutputCallback(
+ int channel, NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, channel, -1, callback,
+ &HALSIM_CancelPCMSolenoidOutputCallback);
+ store->SetUid(HALSIM_RegisterPCMSolenoidOutputCallback(
+ m_index, channel, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ bool GetSolenoidOutput(int channel) const {
+ return HALSIM_GetPCMSolenoidOutput(m_index, channel);
+ }
+
+ void SetSolenoidOutput(int channel, bool solenoidOutput) {
+ HALSIM_SetPCMSolenoidOutput(m_index, channel, solenoidOutput);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterCompressorInitializedCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelPCMCompressorInitializedCallback);
+ store->SetUid(HALSIM_RegisterPCMCompressorInitializedCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ bool GetCompressorInitialized() const {
+ return HALSIM_GetPCMCompressorInitialized(m_index);
+ }
+
+ void SetCompressorInitialized(bool compressorInitialized) {
+ HALSIM_SetPCMCompressorInitialized(m_index, compressorInitialized);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterCompressorOnCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelPCMCompressorOnCallback);
+ store->SetUid(HALSIM_RegisterPCMCompressorOnCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ bool GetCompressorOn() const { return HALSIM_GetPCMCompressorOn(m_index); }
+
+ void SetCompressorOn(bool compressorOn) {
+ HALSIM_SetPCMCompressorOn(m_index, compressorOn);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterClosedLoopEnabledCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelPCMClosedLoopEnabledCallback);
+ store->SetUid(HALSIM_RegisterPCMClosedLoopEnabledCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ bool GetClosedLoopEnabled() const {
+ return HALSIM_GetPCMClosedLoopEnabled(m_index);
+ }
+
+ void SetClosedLoopEnabled(bool closedLoopEnabled) {
+ HALSIM_SetPCMClosedLoopEnabled(m_index, closedLoopEnabled);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterPressureSwitchCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelPCMPressureSwitchCallback);
+ store->SetUid(HALSIM_RegisterPCMPressureSwitchCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ bool GetPressureSwitch() const {
+ return HALSIM_GetPCMPressureSwitch(m_index);
+ }
+
+ void SetPressureSwitch(bool pressureSwitch) {
+ HALSIM_SetPCMPressureSwitch(m_index, pressureSwitch);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterCompressorCurrentCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelPCMCompressorCurrentCallback);
+ store->SetUid(HALSIM_RegisterPCMCompressorCurrentCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ double GetCompressorCurrent() const {
+ return HALSIM_GetPCMCompressorCurrent(m_index);
+ }
+
+ void SetCompressorCurrent(double compressorCurrent) {
+ HALSIM_SetPCMCompressorCurrent(m_index, compressorCurrent);
+ }
+
+ uint8_t GetAllSolenoidOutputs() {
+ uint8_t ret = 0;
+ HALSIM_GetPCMAllSolenoids(m_index, &ret);
+ return ret;
+ }
+
+ void SetAllSolenoidOutputs(uint8_t outputs) {
+ HALSIM_SetPCMAllSolenoids(m_index, outputs);
+ }
+
+ void ResetData() { HALSIM_ResetPCMData(m_index); }
+
+ private:
+ int m_index;
+};
+} // namespace sim
+} // namespace frc
diff --git a/hal/src/main/native/include/simulation/PDPSim.h b/hal/src/main/native/include/simulation/PDPSim.h
new file mode 100644
index 0000000..72d8233
--- /dev/null
+++ b/hal/src/main/native/include/simulation/PDPSim.h
@@ -0,0 +1,96 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 <memory>
+#include <utility>
+
+#include "CallbackStore.h"
+#include "mockdata/PDPData.h"
+
+namespace frc {
+namespace sim {
+class PDPSim {
+ public:
+ explicit PDPSim(int index) { m_index = index; }
+
+ std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelPDPInitializedCallback);
+ store->SetUid(HALSIM_RegisterPDPInitializedCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ bool GetInitialized() const { return HALSIM_GetPDPInitialized(m_index); }
+
+ void SetInitialized(bool initialized) {
+ HALSIM_SetPDPInitialized(m_index, initialized);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterTemperatureCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelPDPTemperatureCallback);
+ store->SetUid(HALSIM_RegisterPDPTemperatureCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ double GetTemperature() const { return HALSIM_GetPDPTemperature(m_index); }
+
+ void SetTemperature(double temperature) {
+ HALSIM_SetPDPTemperature(m_index, temperature);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterVoltageCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelPDPVoltageCallback);
+ store->SetUid(HALSIM_RegisterPDPVoltageCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ double GetVoltage() const { return HALSIM_GetPDPVoltage(m_index); }
+
+ void SetVoltage(double voltage) { HALSIM_SetPDPVoltage(m_index, voltage); }
+
+ std::unique_ptr<CallbackStore> RegisterCurrentCallback(
+ int channel, NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, channel, -1, callback, &HALSIM_CancelPDPCurrentCallback);
+ store->SetUid(HALSIM_RegisterPDPCurrentCallback(
+ m_index, channel, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ double GetCurrent(int channel) const {
+ return HALSIM_GetPDPCurrent(m_index, channel);
+ }
+
+ void SetCurrent(int channel, double current) {
+ HALSIM_SetPDPCurrent(m_index, channel, current);
+ }
+
+ void GetAllCurrents(double* currents) {
+ HALSIM_GetPDPAllCurrents(m_index, currents);
+ }
+
+ void SetAllCurrents(const double* currents) {
+ HALSIM_SetPDPAllCurrents(m_index, currents);
+ }
+
+ void ResetData() { HALSIM_ResetPDPData(m_index); }
+
+ private:
+ int m_index;
+};
+} // namespace sim
+} // namespace frc
diff --git a/hal/src/main/native/include/simulation/PWMSim.h b/hal/src/main/native/include/simulation/PWMSim.h
new file mode 100644
index 0000000..7aba360
--- /dev/null
+++ b/hal/src/main/native/include/simulation/PWMSim.h
@@ -0,0 +1,114 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 <memory>
+#include <utility>
+
+#include "CallbackStore.h"
+#include "mockdata/PWMData.h"
+
+namespace frc {
+namespace sim {
+class PWMSim {
+ public:
+ explicit PWMSim(int index) { m_index = index; }
+
+ std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelPWMInitializedCallback);
+ store->SetUid(HALSIM_RegisterPWMInitializedCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ bool GetInitialized() const { return HALSIM_GetPWMInitialized(m_index); }
+
+ void SetInitialized(bool initialized) {
+ HALSIM_SetPWMInitialized(m_index, initialized);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterRawValueCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelPWMRawValueCallback);
+ store->SetUid(HALSIM_RegisterPWMRawValueCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ int GetRawValue() const { return HALSIM_GetPWMRawValue(m_index); }
+
+ void SetRawValue(int rawValue) { HALSIM_SetPWMRawValue(m_index, rawValue); }
+
+ std::unique_ptr<CallbackStore> RegisterSpeedCallback(NotifyCallback callback,
+ bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelPWMSpeedCallback);
+ store->SetUid(HALSIM_RegisterPWMSpeedCallback(m_index, &CallbackStoreThunk,
+ store.get(), initialNotify));
+ return store;
+ }
+
+ double GetSpeed() const { return HALSIM_GetPWMSpeed(m_index); }
+
+ void SetSpeed(double speed) { HALSIM_SetPWMSpeed(m_index, speed); }
+
+ std::unique_ptr<CallbackStore> RegisterPositionCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelPWMPositionCallback);
+ store->SetUid(HALSIM_RegisterPWMPositionCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ double GetPosition() const { return HALSIM_GetPWMPosition(m_index); }
+
+ void SetPosition(double position) {
+ HALSIM_SetPWMPosition(m_index, position);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterPeriodScaleCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelPWMPeriodScaleCallback);
+ store->SetUid(HALSIM_RegisterPWMPeriodScaleCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ int GetPeriodScale() const { return HALSIM_GetPWMPeriodScale(m_index); }
+
+ void SetPeriodScale(int periodScale) {
+ HALSIM_SetPWMPeriodScale(m_index, periodScale);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterZeroLatchCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelPWMZeroLatchCallback);
+ store->SetUid(HALSIM_RegisterPWMZeroLatchCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ bool GetZeroLatch() const { return HALSIM_GetPWMZeroLatch(m_index); }
+
+ void SetZeroLatch(bool zeroLatch) {
+ HALSIM_SetPWMZeroLatch(m_index, zeroLatch);
+ }
+
+ void ResetData() { HALSIM_ResetPWMData(m_index); }
+
+ private:
+ int m_index;
+};
+} // namespace sim
+} // namespace frc
diff --git a/hal/src/main/native/include/simulation/RelaySim.h b/hal/src/main/native/include/simulation/RelaySim.h
new file mode 100644
index 0000000..6958a40
--- /dev/null
+++ b/hal/src/main/native/include/simulation/RelaySim.h
@@ -0,0 +1,88 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 <memory>
+#include <utility>
+
+#include "CallbackStore.h"
+#include "mockdata/RelayData.h"
+
+namespace frc {
+namespace sim {
+class RelaySim {
+ public:
+ explicit RelaySim(int index) { m_index = index; }
+
+ std::unique_ptr<CallbackStore> RegisterInitializedForwardCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelRelayInitializedForwardCallback);
+ store->SetUid(HALSIM_RegisterRelayInitializedForwardCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ bool GetInitializedForward() const {
+ return HALSIM_GetRelayInitializedForward(m_index);
+ }
+
+ void SetInitializedForward(bool initializedForward) {
+ HALSIM_SetRelayInitializedForward(m_index, initializedForward);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterInitializedReverseCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelRelayInitializedReverseCallback);
+ store->SetUid(HALSIM_RegisterRelayInitializedReverseCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ bool GetInitializedReverse() const {
+ return HALSIM_GetRelayInitializedReverse(m_index);
+ }
+
+ void SetInitializedReverse(bool initializedReverse) {
+ HALSIM_SetRelayInitializedReverse(m_index, initializedReverse);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterForwardCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelRelayForwardCallback);
+ store->SetUid(HALSIM_RegisterRelayForwardCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ bool GetForward() const { return HALSIM_GetRelayForward(m_index); }
+
+ void SetForward(bool forward) { HALSIM_SetRelayForward(m_index, forward); }
+
+ std::unique_ptr<CallbackStore> RegisterReverseCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelRelayReverseCallback);
+ store->SetUid(HALSIM_RegisterRelayReverseCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ bool GetReverse() const { return HALSIM_GetRelayReverse(m_index); }
+
+ void SetReverse(bool reverse) { HALSIM_SetRelayReverse(m_index, reverse); }
+
+ void ResetData() { HALSIM_ResetRelayData(m_index); }
+
+ private:
+ int m_index;
+};
+} // namespace sim
+} // namespace frc
diff --git a/hal/src/main/native/include/simulation/RoboRioSim.h b/hal/src/main/native/include/simulation/RoboRioSim.h
new file mode 100644
index 0000000..9b020ae
--- /dev/null
+++ b/hal/src/main/native/include/simulation/RoboRioSim.h
@@ -0,0 +1,273 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 <memory>
+#include <utility>
+
+#include "CallbackStore.h"
+#include "mockdata/RoboRioData.h"
+
+namespace frc {
+namespace sim {
+class RoboRioSim {
+ public:
+ explicit RoboRioSim(int index) { m_index = index; }
+
+ std::unique_ptr<CallbackStore> RegisterFPGAButtonCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelRoboRioFPGAButtonCallback);
+ store->SetUid(HALSIM_RegisterRoboRioFPGAButtonCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ bool GetFPGAButton() const { return HALSIM_GetRoboRioFPGAButton(m_index); }
+
+ void SetFPGAButton(bool fPGAButton) {
+ HALSIM_SetRoboRioFPGAButton(m_index, fPGAButton);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterVInVoltageCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelRoboRioVInVoltageCallback);
+ store->SetUid(HALSIM_RegisterRoboRioVInVoltageCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ double GetVInVoltage() const { return HALSIM_GetRoboRioVInVoltage(m_index); }
+
+ void SetVInVoltage(double vInVoltage) {
+ HALSIM_SetRoboRioVInVoltage(m_index, vInVoltage);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterVInCurrentCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelRoboRioVInCurrentCallback);
+ store->SetUid(HALSIM_RegisterRoboRioVInCurrentCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ double GetVInCurrent() const { return HALSIM_GetRoboRioVInCurrent(m_index); }
+
+ void SetVInCurrent(double vInCurrent) {
+ HALSIM_SetRoboRioVInCurrent(m_index, vInCurrent);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterUserVoltage6VCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelRoboRioUserVoltage6VCallback);
+ store->SetUid(HALSIM_RegisterRoboRioUserVoltage6VCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ double GetUserVoltage6V() const {
+ return HALSIM_GetRoboRioUserVoltage6V(m_index);
+ }
+
+ void SetUserVoltage6V(double userVoltage6V) {
+ HALSIM_SetRoboRioUserVoltage6V(m_index, userVoltage6V);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterUserCurrent6VCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelRoboRioUserCurrent6VCallback);
+ store->SetUid(HALSIM_RegisterRoboRioUserCurrent6VCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ double GetUserCurrent6V() const {
+ return HALSIM_GetRoboRioUserCurrent6V(m_index);
+ }
+
+ void SetUserCurrent6V(double userCurrent6V) {
+ HALSIM_SetRoboRioUserCurrent6V(m_index, userCurrent6V);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterUserActive6VCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelRoboRioUserActive6VCallback);
+ store->SetUid(HALSIM_RegisterRoboRioUserActive6VCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ bool GetUserActive6V() const {
+ return HALSIM_GetRoboRioUserActive6V(m_index);
+ }
+
+ void SetUserActive6V(bool userActive6V) {
+ HALSIM_SetRoboRioUserActive6V(m_index, userActive6V);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterUserVoltage5VCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelRoboRioUserVoltage5VCallback);
+ store->SetUid(HALSIM_RegisterRoboRioUserVoltage5VCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ double GetUserVoltage5V() const {
+ return HALSIM_GetRoboRioUserVoltage5V(m_index);
+ }
+
+ void SetUserVoltage5V(double userVoltage5V) {
+ HALSIM_SetRoboRioUserVoltage5V(m_index, userVoltage5V);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterUserCurrent5VCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelRoboRioUserCurrent5VCallback);
+ store->SetUid(HALSIM_RegisterRoboRioUserCurrent5VCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ double GetUserCurrent5V() const {
+ return HALSIM_GetRoboRioUserCurrent5V(m_index);
+ }
+
+ void SetUserCurrent5V(double userCurrent5V) {
+ HALSIM_SetRoboRioUserCurrent5V(m_index, userCurrent5V);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterUserActive5VCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelRoboRioUserActive5VCallback);
+ store->SetUid(HALSIM_RegisterRoboRioUserActive5VCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ bool GetUserActive5V() const {
+ return HALSIM_GetRoboRioUserActive5V(m_index);
+ }
+
+ void SetUserActive5V(bool userActive5V) {
+ HALSIM_SetRoboRioUserActive5V(m_index, userActive5V);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterUserVoltage3V3Callback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelRoboRioUserVoltage3V3Callback);
+ store->SetUid(HALSIM_RegisterRoboRioUserVoltage3V3Callback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ double GetUserVoltage3V3() const {
+ return HALSIM_GetRoboRioUserVoltage3V3(m_index);
+ }
+
+ void SetUserVoltage3V3(double userVoltage3V3) {
+ HALSIM_SetRoboRioUserVoltage3V3(m_index, userVoltage3V3);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterUserCurrent3V3Callback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelRoboRioUserCurrent3V3Callback);
+ store->SetUid(HALSIM_RegisterRoboRioUserCurrent3V3Callback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ double GetUserCurrent3V3() const {
+ return HALSIM_GetRoboRioUserCurrent3V3(m_index);
+ }
+
+ void SetUserCurrent3V3(double userCurrent3V3) {
+ HALSIM_SetRoboRioUserCurrent3V3(m_index, userCurrent3V3);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterUserActive3V3Callback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelRoboRioUserActive3V3Callback);
+ store->SetUid(HALSIM_RegisterRoboRioUserActive3V3Callback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ bool GetUserActive3V3() const {
+ return HALSIM_GetRoboRioUserActive3V3(m_index);
+ }
+
+ void SetUserActive3V3(bool userActive3V3) {
+ HALSIM_SetRoboRioUserActive3V3(m_index, userActive3V3);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterUserFaults6VCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelRoboRioUserFaults6VCallback);
+ store->SetUid(HALSIM_RegisterRoboRioUserFaults6VCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ int GetUserFaults6V() const { return HALSIM_GetRoboRioUserFaults6V(m_index); }
+
+ void SetUserFaults6V(int userFaults6V) {
+ HALSIM_SetRoboRioUserFaults6V(m_index, userFaults6V);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterUserFaults5VCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelRoboRioUserFaults5VCallback);
+ store->SetUid(HALSIM_RegisterRoboRioUserFaults5VCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ int GetUserFaults5V() const { return HALSIM_GetRoboRioUserFaults5V(m_index); }
+
+ void SetUserFaults5V(int userFaults5V) {
+ HALSIM_SetRoboRioUserFaults5V(m_index, userFaults5V);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterUserFaults3V3Callback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelRoboRioUserFaults3V3Callback);
+ store->SetUid(HALSIM_RegisterRoboRioUserFaults3V3Callback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ int GetUserFaults3V3() const {
+ return HALSIM_GetRoboRioUserFaults3V3(m_index);
+ }
+
+ void SetUserFaults3V3(int userFaults3V3) {
+ HALSIM_SetRoboRioUserFaults3V3(m_index, userFaults3V3);
+ }
+
+ void ResetData() { HALSIM_ResetRoboRioData(m_index); }
+
+ private:
+ int m_index;
+};
+} // namespace sim
+} // namespace frc
diff --git a/hal/src/main/native/include/simulation/SPIAccelerometerSim.h b/hal/src/main/native/include/simulation/SPIAccelerometerSim.h
new file mode 100644
index 0000000..d7795ee
--- /dev/null
+++ b/hal/src/main/native/include/simulation/SPIAccelerometerSim.h
@@ -0,0 +1,95 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 <memory>
+#include <utility>
+
+#include "CallbackStore.h"
+#include "mockdata/SPIAccelerometerData.h"
+
+namespace frc {
+namespace sim {
+class SPIAccelerometerSim {
+ public:
+ explicit SPIAccelerometerSim(int index) { m_index = index; }
+
+ std::unique_ptr<CallbackStore> RegisterActiveCallback(NotifyCallback callback,
+ bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelSPIAccelerometerActiveCallback);
+ store->SetUid(HALSIM_RegisterSPIAccelerometerActiveCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ bool GetActive() const { return HALSIM_GetSPIAccelerometerActive(m_index); }
+
+ void SetActive(bool active) {
+ HALSIM_SetSPIAccelerometerActive(m_index, active);
+ }
+
+ std::unique_ptr<CallbackStore> RegisterRangeCallback(NotifyCallback callback,
+ bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelSPIAccelerometerRangeCallback);
+ store->SetUid(HALSIM_RegisterSPIAccelerometerRangeCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ int GetRange() const { return HALSIM_GetSPIAccelerometerRange(m_index); }
+
+ void SetRange(int range) { HALSIM_SetSPIAccelerometerRange(m_index, range); }
+
+ std::unique_ptr<CallbackStore> RegisterXCallback(NotifyCallback callback,
+ bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelSPIAccelerometerXCallback);
+ store->SetUid(HALSIM_RegisterSPIAccelerometerXCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ double GetX() const { return HALSIM_GetSPIAccelerometerX(m_index); }
+
+ void SetX(double x) { HALSIM_SetSPIAccelerometerX(m_index, x); }
+
+ std::unique_ptr<CallbackStore> RegisterYCallback(NotifyCallback callback,
+ bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelSPIAccelerometerYCallback);
+ store->SetUid(HALSIM_RegisterSPIAccelerometerYCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ double GetY() const { return HALSIM_GetSPIAccelerometerY(m_index); }
+
+ void SetY(double y) { HALSIM_SetSPIAccelerometerY(m_index, y); }
+
+ std::unique_ptr<CallbackStore> RegisterZCallback(NotifyCallback callback,
+ bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ m_index, -1, callback, &HALSIM_CancelSPIAccelerometerZCallback);
+ store->SetUid(HALSIM_RegisterSPIAccelerometerZCallback(
+ m_index, &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+ }
+
+ double GetZ() const { return HALSIM_GetSPIAccelerometerZ(m_index); }
+
+ void SetZ(double z) { HALSIM_SetSPIAccelerometerZ(m_index, z); }
+
+ void ResetData() { HALSIM_ResetSPIAccelerometerData(m_index); }
+
+ private:
+ int m_index;
+};
+} // namespace sim
+} // namespace frc
diff --git a/hal/src/main/native/include/simulation/SimDeviceSim.h b/hal/src/main/native/include/simulation/SimDeviceSim.h
new file mode 100644
index 0000000..5705a08
--- /dev/null
+++ b/hal/src/main/native/include/simulation/SimDeviceSim.h
@@ -0,0 +1,79 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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 <functional>
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "CallbackStore.h"
+#include "hal/SimDevice.h"
+#include "mockdata/SimDeviceData.h"
+
+namespace frc {
+namespace sim {
+class SimDeviceSim {
+ public:
+ explicit SimDeviceSim(const char* name)
+ : m_handle{HALSIM_GetSimDeviceHandle(name)} {}
+
+ hal::SimValue GetValue(const char* name) const {
+ return HALSIM_GetSimValueHandle(m_handle, name);
+ }
+
+ hal::SimDouble GetDouble(const char* name) const {
+ return HALSIM_GetSimValueHandle(m_handle, name);
+ }
+
+ hal::SimEnum GetEnum(const char* name) const {
+ return HALSIM_GetSimValueHandle(m_handle, name);
+ }
+
+ hal::SimBoolean GetBoolean(const char* name) const {
+ return HALSIM_GetSimValueHandle(m_handle, name);
+ }
+
+ static std::vector<std::string> GetEnumOptions(hal::SimEnum val) {
+ int32_t numOptions;
+ const char** options = HALSIM_GetSimValueEnumOptions(val, &numOptions);
+ std::vector<std::string> rv;
+ rv.reserve(numOptions);
+ for (int32_t i = 0; i < numOptions; ++i) rv.emplace_back(options[i]);
+ return rv;
+ }
+
+ template <typename F>
+ void EnumerateValues(F callback) const {
+ return HALSIM_EnumerateSimValues(
+ m_handle, &callback,
+ [](const char* name, void* param, HAL_SimValueHandle handle,
+ HAL_Bool readonly, const struct HAL_Value* value) {
+ std::invoke(*static_cast<F*>(param), name, handle, readonly, value);
+ });
+ }
+
+ operator HAL_SimDeviceHandle() const { return m_handle; }
+
+ template <typename F>
+ static void EnumerateDevices(const char* prefix, F callback) {
+ return HALSIM_EnumerateSimDevices(
+ prefix, &callback,
+ [](const char* name, void* param, HAL_SimDeviceHandle handle) {
+ std::invoke(*static_cast<F*>(param), name, handle);
+ });
+ }
+
+ static void ResetData() { HALSIM_ResetSimDeviceData(); }
+
+ private:
+ HAL_SimDeviceHandle m_handle;
+};
+} // namespace sim
+} // namespace frc
diff --git a/hal/src/main/native/include/simulation/SimHooks.h b/hal/src/main/native/include/simulation/SimHooks.h
new file mode 100644
index 0000000..537cef3
--- /dev/null
+++ b/hal/src/main/native/include/simulation/SimHooks.h
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 "mockdata/MockHooks.h"
+
+namespace frc {
+namespace sim {
+
+void WaitForProgramStart() { HALSIM_WaitForProgramStart(); }
+
+void SetProgramStarted() { HALSIM_SetProgramStarted(); }
+
+bool GetProgramStarted() { return HALSIM_GetProgramStarted(); }
+
+void RestartTiming() { HALSIM_RestartTiming(); }
+
+} // namespace sim
+} // namespace frc
diff --git a/hal/src/main/native/sim/Accelerometer.cpp b/hal/src/main/native/sim/Accelerometer.cpp
new file mode 100644
index 0000000..1435fd5
--- /dev/null
+++ b/hal/src/main/native/sim/Accelerometer.cpp
@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 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 "hal/Accelerometer.h"
+
+#include "mockdata/AccelerometerDataInternal.h"
+
+using namespace hal;
+
+namespace hal {
+namespace init {
+void InitializeAccelerometer() {}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+void HAL_SetAccelerometerActive(HAL_Bool active) {
+ SimAccelerometerData[0].active = active;
+}
+
+void HAL_SetAccelerometerRange(HAL_AccelerometerRange range) {
+ SimAccelerometerData[0].range = range;
+}
+double HAL_GetAccelerometerX(void) { return SimAccelerometerData[0].x; }
+double HAL_GetAccelerometerY(void) { return SimAccelerometerData[0].y; }
+double HAL_GetAccelerometerZ(void) { return SimAccelerometerData[0].z; }
+} // extern "C"
diff --git a/hal/src/main/native/sim/AddressableLED.cpp b/hal/src/main/native/sim/AddressableLED.cpp
new file mode 100644
index 0000000..70d3f6f
--- /dev/null
+++ b/hal/src/main/native/sim/AddressableLED.cpp
@@ -0,0 +1,168 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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 "hal/AddressableLED.h"
+
+#include "DigitalInternal.h"
+#include "HALInitializer.h"
+#include "PortsInternal.h"
+#include "hal/Errors.h"
+#include "hal/handles/HandlesInternal.h"
+#include "hal/handles/LimitedHandleResource.h"
+#include "mockdata/AddressableLEDDataInternal.h"
+
+using namespace hal;
+
+namespace {
+struct AddressableLED {
+ uint8_t index;
+};
+} // namespace
+
+static LimitedHandleResource<HAL_AddressableLEDHandle, AddressableLED,
+ kNumAddressableLEDs,
+ HAL_HandleEnum::AddressableLED>* ledHandles;
+
+namespace hal {
+namespace init {
+void InitializeAddressableLED() {
+ static LimitedHandleResource<HAL_AddressableLEDHandle, AddressableLED,
+ kNumAddressableLEDs,
+ HAL_HandleEnum::AddressableLED>
+ dcH;
+ ledHandles = &dcH;
+}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+HAL_AddressableLEDHandle HAL_InitializeAddressableLED(
+ HAL_DigitalHandle outputPort, int32_t* status) {
+ hal::init::CheckInit();
+
+ auto digitalPort =
+ hal::digitalChannelHandles->Get(outputPort, hal::HAL_HandleEnum::PWM);
+
+ if (!digitalPort) {
+ // If DIO was passed, channel error, else generic error
+ if (getHandleType(outputPort) == hal::HAL_HandleEnum::DIO) {
+ *status = HAL_LED_CHANNEL_ERROR;
+ } else {
+ *status = HAL_HANDLE_ERROR;
+ }
+ return HAL_kInvalidHandle;
+ }
+
+ if (digitalPort->channel >= kNumPWMHeaders) {
+ *status = HAL_LED_CHANNEL_ERROR;
+ return HAL_kInvalidHandle;
+ }
+
+ HAL_AddressableLEDHandle handle = ledHandles->Allocate();
+ if (handle == HAL_kInvalidHandle) {
+ *status = NO_AVAILABLE_RESOURCES;
+ return HAL_kInvalidHandle;
+ }
+
+ auto led = ledHandles->Get(handle);
+ if (!led) { // would only occur on thread issue
+ *status = HAL_HANDLE_ERROR;
+ return HAL_kInvalidHandle;
+ }
+
+ int16_t index = getHandleIndex(handle);
+ SimAddressableLEDData[index].outputPort = digitalPort->channel;
+ SimAddressableLEDData[index].length = 1;
+ SimAddressableLEDData[index].running = false;
+ SimAddressableLEDData[index].initialized = true;
+ led->index = index;
+ return handle;
+}
+
+void HAL_FreeAddressableLED(HAL_AddressableLEDHandle handle) {
+ auto led = ledHandles->Get(handle);
+ ledHandles->Free(handle);
+ if (!led) return;
+ SimAddressableLEDData[led->index].running = false;
+ SimAddressableLEDData[led->index].initialized = false;
+}
+
+void HAL_SetAddressableLEDOutputPort(HAL_AddressableLEDHandle handle,
+ HAL_DigitalHandle outputPort,
+ int32_t* status) {
+ auto led = ledHandles->Get(handle);
+ if (!led) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ if (auto port = digitalChannelHandles->Get(outputPort, HAL_HandleEnum::PWM)) {
+ SimAddressableLEDData[led->index].outputPort = port->channel;
+ } else {
+ SimAddressableLEDData[led->index].outputPort = -1;
+ }
+}
+
+void HAL_SetAddressableLEDLength(HAL_AddressableLEDHandle handle,
+ int32_t length, int32_t* status) {
+ auto led = ledHandles->Get(handle);
+ if (!led) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ if (length > HAL_kAddressableLEDMaxLength) {
+ *status = PARAMETER_OUT_OF_RANGE;
+ return;
+ }
+ SimAddressableLEDData[led->index].length = length;
+}
+
+void HAL_WriteAddressableLEDData(HAL_AddressableLEDHandle handle,
+ const struct HAL_AddressableLEDData* data,
+ int32_t length, int32_t* status) {
+ auto led = ledHandles->Get(handle);
+ if (!led) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ if (length > SimAddressableLEDData[led->index].length) {
+ *status = PARAMETER_OUT_OF_RANGE;
+ return;
+ }
+ SimAddressableLEDData[led->index].SetData(data, length);
+}
+
+void HAL_SetAddressableLEDBitTiming(HAL_AddressableLEDHandle handle,
+ int32_t lowTime0NanoSeconds,
+ int32_t highTime0NanoSeconds,
+ int32_t lowTime1NanoSeconds,
+ int32_t highTime1NanoSeconds,
+ int32_t* status) {}
+
+void HAL_SetAddressableLEDSyncTime(HAL_AddressableLEDHandle handle,
+ int32_t syncTimeMicroSeconds,
+ int32_t* status) {}
+
+void HAL_StartAddressableLEDOutput(HAL_AddressableLEDHandle handle,
+ int32_t* status) {
+ auto led = ledHandles->Get(handle);
+ if (!led) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ SimAddressableLEDData[led->index].running = true;
+}
+
+void HAL_StopAddressableLEDOutput(HAL_AddressableLEDHandle handle,
+ int32_t* status) {
+ auto led = ledHandles->Get(handle);
+ if (!led) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ SimAddressableLEDData[led->index].running = false;
+}
+} // extern "C"
diff --git a/hal/src/main/native/sim/AnalogAccumulator.cpp b/hal/src/main/native/sim/AnalogAccumulator.cpp
new file mode 100644
index 0000000..537aa15
--- /dev/null
+++ b/hal/src/main/native/sim/AnalogAccumulator.cpp
@@ -0,0 +1,112 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 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 "hal/AnalogAccumulator.h"
+
+#include "AnalogInternal.h"
+#include "mockdata/AnalogInDataInternal.h"
+
+using namespace hal;
+
+namespace hal {
+namespace init {
+void InitializeAnalogAccumulator() {}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+HAL_Bool HAL_IsAccumulatorChannel(HAL_AnalogInputHandle analogPortHandle,
+ int32_t* status) {
+ auto port = analogInputHandles->Get(analogPortHandle);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return false;
+ }
+ for (int32_t i = 0; i < kNumAccumulators; i++) {
+ if (port->channel == kAccumulatorChannels[i]) return true;
+ }
+ return false;
+}
+void HAL_InitAccumulator(HAL_AnalogInputHandle analogPortHandle,
+ int32_t* status) {
+ auto port = analogInputHandles->Get(analogPortHandle);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (!HAL_IsAccumulatorChannel(analogPortHandle, status)) {
+ *status = HAL_INVALID_ACCUMULATOR_CHANNEL;
+ return;
+ }
+
+ SimAnalogInData[port->channel].accumulatorInitialized = true;
+}
+void HAL_ResetAccumulator(HAL_AnalogInputHandle analogPortHandle,
+ int32_t* status) {
+ auto port = analogInputHandles->Get(analogPortHandle);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ SimAnalogInData[port->channel].accumulatorCenter = 0;
+ SimAnalogInData[port->channel].accumulatorCount = 0;
+ SimAnalogInData[port->channel].accumulatorValue = 0;
+}
+void HAL_SetAccumulatorCenter(HAL_AnalogInputHandle analogPortHandle,
+ int32_t center, int32_t* status) {
+ auto port = analogInputHandles->Get(analogPortHandle);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ SimAnalogInData[port->channel].accumulatorCenter = center;
+}
+void HAL_SetAccumulatorDeadband(HAL_AnalogInputHandle analogPortHandle,
+ int32_t deadband, int32_t* status) {
+ auto port = analogInputHandles->Get(analogPortHandle);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ SimAnalogInData[port->channel].accumulatorDeadband = deadband;
+}
+int64_t HAL_GetAccumulatorValue(HAL_AnalogInputHandle analogPortHandle,
+ int32_t* status) {
+ auto port = analogInputHandles->Get(analogPortHandle);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+
+ return SimAnalogInData[port->channel].accumulatorValue;
+}
+int64_t HAL_GetAccumulatorCount(HAL_AnalogInputHandle analogPortHandle,
+ int32_t* status) {
+ auto port = analogInputHandles->Get(analogPortHandle);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+
+ return SimAnalogInData[port->channel].accumulatorCount;
+}
+void HAL_GetAccumulatorOutput(HAL_AnalogInputHandle analogPortHandle,
+ int64_t* value, int64_t* count, int32_t* status) {
+ auto port = analogInputHandles->Get(analogPortHandle);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ *count = SimAnalogInData[port->channel].accumulatorCount;
+ *value = SimAnalogInData[port->channel].accumulatorValue;
+}
+} // extern "C"
diff --git a/hal/src/main/native/sim/AnalogGyro.cpp b/hal/src/main/native/sim/AnalogGyro.cpp
new file mode 100644
index 0000000..b7d84d9
--- /dev/null
+++ b/hal/src/main/native/sim/AnalogGyro.cpp
@@ -0,0 +1,147 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 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 "hal/AnalogGyro.h"
+
+#include <chrono>
+#include <thread>
+
+#include "AnalogInternal.h"
+#include "HALInitializer.h"
+#include "hal/AnalogAccumulator.h"
+#include "hal/AnalogInput.h"
+#include "hal/handles/IndexedHandleResource.h"
+#include "mockdata/AnalogGyroDataInternal.h"
+
+namespace {
+struct AnalogGyro {
+ HAL_AnalogInputHandle handle;
+ uint8_t index;
+};
+} // namespace
+
+using namespace hal;
+
+static IndexedHandleResource<HAL_GyroHandle, AnalogGyro, kNumAccumulators,
+ HAL_HandleEnum::AnalogGyro>* analogGyroHandles;
+
+namespace hal {
+namespace init {
+void InitializeAnalogGyro() {
+ static IndexedHandleResource<HAL_GyroHandle, AnalogGyro, kNumAccumulators,
+ HAL_HandleEnum::AnalogGyro>
+ agH;
+ analogGyroHandles = &agH;
+}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+HAL_GyroHandle HAL_InitializeAnalogGyro(HAL_AnalogInputHandle analogHandle,
+ int32_t* status) {
+ hal::init::CheckInit();
+ if (!HAL_IsAccumulatorChannel(analogHandle, status)) {
+ if (*status == 0) {
+ *status = HAL_INVALID_ACCUMULATOR_CHANNEL;
+ }
+ return HAL_kInvalidHandle;
+ }
+
+ // handle known to be correct, so no need to type check
+ int16_t channel = getHandleIndex(analogHandle);
+
+ auto handle = analogGyroHandles->Allocate(channel, status);
+
+ if (*status != 0)
+ return HAL_kInvalidHandle; // failed to allocate. Pass error back.
+
+ // Initialize port structure
+ auto gyro = analogGyroHandles->Get(handle);
+ if (gyro == nullptr) { // would only error on thread issue
+ *status = HAL_HANDLE_ERROR;
+ return HAL_kInvalidHandle;
+ }
+
+ gyro->handle = analogHandle;
+ gyro->index = channel;
+
+ SimAnalogGyroData[channel].initialized = true;
+
+ return handle;
+}
+
+void HAL_SetupAnalogGyro(HAL_GyroHandle handle, int32_t* status) {
+ // No op
+}
+
+void HAL_FreeAnalogGyro(HAL_GyroHandle handle) {
+ auto gyro = analogGyroHandles->Get(handle);
+ analogGyroHandles->Free(handle);
+ if (gyro == nullptr) return;
+ SimAnalogGyroData[gyro->index].initialized = false;
+}
+
+void HAL_SetAnalogGyroParameters(HAL_GyroHandle handle,
+ double voltsPerDegreePerSecond, double offset,
+ int32_t center, int32_t* status) {
+ // No op
+}
+
+void HAL_SetAnalogGyroVoltsPerDegreePerSecond(HAL_GyroHandle handle,
+ double voltsPerDegreePerSecond,
+ int32_t* status) {
+ // No op
+}
+
+void HAL_ResetAnalogGyro(HAL_GyroHandle handle, int32_t* status) {
+ auto gyro = analogGyroHandles->Get(handle);
+ if (gyro == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ SimAnalogGyroData[gyro->index].angle = 0.0;
+}
+
+void HAL_CalibrateAnalogGyro(HAL_GyroHandle handle, int32_t* status) {
+ // Just do a reset
+ HAL_ResetAnalogGyro(handle, status);
+}
+
+void HAL_SetAnalogGyroDeadband(HAL_GyroHandle handle, double volts,
+ int32_t* status) {
+ // No op
+}
+
+double HAL_GetAnalogGyroAngle(HAL_GyroHandle handle, int32_t* status) {
+ auto gyro = analogGyroHandles->Get(handle);
+ if (gyro == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+
+ return SimAnalogGyroData[gyro->index].angle;
+}
+
+double HAL_GetAnalogGyroRate(HAL_GyroHandle handle, int32_t* status) {
+ auto gyro = analogGyroHandles->Get(handle);
+ if (gyro == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+
+ return SimAnalogGyroData[gyro->index].rate;
+}
+
+double HAL_GetAnalogGyroOffset(HAL_GyroHandle handle, int32_t* status) {
+ return 0.0;
+}
+
+int32_t HAL_GetAnalogGyroCenter(HAL_GyroHandle handle, int32_t* status) {
+ return 0;
+}
+} // extern "C"
diff --git a/hal/src/main/native/sim/AnalogInput.cpp b/hal/src/main/native/sim/AnalogInput.cpp
new file mode 100644
index 0000000..8ecde74
--- /dev/null
+++ b/hal/src/main/native/sim/AnalogInput.cpp
@@ -0,0 +1,196 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "hal/AnalogInput.h"
+
+#include "AnalogInternal.h"
+#include "HALInitializer.h"
+#include "PortsInternal.h"
+#include "hal/AnalogAccumulator.h"
+#include "hal/handles/HandlesInternal.h"
+#include "mockdata/AnalogInDataInternal.h"
+
+using namespace hal;
+
+namespace hal {
+namespace init {
+void InitializeAnalogInput() {}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+HAL_AnalogInputHandle HAL_InitializeAnalogInputPort(HAL_PortHandle portHandle,
+ int32_t* status) {
+ hal::init::CheckInit();
+ int16_t channel = getPortHandleChannel(portHandle);
+ if (channel == InvalidHandleIndex) {
+ *status = PARAMETER_OUT_OF_RANGE;
+ return HAL_kInvalidHandle;
+ }
+
+ HAL_AnalogInputHandle handle = analogInputHandles->Allocate(channel, status);
+
+ if (*status != 0)
+ return HAL_kInvalidHandle; // failed to allocate. Pass error back.
+
+ // Initialize port structure
+ auto analog_port = analogInputHandles->Get(handle);
+ if (analog_port == nullptr) { // would only error on thread issue
+ *status = HAL_HANDLE_ERROR;
+ return HAL_kInvalidHandle;
+ }
+
+ analog_port->channel = static_cast<uint8_t>(channel);
+ if (HAL_IsAccumulatorChannel(handle, status)) {
+ analog_port->isAccumulator = true;
+ } else {
+ analog_port->isAccumulator = false;
+ }
+
+ SimAnalogInData[channel].initialized = true;
+ SimAnalogInData[channel].accumulatorInitialized = false;
+ SimAnalogInData[channel].simDevice = 0;
+
+ return handle;
+}
+void HAL_FreeAnalogInputPort(HAL_AnalogInputHandle analogPortHandle) {
+ auto port = analogInputHandles->Get(analogPortHandle);
+ // no status, so no need to check for a proper free.
+ analogInputHandles->Free(analogPortHandle);
+ if (port == nullptr) return;
+ SimAnalogInData[port->channel].initialized = false;
+ SimAnalogInData[port->channel].accumulatorInitialized = false;
+}
+
+HAL_Bool HAL_CheckAnalogModule(int32_t module) { return module == 1; }
+
+HAL_Bool HAL_CheckAnalogInputChannel(int32_t channel) {
+ return channel < kNumAnalogInputs && channel >= 0;
+}
+
+void HAL_SetAnalogInputSimDevice(HAL_AnalogInputHandle handle,
+ HAL_SimDeviceHandle device) {
+ auto port = analogInputHandles->Get(handle);
+ if (port == nullptr) return;
+ SimAnalogInData[port->channel].simDevice = device;
+}
+
+void HAL_SetAnalogSampleRate(double samplesPerSecond, int32_t* status) {
+ // No op
+}
+double HAL_GetAnalogSampleRate(int32_t* status) { return kDefaultSampleRate; }
+void HAL_SetAnalogAverageBits(HAL_AnalogInputHandle analogPortHandle,
+ int32_t bits, int32_t* status) {
+ auto port = analogInputHandles->Get(analogPortHandle);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ SimAnalogInData[port->channel].averageBits = bits;
+}
+int32_t HAL_GetAnalogAverageBits(HAL_AnalogInputHandle analogPortHandle,
+ int32_t* status) {
+ auto port = analogInputHandles->Get(analogPortHandle);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+
+ return SimAnalogInData[port->channel].averageBits;
+}
+void HAL_SetAnalogOversampleBits(HAL_AnalogInputHandle analogPortHandle,
+ int32_t bits, int32_t* status) {
+ auto port = analogInputHandles->Get(analogPortHandle);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ SimAnalogInData[port->channel].oversampleBits = bits;
+}
+int32_t HAL_GetAnalogOversampleBits(HAL_AnalogInputHandle analogPortHandle,
+ int32_t* status) {
+ auto port = analogInputHandles->Get(analogPortHandle);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+
+ return SimAnalogInData[port->channel].oversampleBits;
+}
+int32_t HAL_GetAnalogValue(HAL_AnalogInputHandle analogPortHandle,
+ int32_t* status) {
+ auto port = analogInputHandles->Get(analogPortHandle);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+
+ double voltage = SimAnalogInData[port->channel].voltage;
+ return HAL_GetAnalogVoltsToValue(analogPortHandle, voltage, status);
+}
+int32_t HAL_GetAnalogAverageValue(HAL_AnalogInputHandle analogPortHandle,
+ int32_t* status) {
+ // No averaging supported
+ return HAL_GetAnalogValue(analogPortHandle, status);
+}
+int32_t HAL_GetAnalogVoltsToValue(HAL_AnalogInputHandle analogPortHandle,
+ double voltage, int32_t* status) {
+ if (voltage > 5.0) {
+ voltage = 5.0;
+ *status = VOLTAGE_OUT_OF_RANGE;
+ }
+ if (voltage < 0.0) {
+ voltage = 0.0;
+ *status = VOLTAGE_OUT_OF_RANGE;
+ }
+ int32_t LSBWeight = HAL_GetAnalogLSBWeight(analogPortHandle, status);
+ int32_t offset = HAL_GetAnalogOffset(analogPortHandle, status);
+ int32_t value =
+ static_cast<int32_t>((voltage + offset * 1.0e-9) / (LSBWeight * 1.0e-9));
+ return value;
+}
+double HAL_GetAnalogVoltage(HAL_AnalogInputHandle analogPortHandle,
+ int32_t* status) {
+ auto port = analogInputHandles->Get(analogPortHandle);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0.0;
+ }
+
+ return SimAnalogInData[port->channel].voltage;
+}
+
+double HAL_GetAnalogValueToVolts(HAL_AnalogInputHandle analogPortHandle,
+ int32_t rawValue, int32_t* status) {
+ int32_t LSBWeight = HAL_GetAnalogLSBWeight(analogPortHandle, status);
+ int32_t offset = HAL_GetAnalogOffset(analogPortHandle, status);
+ double voltage = LSBWeight * 1.0e-9 * rawValue - offset * 1.0e-9;
+ return voltage;
+}
+
+double HAL_GetAnalogAverageVoltage(HAL_AnalogInputHandle analogPortHandle,
+ int32_t* status) {
+ auto port = analogInputHandles->Get(analogPortHandle);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0.0;
+ }
+
+ // No averaging supported
+ return SimAnalogInData[port->channel].voltage;
+}
+int32_t HAL_GetAnalogLSBWeight(HAL_AnalogInputHandle analogPortHandle,
+ int32_t* status) {
+ return 1220703;
+}
+int32_t HAL_GetAnalogOffset(HAL_AnalogInputHandle analogPortHandle,
+ int32_t* status) {
+ return 0;
+}
+} // extern "C"
diff --git a/hal/src/main/native/sim/AnalogInternal.cpp b/hal/src/main/native/sim/AnalogInternal.cpp
new file mode 100644
index 0000000..1e6a755
--- /dev/null
+++ b/hal/src/main/native/sim/AnalogInternal.cpp
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 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 "AnalogInternal.h"
+
+#include "PortsInternal.h"
+#include "hal/AnalogInput.h"
+
+namespace hal {
+IndexedHandleResource<HAL_AnalogInputHandle, hal::AnalogPort, kNumAnalogInputs,
+ HAL_HandleEnum::AnalogInput>* analogInputHandles;
+} // namespace hal
+
+namespace hal {
+namespace init {
+void InitializeAnalogInternal() {
+ static IndexedHandleResource<HAL_AnalogInputHandle, hal::AnalogPort,
+ kNumAnalogInputs, HAL_HandleEnum::AnalogInput>
+ aiH;
+ analogInputHandles = &aiH;
+}
+} // namespace init
+} // namespace hal
diff --git a/hal/src/main/native/sim/AnalogInternal.h b/hal/src/main/native/sim/AnalogInternal.h
new file mode 100644
index 0000000..bcc5c95
--- /dev/null
+++ b/hal/src/main/native/sim/AnalogInternal.h
@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 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 <stdint.h>
+
+#include <memory>
+
+#include "PortsInternal.h"
+#include "hal/Ports.h"
+#include "hal/handles/IndexedHandleResource.h"
+
+namespace hal {
+constexpr int32_t kTimebase = 40000000; ///< 40 MHz clock
+constexpr int32_t kDefaultOversampleBits = 0;
+constexpr int32_t kDefaultAverageBits = 7;
+constexpr double kDefaultSampleRate = 50000.0;
+static constexpr uint32_t kAccumulatorChannels[] = {0, 1};
+
+struct AnalogPort {
+ uint8_t channel;
+ bool isAccumulator;
+};
+
+extern IndexedHandleResource<HAL_AnalogInputHandle, hal::AnalogPort,
+ kNumAnalogInputs, HAL_HandleEnum::AnalogInput>*
+ analogInputHandles;
+
+int32_t GetAnalogTriggerInputIndex(HAL_AnalogTriggerHandle handle,
+ int32_t* status);
+} // namespace hal
diff --git a/hal/src/main/native/sim/AnalogOutput.cpp b/hal/src/main/native/sim/AnalogOutput.cpp
new file mode 100644
index 0000000..2e3a348
--- /dev/null
+++ b/hal/src/main/native/sim/AnalogOutput.cpp
@@ -0,0 +1,102 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 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 "hal/AnalogOutput.h"
+
+#include "HALInitializer.h"
+#include "PortsInternal.h"
+#include "hal/Errors.h"
+#include "hal/handles/HandlesInternal.h"
+#include "hal/handles/IndexedHandleResource.h"
+#include "mockdata/AnalogOutDataInternal.h"
+
+using namespace hal;
+
+namespace {
+struct AnalogOutput {
+ uint8_t channel;
+};
+} // namespace
+
+static IndexedHandleResource<HAL_AnalogOutputHandle, AnalogOutput,
+ kNumAnalogOutputs, HAL_HandleEnum::AnalogOutput>*
+ analogOutputHandles;
+
+namespace hal {
+namespace init {
+void InitializeAnalogOutput() {
+ static IndexedHandleResource<HAL_AnalogOutputHandle, AnalogOutput,
+ kNumAnalogOutputs, HAL_HandleEnum::AnalogOutput>
+ aoH;
+ analogOutputHandles = &aoH;
+}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+HAL_AnalogOutputHandle HAL_InitializeAnalogOutputPort(HAL_PortHandle portHandle,
+ int32_t* status) {
+ hal::init::CheckInit();
+ int16_t channel = getPortHandleChannel(portHandle);
+ if (channel == InvalidHandleIndex) {
+ *status = PARAMETER_OUT_OF_RANGE;
+ return HAL_kInvalidHandle;
+ }
+
+ HAL_AnalogOutputHandle handle =
+ analogOutputHandles->Allocate(channel, status);
+
+ if (*status != 0)
+ return HAL_kInvalidHandle; // failed to allocate. Pass error back.
+
+ auto port = analogOutputHandles->Get(handle);
+ if (port == nullptr) { // would only error on thread issue
+ *status = HAL_HANDLE_ERROR;
+ return HAL_kInvalidHandle;
+ }
+
+ port->channel = static_cast<uint8_t>(channel);
+
+ // Initialize sim analog input
+ SimAnalogOutData[channel].initialized = true;
+ return handle;
+}
+
+void HAL_FreeAnalogOutputPort(HAL_AnalogOutputHandle analogOutputHandle) {
+ // no status, so no need to check for a proper free.
+ auto port = analogOutputHandles->Get(analogOutputHandle);
+ if (port == nullptr) return;
+ analogOutputHandles->Free(analogOutputHandle);
+ SimAnalogOutData[port->channel].initialized = false;
+}
+
+HAL_Bool HAL_CheckAnalogOutputChannel(int32_t channel) {
+ return channel < kNumAnalogOutputs && channel >= 0;
+}
+
+void HAL_SetAnalogOutput(HAL_AnalogOutputHandle analogOutputHandle,
+ double voltage, int32_t* status) {
+ auto port = analogOutputHandles->Get(analogOutputHandle);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ SimAnalogOutData[port->channel].voltage = voltage;
+}
+
+double HAL_GetAnalogOutput(HAL_AnalogOutputHandle analogOutputHandle,
+ int32_t* status) {
+ auto port = analogOutputHandles->Get(analogOutputHandle);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0.0;
+ }
+
+ return SimAnalogOutData[port->channel].voltage;
+}
+} // extern "C"
diff --git a/hal/src/main/native/sim/AnalogTrigger.cpp b/hal/src/main/native/sim/AnalogTrigger.cpp
new file mode 100644
index 0000000..3ddacee
--- /dev/null
+++ b/hal/src/main/native/sim/AnalogTrigger.cpp
@@ -0,0 +1,285 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "hal/AnalogTrigger.h"
+
+#include "AnalogInternal.h"
+#include "HALInitializer.h"
+#include "PortsInternal.h"
+#include "hal/AnalogInput.h"
+#include "hal/Errors.h"
+#include "hal/handles/HandlesInternal.h"
+#include "hal/handles/LimitedHandleResource.h"
+#include "mockdata/AnalogInDataInternal.h"
+#include "mockdata/AnalogTriggerDataInternal.h"
+
+namespace {
+struct AnalogTrigger {
+ HAL_AnalogInputHandle analogHandle;
+ uint8_t index;
+ HAL_Bool trigState;
+};
+} // namespace
+
+using namespace hal;
+
+static LimitedHandleResource<HAL_AnalogTriggerHandle, AnalogTrigger,
+ kNumAnalogTriggers, HAL_HandleEnum::AnalogTrigger>*
+ analogTriggerHandles;
+
+namespace hal {
+namespace init {
+void InitializeAnalogTrigger() {
+ static LimitedHandleResource<HAL_AnalogTriggerHandle, AnalogTrigger,
+ kNumAnalogTriggers,
+ HAL_HandleEnum::AnalogTrigger>
+ atH;
+ analogTriggerHandles = &atH;
+}
+} // namespace init
+} // namespace hal
+
+int32_t hal::GetAnalogTriggerInputIndex(HAL_AnalogTriggerHandle handle,
+ int32_t* status) {
+ auto trigger = analogTriggerHandles->Get(handle);
+ if (trigger == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return -1;
+ }
+
+ auto analog_port = analogInputHandles->Get(trigger->analogHandle);
+ if (analog_port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return -1;
+ }
+
+ return analog_port->channel;
+}
+
+extern "C" {
+
+HAL_AnalogTriggerHandle HAL_InitializeAnalogTrigger(
+ HAL_AnalogInputHandle portHandle, int32_t* status) {
+ hal::init::CheckInit();
+ // ensure we are given a valid and active AnalogInput handle
+ auto analog_port = analogInputHandles->Get(portHandle);
+ if (analog_port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return HAL_kInvalidHandle;
+ }
+ HAL_AnalogTriggerHandle handle = analogTriggerHandles->Allocate();
+ if (handle == HAL_kInvalidHandle) {
+ *status = NO_AVAILABLE_RESOURCES;
+ return HAL_kInvalidHandle;
+ }
+ auto trigger = analogTriggerHandles->Get(handle);
+ if (trigger == nullptr) { // would only occur on thread issue
+ *status = HAL_HANDLE_ERROR;
+ return HAL_kInvalidHandle;
+ }
+ trigger->analogHandle = portHandle;
+ trigger->index = static_cast<uint8_t>(getHandleIndex(handle));
+
+ SimAnalogTriggerData[trigger->index].initialized = true;
+
+ trigger->trigState = false;
+
+ return handle;
+}
+
+HAL_AnalogTriggerHandle HAL_InitializeAnalogTriggerDutyCycle(
+ HAL_DutyCycleHandle dutyCycleHandle, int32_t* status) {
+ *status = HAL_SIM_NOT_SUPPORTED;
+ return HAL_kInvalidHandle;
+}
+
+void HAL_CleanAnalogTrigger(HAL_AnalogTriggerHandle analogTriggerHandle,
+ int32_t* status) {
+ auto trigger = analogTriggerHandles->Get(analogTriggerHandle);
+ analogTriggerHandles->Free(analogTriggerHandle);
+ if (trigger == nullptr) return;
+ SimAnalogTriggerData[trigger->index].initialized = false;
+ // caller owns the analog input handle.
+}
+
+static double GetAnalogValueToVoltage(
+ HAL_AnalogTriggerHandle analogTriggerHandle, int32_t value,
+ int32_t* status) {
+ int32_t LSBWeight = HAL_GetAnalogLSBWeight(analogTriggerHandle, status);
+ int32_t offset = HAL_GetAnalogOffset(analogTriggerHandle, status);
+
+ double voltage = LSBWeight * 1.0e-9 * value - offset * 1.0e-9;
+ return voltage;
+}
+
+void HAL_SetAnalogTriggerLimitsRaw(HAL_AnalogTriggerHandle analogTriggerHandle,
+ int32_t lower, int32_t upper,
+ int32_t* status) {
+ auto trigger = analogTriggerHandles->Get(analogTriggerHandle);
+ if (trigger == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ if (lower > upper) {
+ *status = ANALOG_TRIGGER_LIMIT_ORDER_ERROR;
+ }
+
+ double trigLower =
+ GetAnalogValueToVoltage(trigger->analogHandle, lower, status);
+ if (status != 0) return;
+ double trigUpper =
+ GetAnalogValueToVoltage(trigger->analogHandle, upper, status);
+ if (status != 0) return;
+
+ SimAnalogTriggerData[trigger->index].triggerUpperBound = trigUpper;
+ SimAnalogTriggerData[trigger->index].triggerLowerBound = trigLower;
+}
+
+void HAL_SetAnalogTriggerLimitsDutyCycle(
+ HAL_AnalogTriggerHandle analogTriggerHandle, double lower, double upper,
+ int32_t* status) {
+ *status = HAL_SIM_NOT_SUPPORTED;
+}
+
+void HAL_SetAnalogTriggerLimitsVoltage(
+ HAL_AnalogTriggerHandle analogTriggerHandle, double lower, double upper,
+ int32_t* status) {
+ auto trigger = analogTriggerHandles->Get(analogTriggerHandle);
+ if (trigger == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ if (lower > upper) {
+ *status = ANALOG_TRIGGER_LIMIT_ORDER_ERROR;
+ }
+
+ SimAnalogTriggerData[trigger->index].triggerUpperBound = upper;
+ SimAnalogTriggerData[trigger->index].triggerLowerBound = lower;
+}
+void HAL_SetAnalogTriggerAveraged(HAL_AnalogTriggerHandle analogTriggerHandle,
+ HAL_Bool useAveragedValue, int32_t* status) {
+ auto trigger = analogTriggerHandles->Get(analogTriggerHandle);
+ if (trigger == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ AnalogTriggerData* triggerData = &SimAnalogTriggerData[trigger->index];
+
+ if (triggerData->triggerMode.Get() != HALSIM_AnalogTriggerUnassigned) {
+ *status = INCOMPATIBLE_STATE;
+ return;
+ }
+
+ auto setVal = useAveragedValue ? HALSIM_AnalogTriggerAveraged
+ : HALSIM_AnalogTriggerUnassigned;
+ triggerData->triggerMode = setVal;
+}
+
+void HAL_SetAnalogTriggerFiltered(HAL_AnalogTriggerHandle analogTriggerHandle,
+ HAL_Bool useFilteredValue, int32_t* status) {
+ auto trigger = analogTriggerHandles->Get(analogTriggerHandle);
+ if (trigger == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ AnalogTriggerData* triggerData = &SimAnalogTriggerData[trigger->index];
+
+ if (triggerData->triggerMode.Get() != HALSIM_AnalogTriggerUnassigned) {
+ *status = INCOMPATIBLE_STATE;
+ return;
+ }
+
+ auto setVal = useFilteredValue ? HALSIM_AnalogTriggerFiltered
+ : HALSIM_AnalogTriggerUnassigned;
+ triggerData->triggerMode = setVal;
+}
+
+static double GetTriggerValue(AnalogTrigger* trigger, int32_t* status) {
+ auto analogIn = analogInputHandles->Get(trigger->analogHandle);
+ if (analogIn == nullptr) {
+ // Returning HAL Handle Error, but going to ignore lower down
+ *status = HAL_HANDLE_ERROR;
+ return 0.0;
+ }
+
+ return SimAnalogInData[analogIn->channel].voltage;
+}
+
+HAL_Bool HAL_GetAnalogTriggerInWindow(
+ HAL_AnalogTriggerHandle analogTriggerHandle, int32_t* status) {
+ auto trigger = analogTriggerHandles->Get(analogTriggerHandle);
+ if (trigger == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return false;
+ }
+
+ double voltage = GetTriggerValue(trigger.get(), status);
+ if (*status == HAL_HANDLE_ERROR) {
+ // Don't error if analog has been destroyed
+ *status = 0;
+ return false;
+ }
+
+ double trigUpper = SimAnalogTriggerData[trigger->index].triggerUpperBound;
+ double trigLower = SimAnalogTriggerData[trigger->index].triggerLowerBound;
+
+ return voltage >= trigLower && voltage <= trigUpper;
+}
+HAL_Bool HAL_GetAnalogTriggerTriggerState(
+ HAL_AnalogTriggerHandle analogTriggerHandle, int32_t* status) {
+ auto trigger = analogTriggerHandles->Get(analogTriggerHandle);
+ if (trigger == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return false;
+ }
+
+ double voltage = GetTriggerValue(trigger.get(), status);
+ if (*status == HAL_HANDLE_ERROR) {
+ // Don't error if analog has been destroyed
+ *status = 0;
+ return false;
+ }
+
+ double trigUpper = SimAnalogTriggerData[trigger->index].triggerUpperBound;
+ double trigLower = SimAnalogTriggerData[trigger->index].triggerLowerBound;
+
+ if (voltage < trigLower) {
+ trigger->trigState = false;
+ return false;
+ }
+ if (voltage > trigUpper) {
+ trigger->trigState = true;
+ return true;
+ }
+ return trigger->trigState;
+}
+HAL_Bool HAL_GetAnalogTriggerOutput(HAL_AnalogTriggerHandle analogTriggerHandle,
+ HAL_AnalogTriggerType type,
+ int32_t* status) {
+ if (type == HAL_Trigger_kInWindow) {
+ return HAL_GetAnalogTriggerInWindow(analogTriggerHandle, status);
+ } else if (type == HAL_Trigger_kState) {
+ return HAL_GetAnalogTriggerTriggerState(analogTriggerHandle, status);
+ } else {
+ *status = ANALOG_TRIGGER_PULSE_OUTPUT_ERROR;
+ return false;
+ }
+}
+
+int32_t HAL_GetAnalogTriggerFPGAIndex(
+ HAL_AnalogTriggerHandle analogTriggerHandle, int32_t* status) {
+ auto trigger = analogTriggerHandles->Get(analogTriggerHandle);
+ if (trigger == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return -1;
+ }
+ return trigger->index;
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/sim/CAN.cpp b/hal/src/main/native/sim/CAN.cpp
new file mode 100644
index 0000000..1e7af73
--- /dev/null
+++ b/hal/src/main/native/sim/CAN.cpp
@@ -0,0 +1,63 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 "hal/CAN.h"
+
+#include "mockdata/CanDataInternal.h"
+
+using namespace hal;
+
+namespace hal {
+namespace init {
+void InitializeCAN() {}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+
+void HAL_CAN_SendMessage(uint32_t messageID, const uint8_t* data,
+ uint8_t dataSize, int32_t periodMs, int32_t* status) {
+ SimCanData->sendMessage(messageID, data, dataSize, periodMs, status);
+}
+void HAL_CAN_ReceiveMessage(uint32_t* messageID, uint32_t messageIDMask,
+ uint8_t* data, uint8_t* dataSize,
+ uint32_t* timeStamp, int32_t* status) {
+ // Use a data size of 42 as call check. Difficult to add check to invoke
+ // handler
+ *dataSize = 42;
+ auto tmpStatus = *status;
+ SimCanData->receiveMessage(messageID, messageIDMask, data, dataSize,
+ timeStamp, status);
+ // If no handler invoked, return message not found
+ if (*dataSize == 42 && *status == tmpStatus) {
+ *status = HAL_ERR_CANSessionMux_MessageNotFound;
+ }
+}
+void HAL_CAN_OpenStreamSession(uint32_t* sessionHandle, uint32_t messageID,
+ uint32_t messageIDMask, uint32_t maxMessages,
+ int32_t* status) {
+ SimCanData->openStreamSession(sessionHandle, messageID, messageIDMask,
+ maxMessages, status);
+}
+void HAL_CAN_CloseStreamSession(uint32_t sessionHandle) {
+ SimCanData->closeStreamSession(sessionHandle);
+}
+void HAL_CAN_ReadStreamSession(uint32_t sessionHandle,
+ struct HAL_CANStreamMessage* messages,
+ uint32_t messagesToRead, uint32_t* messagesRead,
+ int32_t* status) {
+ SimCanData->readStreamSession(sessionHandle, messages, messagesToRead,
+ messagesRead, status);
+}
+void HAL_CAN_GetCANStatus(float* percentBusUtilization, uint32_t* busOffCount,
+ uint32_t* txFullCount, uint32_t* receiveErrorCount,
+ uint32_t* transmitErrorCount, int32_t* status) {
+ SimCanData->getCANStatus(percentBusUtilization, busOffCount, txFullCount,
+ receiveErrorCount, transmitErrorCount, status);
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/sim/CANAPI.cpp b/hal/src/main/native/sim/CANAPI.cpp
new file mode 100644
index 0000000..08b0989
--- /dev/null
+++ b/hal/src/main/native/sim/CANAPI.cpp
@@ -0,0 +1,356 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 "hal/CANAPI.h"
+
+#include <atomic>
+#include <ctime>
+
+#include <wpi/DenseMap.h>
+
+#include "CANAPIInternal.h"
+#include "HALInitializer.h"
+#include "hal/CAN.h"
+#include "hal/Errors.h"
+#include "hal/HAL.h"
+#include "hal/handles/UnlimitedHandleResource.h"
+
+using namespace hal;
+
+namespace {
+struct Receives {
+ uint64_t lastTimeStamp;
+ uint8_t data[8];
+ uint8_t length;
+};
+
+struct CANStorage {
+ HAL_CANManufacturer manufacturer;
+ HAL_CANDeviceType deviceType;
+ uint8_t deviceId;
+ wpi::mutex mapMutex;
+ wpi::SmallDenseMap<int32_t, int32_t> periodicSends;
+ wpi::SmallDenseMap<int32_t, Receives> receives;
+};
+} // namespace
+
+static UnlimitedHandleResource<HAL_CANHandle, CANStorage, HAL_HandleEnum::CAN>*
+ canHandles;
+
+static uint32_t GetPacketBaseTime() {
+ int status = 0;
+ auto basetime = HAL_GetFPGATime(&status);
+ // us to ms
+ return (basetime / 1000ull) & 0xFFFFFFFF;
+}
+
+namespace hal {
+namespace init {
+void InitializeCANAPI() {
+ static UnlimitedHandleResource<HAL_CANHandle, CANStorage, HAL_HandleEnum::CAN>
+ cH;
+ canHandles = &cH;
+}
+} // namespace init
+namespace can {
+int32_t GetCANModuleFromHandle(HAL_CANHandle handle, int32_t* status) {
+ auto can = canHandles->Get(handle);
+ if (!can) {
+ *status = HAL_HANDLE_ERROR;
+ return -1;
+ }
+ return can->deviceId;
+}
+} // namespace can
+} // namespace hal
+
+static int32_t CreateCANId(CANStorage* storage, int32_t apiId) {
+ int32_t createdId = 0;
+ createdId |= (static_cast<int32_t>(storage->deviceType) & 0x1F) << 24;
+ createdId |= (static_cast<int32_t>(storage->manufacturer) & 0xFF) << 16;
+ createdId |= (apiId & 0x3FF) << 6;
+ createdId |= (storage->deviceId & 0x3F);
+ return createdId;
+}
+
+HAL_CANHandle HAL_InitializeCAN(HAL_CANManufacturer manufacturer,
+ int32_t deviceId, HAL_CANDeviceType deviceType,
+ int32_t* status) {
+ hal::init::CheckInit();
+ auto can = std::make_shared<CANStorage>();
+
+ auto handle = canHandles->Allocate(can);
+
+ if (handle == HAL_kInvalidHandle) {
+ *status = NO_AVAILABLE_RESOURCES;
+ return HAL_kInvalidHandle;
+ }
+
+ can->deviceId = deviceId;
+ can->deviceType = deviceType;
+ can->manufacturer = manufacturer;
+
+ return handle;
+}
+
+void HAL_CleanCAN(HAL_CANHandle handle) {
+ auto data = canHandles->Free(handle);
+
+ std::scoped_lock lock(data->mapMutex);
+
+ for (auto&& i : data->periodicSends) {
+ int32_t s = 0;
+ auto id = CreateCANId(data.get(), i.first);
+ HAL_CAN_SendMessage(id, nullptr, 0, HAL_CAN_SEND_PERIOD_STOP_REPEATING, &s);
+ i.second = -1;
+ }
+}
+
+void HAL_WriteCANPacket(HAL_CANHandle handle, const uint8_t* data,
+ int32_t length, int32_t apiId, int32_t* status) {
+ auto can = canHandles->Get(handle);
+ if (!can) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ auto id = CreateCANId(can.get(), apiId);
+
+ HAL_CAN_SendMessage(id, data, length, HAL_CAN_SEND_PERIOD_NO_REPEAT, status);
+
+ if (*status != 0) {
+ return;
+ }
+ std::scoped_lock lock(can->mapMutex);
+ can->periodicSends[apiId] = -1;
+}
+
+void HAL_WriteCANPacketRepeating(HAL_CANHandle handle, const uint8_t* data,
+ int32_t length, int32_t apiId,
+ int32_t repeatMs, int32_t* status) {
+ auto can = canHandles->Get(handle);
+ if (!can) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ auto id = CreateCANId(can.get(), apiId);
+
+ HAL_CAN_SendMessage(id, data, length, repeatMs, status);
+
+ if (*status != 0) {
+ return;
+ }
+ std::scoped_lock lock(can->mapMutex);
+ can->periodicSends[apiId] = repeatMs;
+}
+
+void HAL_WriteCANRTRFrame(HAL_CANHandle handle, int32_t length, int32_t apiId,
+ int32_t* status) {
+ auto can = canHandles->Get(handle);
+ if (!can) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ auto id = CreateCANId(can.get(), apiId);
+ id |= HAL_CAN_IS_FRAME_REMOTE;
+ uint8_t data[8];
+ std::memset(data, 0, sizeof(data));
+
+ HAL_CAN_SendMessage(id, data, length, HAL_CAN_SEND_PERIOD_NO_REPEAT, status);
+
+ if (*status != 0) {
+ return;
+ }
+ std::scoped_lock lock(can->mapMutex);
+ can->periodicSends[apiId] = -1;
+}
+
+void HAL_StopCANPacketRepeating(HAL_CANHandle handle, int32_t apiId,
+ int32_t* status) {
+ auto can = canHandles->Get(handle);
+ if (!can) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ auto id = CreateCANId(can.get(), apiId);
+
+ HAL_CAN_SendMessage(id, nullptr, 0, HAL_CAN_SEND_PERIOD_STOP_REPEATING,
+ status);
+
+ if (*status != 0) {
+ return;
+ }
+ std::scoped_lock lock(can->mapMutex);
+ can->periodicSends[apiId] = -1;
+}
+
+void HAL_ReadCANPacketNew(HAL_CANHandle handle, int32_t apiId, uint8_t* data,
+ int32_t* length, uint64_t* receivedTimestamp,
+ int32_t* status) {
+ auto can = canHandles->Get(handle);
+ if (!can) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ uint32_t messageId = CreateCANId(can.get(), apiId);
+ uint8_t dataSize = 0;
+ uint32_t ts = 0;
+ HAL_CAN_ReceiveMessage(&messageId, 0x1FFFFFFF, data, &dataSize, &ts, status);
+
+ if (*status == 0) {
+ std::scoped_lock lock(can->mapMutex);
+ auto& msg = can->receives[messageId];
+ msg.length = dataSize;
+ msg.lastTimeStamp = ts;
+ // The NetComm call placed in data, copy into the msg
+ std::memcpy(msg.data, data, dataSize);
+ }
+ *length = dataSize;
+ *receivedTimestamp = ts;
+}
+
+void HAL_ReadCANPacketLatest(HAL_CANHandle handle, int32_t apiId, uint8_t* data,
+ int32_t* length, uint64_t* receivedTimestamp,
+ int32_t* status) {
+ auto can = canHandles->Get(handle);
+ if (!can) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ uint32_t messageId = CreateCANId(can.get(), apiId);
+ uint8_t dataSize = 0;
+ uint32_t ts = 0;
+ HAL_CAN_ReceiveMessage(&messageId, 0x1FFFFFFF, data, &dataSize, &ts, status);
+
+ std::scoped_lock lock(can->mapMutex);
+ if (*status == 0) {
+ // fresh update
+ auto& msg = can->receives[messageId];
+ msg.length = dataSize;
+ *length = dataSize;
+ msg.lastTimeStamp = ts;
+ *receivedTimestamp = ts;
+ // The NetComm call placed in data, copy into the msg
+ std::memcpy(msg.data, data, dataSize);
+ } else {
+ auto i = can->receives.find(messageId);
+ if (i != can->receives.end()) {
+ // Read the data from the stored message into the output
+ std::memcpy(data, i->second.data, i->second.length);
+ *length = i->second.length;
+ *receivedTimestamp = i->second.lastTimeStamp;
+ *status = 0;
+ }
+ }
+}
+
+void HAL_ReadCANPacketTimeout(HAL_CANHandle handle, int32_t apiId,
+ uint8_t* data, int32_t* length,
+ uint64_t* receivedTimestamp, int32_t timeoutMs,
+ int32_t* status) {
+ auto can = canHandles->Get(handle);
+ if (!can) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ uint32_t messageId = CreateCANId(can.get(), apiId);
+ uint8_t dataSize = 0;
+ uint32_t ts = 0;
+ HAL_CAN_ReceiveMessage(&messageId, 0x1FFFFFFF, data, &dataSize, &ts, status);
+
+ std::scoped_lock lock(can->mapMutex);
+ if (*status == 0) {
+ // fresh update
+ auto& msg = can->receives[messageId];
+ msg.length = dataSize;
+ *length = dataSize;
+ msg.lastTimeStamp = ts;
+ *receivedTimestamp = ts;
+ // The NetComm call placed in data, copy into the msg
+ std::memcpy(msg.data, data, dataSize);
+ } else {
+ auto i = can->receives.find(messageId);
+ if (i != can->receives.end()) {
+ // Found, check if new enough
+ uint32_t now = GetPacketBaseTime();
+ if (now - i->second.lastTimeStamp > static_cast<uint32_t>(timeoutMs)) {
+ // Timeout, return bad status
+ *status = HAL_CAN_TIMEOUT;
+ return;
+ }
+ // Read the data from the stored message into the output
+ std::memcpy(data, i->second.data, i->second.length);
+ *length = i->second.length;
+ *receivedTimestamp = i->second.lastTimeStamp;
+ *status = 0;
+ }
+ }
+}
+
+void HAL_ReadCANPeriodicPacket(HAL_CANHandle handle, int32_t apiId,
+ uint8_t* data, int32_t* length,
+ uint64_t* receivedTimestamp, int32_t timeoutMs,
+ int32_t periodMs, int32_t* status) {
+ auto can = canHandles->Get(handle);
+ if (!can) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ uint32_t messageId = CreateCANId(can.get(), apiId);
+
+ {
+ std::scoped_lock lock(can->mapMutex);
+ auto i = can->receives.find(messageId);
+ if (i != can->receives.end()) {
+ // Found, check if new enough
+ uint32_t now = GetPacketBaseTime();
+ if (now - i->second.lastTimeStamp < static_cast<uint32_t>(periodMs)) {
+ *status = 0;
+ // Read the data from the stored message into the output
+ std::memcpy(data, i->second.data, i->second.length);
+ *length = i->second.length;
+ *receivedTimestamp = i->second.lastTimeStamp;
+ return;
+ }
+ }
+ }
+
+ uint8_t dataSize = 0;
+ uint32_t ts = 0;
+ HAL_CAN_ReceiveMessage(&messageId, 0x1FFFFFFF, data, &dataSize, &ts, status);
+
+ std::scoped_lock lock(can->mapMutex);
+ if (*status == 0) {
+ // fresh update
+ auto& msg = can->receives[messageId];
+ msg.length = dataSize;
+ *length = dataSize;
+ msg.lastTimeStamp = ts;
+ *receivedTimestamp = ts;
+ // The NetComm call placed in data, copy into the msg
+ std::memcpy(msg.data, data, dataSize);
+ } else {
+ auto i = can->receives.find(messageId);
+ if (i != can->receives.end()) {
+ // Found, check if new enough
+ uint32_t now = GetPacketBaseTime();
+ if (now - i->second.lastTimeStamp > static_cast<uint32_t>(timeoutMs)) {
+ // Timeout, return bad status
+ *status = HAL_CAN_TIMEOUT;
+ return;
+ }
+ // Read the data from the stored message into the output
+ std::memcpy(data, i->second.data, i->second.length);
+ *length = i->second.length;
+ *receivedTimestamp = i->second.lastTimeStamp;
+ *status = 0;
+ }
+ }
+}
diff --git a/hal/src/main/native/sim/CANAPIInternal.h b/hal/src/main/native/sim/CANAPIInternal.h
new file mode 100644
index 0000000..074f682
--- /dev/null
+++ b/hal/src/main/native/sim/CANAPIInternal.h
@@ -0,0 +1,16 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 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 "hal/Types.h"
+
+namespace hal {
+namespace can {
+int32_t GetCANModuleFromHandle(HAL_CANHandle handle, int32_t* status);
+} // namespace can
+} // namespace hal
diff --git a/hal/src/main/native/sim/CallbackStore.cpp b/hal/src/main/native/sim/CallbackStore.cpp
new file mode 100644
index 0000000..d278b93
--- /dev/null
+++ b/hal/src/main/native/sim/CallbackStore.cpp
@@ -0,0 +1,13 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 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 "simulation/CallbackStore.h"
+
+void frc::sim::CallbackStoreThunk(const char* name, void* param,
+ const HAL_Value* value) {
+ reinterpret_cast<CallbackStore*>(param)->callback(name, value);
+}
diff --git a/hal/src/main/native/sim/Compressor.cpp b/hal/src/main/native/sim/Compressor.cpp
new file mode 100644
index 0000000..b5c5867
--- /dev/null
+++ b/hal/src/main/native/sim/Compressor.cpp
@@ -0,0 +1,123 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 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 "hal/Compressor.h"
+
+#include "HALInitializer.h"
+#include "PortsInternal.h"
+#include "hal/Errors.h"
+#include "hal/handles/HandlesInternal.h"
+#include "mockdata/PCMDataInternal.h"
+
+using namespace hal;
+
+namespace hal {
+namespace init {
+void InitializeCompressor() {}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+
+HAL_CompressorHandle HAL_InitializeCompressor(int32_t module, int32_t* status) {
+ hal::init::CheckInit();
+ // As compressors can have unlimited objects, just create a
+ // handle with the module number as the index.
+
+ SimPCMData[module].compressorInitialized = true;
+ return (HAL_CompressorHandle)createHandle(static_cast<int16_t>(module),
+ HAL_HandleEnum::Compressor, 0);
+}
+
+HAL_Bool HAL_CheckCompressorModule(int32_t module) {
+ return module < kNumPCMModules && module >= 0;
+}
+
+HAL_Bool HAL_GetCompressor(HAL_CompressorHandle compressorHandle,
+ int32_t* status) {
+ int16_t index =
+ getHandleTypedIndex(compressorHandle, HAL_HandleEnum::Compressor, 0);
+ if (index == InvalidHandleIndex) {
+ *status = HAL_HANDLE_ERROR;
+ return false;
+ }
+
+ return SimPCMData[index].compressorOn;
+}
+
+void HAL_SetCompressorClosedLoopControl(HAL_CompressorHandle compressorHandle,
+ HAL_Bool value, int32_t* status) {
+ int16_t index =
+ getHandleTypedIndex(compressorHandle, HAL_HandleEnum::Compressor, 0);
+ if (index == InvalidHandleIndex) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ SimPCMData[index].closedLoopEnabled = value;
+}
+
+HAL_Bool HAL_GetCompressorClosedLoopControl(
+ HAL_CompressorHandle compressorHandle, int32_t* status) {
+ int16_t index =
+ getHandleTypedIndex(compressorHandle, HAL_HandleEnum::Compressor, 0);
+ if (index == InvalidHandleIndex) {
+ *status = HAL_HANDLE_ERROR;
+ return false;
+ }
+
+ return SimPCMData[index].closedLoopEnabled;
+}
+
+HAL_Bool HAL_GetCompressorPressureSwitch(HAL_CompressorHandle compressorHandle,
+ int32_t* status) {
+ int16_t index =
+ getHandleTypedIndex(compressorHandle, HAL_HandleEnum::Compressor, 0);
+ if (index == InvalidHandleIndex) {
+ *status = HAL_HANDLE_ERROR;
+ return false;
+ }
+
+ return SimPCMData[index].pressureSwitch;
+}
+
+double HAL_GetCompressorCurrent(HAL_CompressorHandle compressorHandle,
+ int32_t* status) {
+ int16_t index =
+ getHandleTypedIndex(compressorHandle, HAL_HandleEnum::Compressor, 0);
+ if (index == InvalidHandleIndex) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+
+ return SimPCMData[index].compressorCurrent;
+}
+HAL_Bool HAL_GetCompressorCurrentTooHighFault(
+ HAL_CompressorHandle compressorHandle, int32_t* status) {
+ return false;
+}
+HAL_Bool HAL_GetCompressorCurrentTooHighStickyFault(
+ HAL_CompressorHandle compressorHandle, int32_t* status) {
+ return false;
+}
+HAL_Bool HAL_GetCompressorShortedStickyFault(
+ HAL_CompressorHandle compressorHandle, int32_t* status) {
+ return false;
+}
+HAL_Bool HAL_GetCompressorShortedFault(HAL_CompressorHandle compressorHandle,
+ int32_t* status) {
+ return false;
+}
+HAL_Bool HAL_GetCompressorNotConnectedStickyFault(
+ HAL_CompressorHandle compressorHandle, int32_t* status) {
+ return false;
+}
+HAL_Bool HAL_GetCompressorNotConnectedFault(
+ HAL_CompressorHandle compressorHandle, int32_t* status) {
+ return false;
+}
+} // extern "C"
diff --git a/hal/src/main/native/sim/Constants.cpp b/hal/src/main/native/sim/Constants.cpp
new file mode 100644
index 0000000..64cb52b
--- /dev/null
+++ b/hal/src/main/native/sim/Constants.cpp
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 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 "hal/Constants.h"
+
+#include "ConstantsInternal.h"
+
+using namespace hal;
+
+namespace hal {
+namespace init {
+void InitializeConstants() {}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+int32_t HAL_GetSystemClockTicksPerMicrosecond(void) {
+ return kSystemClockTicksPerMicrosecond;
+}
+} // extern "C"
diff --git a/hal/src/main/native/sim/ConstantsInternal.h b/hal/src/main/native/sim/ConstantsInternal.h
new file mode 100644
index 0000000..c3a6e8f
--- /dev/null
+++ b/hal/src/main/native/sim/ConstantsInternal.h
@@ -0,0 +1,14 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 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 <stdint.h>
+
+namespace hal {
+constexpr int32_t kSystemClockTicksPerMicrosecond = 40;
+} // namespace hal
diff --git a/hal/src/main/native/sim/Counter.cpp b/hal/src/main/native/sim/Counter.cpp
new file mode 100644
index 0000000..37454d0
--- /dev/null
+++ b/hal/src/main/native/sim/Counter.cpp
@@ -0,0 +1,98 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 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 "hal/Counter.h"
+
+#include "CounterInternal.h"
+#include "HALInitializer.h"
+#include "PortsInternal.h"
+#include "hal/Errors.h"
+#include "hal/handles/HandlesInternal.h"
+#include "hal/handles/LimitedHandleResource.h"
+
+namespace hal {
+
+LimitedHandleResource<HAL_CounterHandle, Counter, kNumCounters,
+ HAL_HandleEnum::Counter>* counterHandles;
+} // namespace hal
+
+namespace hal {
+namespace init {
+void InitializeCounter() {
+ static LimitedHandleResource<HAL_CounterHandle, Counter, kNumCounters,
+ HAL_HandleEnum::Counter>
+ cH;
+ counterHandles = &cH;
+}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+HAL_CounterHandle HAL_InitializeCounter(HAL_Counter_Mode mode, int32_t* index,
+ int32_t* status) {
+ hal::init::CheckInit();
+ return 0;
+}
+void HAL_FreeCounter(HAL_CounterHandle counterHandle, int32_t* status) {}
+void HAL_SetCounterAverageSize(HAL_CounterHandle counterHandle, int32_t size,
+ int32_t* status) {}
+void HAL_SetCounterUpSource(HAL_CounterHandle counterHandle,
+ HAL_Handle digitalSourceHandle,
+ HAL_AnalogTriggerType analogTriggerType,
+ int32_t* status) {}
+void HAL_SetCounterUpSourceEdge(HAL_CounterHandle counterHandle,
+ HAL_Bool risingEdge, HAL_Bool fallingEdge,
+ int32_t* status) {}
+void HAL_ClearCounterUpSource(HAL_CounterHandle counterHandle,
+ int32_t* status) {}
+void HAL_SetCounterDownSource(HAL_CounterHandle counterHandle,
+ HAL_Handle digitalSourceHandle,
+ HAL_AnalogTriggerType analogTriggerType,
+ int32_t* status) {}
+void HAL_SetCounterDownSourceEdge(HAL_CounterHandle counterHandle,
+ HAL_Bool risingEdge, HAL_Bool fallingEdge,
+ int32_t* status) {}
+void HAL_ClearCounterDownSource(HAL_CounterHandle counterHandle,
+ int32_t* status) {}
+void HAL_SetCounterUpDownMode(HAL_CounterHandle counterHandle,
+ int32_t* status) {}
+void HAL_SetCounterExternalDirectionMode(HAL_CounterHandle counterHandle,
+ int32_t* status) {}
+void HAL_SetCounterSemiPeriodMode(HAL_CounterHandle counterHandle,
+ HAL_Bool highSemiPeriod, int32_t* status) {}
+void HAL_SetCounterPulseLengthMode(HAL_CounterHandle counterHandle,
+ double threshold, int32_t* status) {}
+int32_t HAL_GetCounterSamplesToAverage(HAL_CounterHandle counterHandle,
+ int32_t* status) {
+ return 0;
+}
+void HAL_SetCounterSamplesToAverage(HAL_CounterHandle counterHandle,
+ int32_t samplesToAverage, int32_t* status) {
+}
+void HAL_ResetCounter(HAL_CounterHandle counterHandle, int32_t* status) {}
+int32_t HAL_GetCounter(HAL_CounterHandle counterHandle, int32_t* status) {
+ return 0;
+}
+double HAL_GetCounterPeriod(HAL_CounterHandle counterHandle, int32_t* status) {
+ return 0.0;
+}
+void HAL_SetCounterMaxPeriod(HAL_CounterHandle counterHandle, double maxPeriod,
+ int32_t* status) {}
+void HAL_SetCounterUpdateWhenEmpty(HAL_CounterHandle counterHandle,
+ HAL_Bool enabled, int32_t* status) {}
+HAL_Bool HAL_GetCounterStopped(HAL_CounterHandle counterHandle,
+ int32_t* status) {
+ return false;
+}
+HAL_Bool HAL_GetCounterDirection(HAL_CounterHandle counterHandle,
+ int32_t* status) {
+ return false;
+}
+void HAL_SetCounterReverseDirection(HAL_CounterHandle counterHandle,
+ HAL_Bool reverseDirection,
+ int32_t* status) {}
+} // extern "C"
diff --git a/hal/src/main/native/sim/CounterInternal.h b/hal/src/main/native/sim/CounterInternal.h
new file mode 100644
index 0000000..70fbe54
--- /dev/null
+++ b/hal/src/main/native/sim/CounterInternal.h
@@ -0,0 +1,23 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 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 "PortsInternal.h"
+#include "hal/handles/HandlesInternal.h"
+#include "hal/handles/LimitedHandleResource.h"
+
+namespace hal {
+
+struct Counter {
+ uint8_t index;
+};
+
+extern LimitedHandleResource<HAL_CounterHandle, Counter, kNumCounters,
+ HAL_HandleEnum::Counter>* counterHandles;
+
+} // namespace hal
diff --git a/hal/src/main/native/sim/DIO.cpp b/hal/src/main/native/sim/DIO.cpp
new file mode 100644
index 0000000..2158136
--- /dev/null
+++ b/hal/src/main/native/sim/DIO.cpp
@@ -0,0 +1,253 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 "hal/DIO.h"
+
+#include <cmath>
+
+#include "DigitalInternal.h"
+#include "HALInitializer.h"
+#include "PortsInternal.h"
+#include "hal/handles/HandlesInternal.h"
+#include "hal/handles/LimitedHandleResource.h"
+#include "mockdata/DIODataInternal.h"
+#include "mockdata/DigitalPWMDataInternal.h"
+
+using namespace hal;
+
+static LimitedHandleResource<HAL_DigitalPWMHandle, uint8_t,
+ kNumDigitalPWMOutputs, HAL_HandleEnum::DigitalPWM>*
+ digitalPWMHandles;
+
+namespace hal {
+namespace init {
+void InitializeDIO() {
+ static LimitedHandleResource<HAL_DigitalPWMHandle, uint8_t,
+ kNumDigitalPWMOutputs,
+ HAL_HandleEnum::DigitalPWM>
+ dpH;
+ digitalPWMHandles = &dpH;
+}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+
+HAL_DigitalHandle HAL_InitializeDIOPort(HAL_PortHandle portHandle,
+ HAL_Bool input, int32_t* status) {
+ hal::init::CheckInit();
+ if (*status != 0) return HAL_kInvalidHandle;
+
+ int16_t channel = getPortHandleChannel(portHandle);
+ if (channel == InvalidHandleIndex) {
+ *status = PARAMETER_OUT_OF_RANGE;
+ return HAL_kInvalidHandle;
+ }
+
+ auto handle =
+ digitalChannelHandles->Allocate(channel, HAL_HandleEnum::DIO, status);
+
+ if (*status != 0)
+ return HAL_kInvalidHandle; // failed to allocate. Pass error back.
+
+ auto port = digitalChannelHandles->Get(handle, HAL_HandleEnum::DIO);
+ if (port == nullptr) { // would only occur on thread issue.
+ *status = HAL_HANDLE_ERROR;
+ return HAL_kInvalidHandle;
+ }
+
+ port->channel = static_cast<uint8_t>(channel);
+
+ SimDIOData[channel].initialized = true;
+ SimDIOData[channel].isInput = input;
+ SimDIOData[channel].simDevice = 0;
+
+ return handle;
+}
+
+HAL_Bool HAL_CheckDIOChannel(int32_t channel) {
+ return channel < kNumDigitalChannels && channel >= 0;
+}
+
+void HAL_FreeDIOPort(HAL_DigitalHandle dioPortHandle) {
+ auto port = digitalChannelHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
+ // no status, so no need to check for a proper free.
+ digitalChannelHandles->Free(dioPortHandle, HAL_HandleEnum::DIO);
+ if (port == nullptr) return;
+ SimDIOData[port->channel].initialized = false;
+}
+
+void HAL_SetDIOSimDevice(HAL_DigitalHandle handle, HAL_SimDeviceHandle device) {
+ auto port = digitalChannelHandles->Get(handle, HAL_HandleEnum::DIO);
+ if (port == nullptr) return;
+ SimDIOData[port->channel].simDevice = device;
+}
+
+HAL_DigitalPWMHandle HAL_AllocateDigitalPWM(int32_t* status) {
+ auto handle = digitalPWMHandles->Allocate();
+ if (handle == HAL_kInvalidHandle) {
+ *status = NO_AVAILABLE_RESOURCES;
+ return HAL_kInvalidHandle;
+ }
+
+ auto id = digitalPWMHandles->Get(handle);
+ if (id == nullptr) { // would only occur on thread issue.
+ *status = HAL_HANDLE_ERROR;
+ return HAL_kInvalidHandle;
+ }
+ *id = static_cast<uint8_t>(getHandleIndex(handle));
+
+ SimDigitalPWMData[*id].initialized = true;
+
+ return handle;
+}
+
+void HAL_FreeDigitalPWM(HAL_DigitalPWMHandle pwmGenerator, int32_t* status) {
+ auto port = digitalPWMHandles->Get(pwmGenerator);
+ digitalPWMHandles->Free(pwmGenerator);
+ if (port == nullptr) return;
+ int32_t id = *port;
+ SimDigitalPWMData[id].initialized = false;
+}
+
+void HAL_SetDigitalPWMRate(double rate, int32_t* status) {
+ // Currently rounding in the log rate domain... heavy weight toward picking a
+ // higher freq.
+ // TODO: Round in the linear rate domain.
+ // uint8_t pwmPeriodPower = static_cast<uint8_t>(
+ // std::log(1.0 / (kExpectedLoopTiming * 0.25E-6 * rate)) /
+ // std::log(2.0) +
+ // 0.5);
+ // TODO(THAD) : Add a case to set this in the simulator
+ // digitalSystem->writePWMPeriodPower(pwmPeriodPower, status);
+}
+
+void HAL_SetDigitalPWMDutyCycle(HAL_DigitalPWMHandle pwmGenerator,
+ double dutyCycle, int32_t* status) {
+ auto port = digitalPWMHandles->Get(pwmGenerator);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ int32_t id = *port;
+ if (dutyCycle > 1.0) dutyCycle = 1.0;
+ if (dutyCycle < 0.0) dutyCycle = 0.0;
+ SimDigitalPWMData[id].dutyCycle = dutyCycle;
+}
+
+void HAL_SetDigitalPWMOutputChannel(HAL_DigitalPWMHandle pwmGenerator,
+ int32_t channel, int32_t* status) {
+ auto port = digitalPWMHandles->Get(pwmGenerator);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ int32_t id = *port;
+ SimDigitalPWMData[id].pin = channel;
+}
+
+void HAL_SetDIO(HAL_DigitalHandle dioPortHandle, HAL_Bool value,
+ int32_t* status) {
+ auto port = digitalChannelHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ if (value != 0 && value != 1) {
+ if (value != 0) value = 1;
+ }
+ SimDIOData[port->channel].value = value;
+}
+
+void HAL_SetDIODirection(HAL_DigitalHandle dioPortHandle, HAL_Bool input,
+ int32_t* status) {
+ auto port = digitalChannelHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ SimDIOData[port->channel].isInput = input;
+}
+
+HAL_Bool HAL_GetDIO(HAL_DigitalHandle dioPortHandle, int32_t* status) {
+ auto port = digitalChannelHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return false;
+ }
+ HAL_Bool value = SimDIOData[port->channel].value;
+ if (value > 1) value = 1;
+ if (value < 0) value = 0;
+ return value;
+}
+
+HAL_Bool HAL_GetDIODirection(HAL_DigitalHandle dioPortHandle, int32_t* status) {
+ auto port = digitalChannelHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return false;
+ }
+ HAL_Bool value = SimDIOData[port->channel].isInput;
+ if (value > 1) value = 1;
+ if (value < 0) value = 0;
+ return value;
+}
+
+void HAL_Pulse(HAL_DigitalHandle dioPortHandle, double pulseLength,
+ int32_t* status) {
+ auto port = digitalChannelHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ // TODO (Thad) Add this
+}
+
+HAL_Bool HAL_IsPulsing(HAL_DigitalHandle dioPortHandle, int32_t* status) {
+ auto port = digitalChannelHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return false;
+ }
+ return false;
+ // TODO (Thad) Add this
+}
+
+HAL_Bool HAL_IsAnyPulsing(int32_t* status) {
+ return false; // TODO(Thad) Figure this out
+}
+
+void HAL_SetFilterSelect(HAL_DigitalHandle dioPortHandle, int32_t filterIndex,
+ int32_t* status) {
+ auto port = digitalChannelHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ // TODO(Thad) Figure this out
+}
+
+int32_t HAL_GetFilterSelect(HAL_DigitalHandle dioPortHandle, int32_t* status) {
+ auto port = digitalChannelHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+ return 0;
+ // TODO(Thad) Figure this out
+}
+
+void HAL_SetFilterPeriod(int32_t filterIndex, int64_t value, int32_t* status) {
+ // TODO(Thad) figure this out
+}
+
+int64_t HAL_GetFilterPeriod(int32_t filterIndex, int32_t* status) {
+ return 0; // TODO(Thad) figure this out
+}
+} // extern "C"
diff --git a/hal/src/main/native/sim/DMA.cpp b/hal/src/main/native/sim/DMA.cpp
new file mode 100644
index 0000000..cea5a29
--- /dev/null
+++ b/hal/src/main/native/sim/DMA.cpp
@@ -0,0 +1,124 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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 "hal/DMA.h"
+
+extern "C" {
+HAL_DMAHandle HAL_InitializeDMA(int32_t* status) { return HAL_kInvalidHandle; }
+void HAL_FreeDMA(HAL_DMAHandle handle) {}
+
+void HAL_SetDMAPause(HAL_DMAHandle handle, HAL_Bool pause, int32_t* status) {}
+void HAL_SetDMARate(HAL_DMAHandle handle, int32_t cycles, int32_t* status) {}
+
+void HAL_AddDMAEncoder(HAL_DMAHandle handle, HAL_EncoderHandle encoderHandle,
+ int32_t* status) {}
+void HAL_AddDMAEncoderPeriod(HAL_DMAHandle handle,
+ HAL_EncoderHandle encoderHandle, int32_t* status) {
+}
+void HAL_AddDMACounter(HAL_DMAHandle handle, HAL_CounterHandle counterHandle,
+ int32_t* status) {}
+void HAL_AddDMACounterPeriod(HAL_DMAHandle handle,
+ HAL_CounterHandle counterHandle, int32_t* status) {
+}
+void HAL_AddDMADigitalSource(HAL_DMAHandle handle,
+ HAL_Handle digitalSourceHandle, int32_t* status) {}
+void HAL_AddDMAAnalogInput(HAL_DMAHandle handle,
+ HAL_AnalogInputHandle aInHandle, int32_t* status) {}
+
+void HAL_AddDMAAveragedAnalogInput(HAL_DMAHandle handle,
+ HAL_AnalogInputHandle aInHandle,
+ int32_t* status) {}
+
+void HAL_AddDMAAnalogAccumulator(HAL_DMAHandle handle,
+ HAL_AnalogInputHandle aInHandle,
+ int32_t* status) {}
+
+void HAL_AddDMADutyCycle(HAL_DMAHandle handle,
+ HAL_DutyCycleHandle dutyCycleHandle, int32_t* status) {
+}
+
+void HAL_SetDMAExternalTrigger(HAL_DMAHandle handle,
+ HAL_Handle digitalSourceHandle,
+ HAL_AnalogTriggerType analogTriggerType,
+ HAL_Bool rising, HAL_Bool falling,
+ int32_t* status) {}
+
+void HAL_StartDMA(HAL_DMAHandle handle, int32_t queueDepth, int32_t* status) {}
+void HAL_StopDMA(HAL_DMAHandle handle, int32_t* status) {}
+
+void* HAL_GetDMADirectPointer(HAL_DMAHandle handle) { return nullptr; }
+
+enum HAL_DMAReadStatus HAL_ReadDMADirect(void* dmaPointer,
+ HAL_DMASample* dmaSample,
+ int32_t timeoutMs,
+ int32_t* remainingOut,
+ int32_t* status) {
+ return HAL_DMA_ERROR;
+}
+
+enum HAL_DMAReadStatus HAL_ReadDMA(HAL_DMAHandle handle,
+ HAL_DMASample* dmaSample, int32_t timeoutMs,
+ int32_t* remainingOut, int32_t* status) {
+ return HAL_DMA_ERROR;
+}
+
+// Sampling Code
+uint64_t HAL_GetDMASampleTime(const HAL_DMASample* dmaSample, int32_t* status) {
+ return 0;
+}
+
+int32_t HAL_GetDMASampleEncoderRaw(const HAL_DMASample* dmaSample,
+ HAL_EncoderHandle encoderHandle,
+ int32_t* status) {
+ return 0;
+}
+
+int32_t HAL_GetDMASampleCounter(const HAL_DMASample* dmaSample,
+ HAL_CounterHandle counterHandle,
+ int32_t* status) {
+ return 0;
+}
+
+int32_t HAL_GetDMASampleEncoderPeriodRaw(const HAL_DMASample* dmaSample,
+ HAL_EncoderHandle encoderHandle,
+ int32_t* status) {
+ return 0;
+}
+
+int32_t HAL_GetDMASampleCounterPeriod(const HAL_DMASample* dmaSample,
+ HAL_CounterHandle counterHandle,
+ int32_t* status) {
+ return 0;
+}
+HAL_Bool HAL_GetDMASampleDigitalSource(const HAL_DMASample* dmaSample,
+ HAL_Handle dSourceHandle,
+ int32_t* status) {
+ return 0;
+}
+int32_t HAL_GetDMASampleAnalogInputRaw(const HAL_DMASample* dmaSample,
+ HAL_AnalogInputHandle aInHandle,
+ int32_t* status) {
+ return 0;
+}
+
+int32_t HAL_GetDMASampleAveragedAnalogInputRaw(const HAL_DMASample* dmaSample,
+ HAL_AnalogInputHandle aInHandle,
+ int32_t* status) {
+ return 0;
+}
+
+void HAL_GetDMASampleAnalogAccumulator(const HAL_DMASample* dmaSample,
+ HAL_AnalogInputHandle aInHandle,
+ int64_t* count, int64_t* value,
+ int32_t* status) {}
+
+int32_t HAL_GetDMASampleDutyCycleOutputRaw(const HAL_DMASample* dmaSample,
+ HAL_DutyCycleHandle dutyCycleHandle,
+ int32_t* status) {
+ return 0;
+}
+} // extern "C"
diff --git a/hal/src/main/native/sim/DigitalInternal.cpp b/hal/src/main/native/sim/DigitalInternal.cpp
new file mode 100644
index 0000000..070754a
--- /dev/null
+++ b/hal/src/main/native/sim/DigitalInternal.cpp
@@ -0,0 +1,77 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 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 "DigitalInternal.h"
+
+#include "ConstantsInternal.h"
+#include "PortsInternal.h"
+#include "hal/AnalogTrigger.h"
+#include "hal/HAL.h"
+#include "hal/Ports.h"
+
+namespace hal {
+
+DigitalHandleResource<HAL_DigitalHandle, DigitalPort,
+ kNumDigitalChannels + kNumPWMHeaders>*
+ digitalChannelHandles;
+
+namespace init {
+void InitializeDigitalInternal() {
+ static DigitalHandleResource<HAL_DigitalHandle, DigitalPort,
+ kNumDigitalChannels + kNumPWMHeaders>
+ dcH;
+ digitalChannelHandles = &dcH;
+}
+} // namespace init
+
+bool remapDigitalSource(HAL_Handle digitalSourceHandle,
+ HAL_AnalogTriggerType analogTriggerType,
+ uint8_t& channel, uint8_t& module,
+ bool& analogTrigger) {
+ if (isHandleType(digitalSourceHandle, HAL_HandleEnum::AnalogTrigger)) {
+ // If handle passed, index is not negative
+ int32_t index = getHandleIndex(digitalSourceHandle);
+ channel = (index << 2) + analogTriggerType;
+ module = channel >> 4;
+ analogTrigger = true;
+ return true;
+ } else if (isHandleType(digitalSourceHandle, HAL_HandleEnum::DIO)) {
+ int32_t index = getHandleIndex(digitalSourceHandle);
+ if (index >= kNumDigitalHeaders) {
+ channel = remapMXPChannel(index);
+ module = 1;
+ } else {
+ channel = index;
+ module = 0;
+ }
+ analogTrigger = false;
+ return true;
+ } else {
+ return false;
+ }
+}
+
+int32_t remapMXPChannel(int32_t channel) { return channel - 10; }
+
+int32_t remapMXPPWMChannel(int32_t channel) {
+ if (channel < 14) {
+ return channel - 10; // first block of 4 pwms (MXP 0-3)
+ } else {
+ return channel - 6; // block of PWMs after SPI
+ }
+}
+
+int32_t GetDigitalInputChannel(HAL_DigitalHandle handle, int32_t* status) {
+ auto digital = digitalChannelHandles->Get(handle, HAL_HandleEnum::DIO);
+ if (digital == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return -1;
+ }
+
+ return digital->channel;
+}
+} // namespace hal
diff --git a/hal/src/main/native/sim/DigitalInternal.h b/hal/src/main/native/sim/DigitalInternal.h
new file mode 100644
index 0000000..9df4c51
--- /dev/null
+++ b/hal/src/main/native/sim/DigitalInternal.h
@@ -0,0 +1,93 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 <stdint.h>
+
+#include <memory>
+
+#include "PortsInternal.h"
+#include "hal/AnalogTrigger.h"
+#include "hal/Ports.h"
+#include "hal/Types.h"
+#include "hal/handles/DigitalHandleResource.h"
+#include "hal/handles/HandlesInternal.h"
+
+namespace hal {
+/**
+ * MXP channels when used as digital output PWM are offset from actual value
+ */
+constexpr int32_t kMXPDigitalPWMOffset = 6;
+
+constexpr int32_t kExpectedLoopTiming = 40;
+
+/**
+ * kDefaultPwmPeriod is in ms
+ *
+ * - 20ms periods (50 Hz) are the "safest" setting in that this works for all
+ * devices
+ * - 20ms periods seem to be desirable for Vex Motors
+ * - 20ms periods are the specified period for HS-322HD servos, but work
+ * reliably down to 10.0 ms; starting at about 8.5ms, the servo sometimes hums
+ * and get hot; by 5.0ms the hum is nearly continuous
+ * - 10ms periods work well for Victor 884
+ * - 5ms periods allows higher update rates for Luminary Micro Jaguar speed
+ * controllers. Due to the shipping firmware on the Jaguar, we can't run the
+ * update period less than 5.05 ms.
+ *
+ * kDefaultPwmPeriod is the 1x period (5.05 ms). In hardware, the period
+ * scaling is implemented as an output squelch to get longer periods for old
+ * devices.
+ */
+constexpr float kDefaultPwmPeriod = 5.05f;
+/**
+ * kDefaultPwmCenter is the PWM range center in ms
+ */
+constexpr float kDefaultPwmCenter = 1.5f;
+/**
+ * kDefaultPWMStepsDown is the number of PWM steps below the centerpoint
+ */
+constexpr int32_t kDefaultPwmStepsDown = 1000;
+constexpr int32_t kPwmDisabled = 0;
+
+struct DigitalPort {
+ uint8_t channel;
+ bool configSet = false;
+ bool eliminateDeadband = false;
+ int32_t maxPwm = 0;
+ int32_t deadbandMaxPwm = 0;
+ int32_t centerPwm = 0;
+ int32_t deadbandMinPwm = 0;
+ int32_t minPwm = 0;
+};
+
+extern DigitalHandleResource<HAL_DigitalHandle, DigitalPort,
+ kNumDigitalChannels + kNumPWMHeaders>*
+ digitalChannelHandles;
+
+/**
+ * Remap the digital source channel and set the module.
+ *
+ * If it's an analog trigger, determine the module from the high order routing
+ * channel else do normal digital input remapping based on channel number
+ * (MXP).
+ */
+bool remapDigitalSource(HAL_Handle digitalSourceHandle,
+ HAL_AnalogTriggerType analogTriggerType,
+ uint8_t& channel, uint8_t& module, bool& analogTrigger);
+
+/**
+ * Map DIO channel numbers from their physical number (10 to 26) to their
+ * position in the bit field.
+ */
+int32_t remapMXPChannel(int32_t channel);
+
+int32_t remapMXPPWMChannel(int32_t channel);
+
+int32_t GetDigitalInputChannel(HAL_DigitalHandle handle, int32_t* status);
+} // namespace hal
diff --git a/hal/src/main/native/sim/DriverStation.cpp b/hal/src/main/native/sim/DriverStation.cpp
new file mode 100644
index 0000000..e8404cf
--- /dev/null
+++ b/hal/src/main/native/sim/DriverStation.cpp
@@ -0,0 +1,335 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "hal/DriverStation.h"
+
+#ifdef __APPLE__
+#include <pthread.h>
+#endif
+
+#include <cstdio>
+#include <cstdlib>
+#include <cstring>
+#include <string>
+
+#include <wpi/condition_variable.h>
+#include <wpi/mutex.h>
+
+#include "HALInitializer.h"
+#include "mockdata/DriverStationDataInternal.h"
+#include "mockdata/MockHooks.h"
+
+static wpi::mutex msgMutex;
+static wpi::condition_variable* newDSDataAvailableCond;
+static wpi::mutex newDSDataAvailableMutex;
+static int newDSDataAvailableCounter{0};
+static std::atomic_bool isFinalized{false};
+static std::atomic<HALSIM_SendErrorHandler> sendErrorHandler{nullptr};
+
+namespace hal {
+namespace init {
+void InitializeDriverStation() {
+ static wpi::condition_variable nddaC;
+ newDSDataAvailableCond = &nddaC;
+}
+} // namespace init
+} // namespace hal
+
+using namespace hal;
+
+extern "C" {
+
+void HALSIM_SetSendError(HALSIM_SendErrorHandler handler) {
+ sendErrorHandler.store(handler);
+}
+
+int32_t HAL_SendError(HAL_Bool isError, int32_t errorCode, HAL_Bool isLVCode,
+ const char* details, const char* location,
+ const char* callStack, HAL_Bool printMsg) {
+ auto errorHandler = sendErrorHandler.load();
+ if (errorHandler)
+ return errorHandler(isError, errorCode, isLVCode, details, location,
+ callStack, printMsg);
+ // Avoid flooding console by keeping track of previous 5 error
+ // messages and only printing again if they're longer than 1 second old.
+ static constexpr int KEEP_MSGS = 5;
+ std::scoped_lock lock(msgMutex);
+ static std::string prevMsg[KEEP_MSGS];
+ static std::chrono::time_point<std::chrono::steady_clock>
+ prevMsgTime[KEEP_MSGS];
+ static bool initialized = false;
+ if (!initialized) {
+ for (int i = 0; i < KEEP_MSGS; i++) {
+ prevMsgTime[i] =
+ std::chrono::steady_clock::now() - std::chrono::seconds(2);
+ }
+ initialized = true;
+ }
+
+ auto curTime = std::chrono::steady_clock::now();
+ int i;
+ for (i = 0; i < KEEP_MSGS; ++i) {
+ if (prevMsg[i] == details) break;
+ }
+ int retval = 0;
+ if (i == KEEP_MSGS || (curTime - prevMsgTime[i]) >= std::chrono::seconds(1)) {
+ printMsg = true;
+ if (printMsg) {
+ if (location && location[0] != '\0') {
+ std::fprintf(stderr, "%s at %s: ", isError ? "Error" : "Warning",
+ location);
+ }
+ std::fprintf(stderr, "%s\n", details);
+ if (callStack && callStack[0] != '\0') {
+ std::fprintf(stderr, "%s\n", callStack);
+ }
+ }
+ if (i == KEEP_MSGS) {
+ // replace the oldest one
+ i = 0;
+ auto first = prevMsgTime[0];
+ for (int j = 1; j < KEEP_MSGS; ++j) {
+ if (prevMsgTime[j] < first) {
+ first = prevMsgTime[j];
+ i = j;
+ }
+ }
+ prevMsg[i] = details;
+ }
+ prevMsgTime[i] = curTime;
+ }
+ return retval;
+}
+
+int32_t HAL_GetControlWord(HAL_ControlWord* controlWord) {
+ controlWord->enabled = SimDriverStationData->enabled;
+ controlWord->autonomous = SimDriverStationData->autonomous;
+ controlWord->test = SimDriverStationData->test;
+ controlWord->eStop = SimDriverStationData->eStop;
+ controlWord->fmsAttached = SimDriverStationData->fmsAttached;
+ controlWord->dsAttached = SimDriverStationData->dsAttached;
+ return 0;
+}
+
+HAL_AllianceStationID HAL_GetAllianceStation(int32_t* status) {
+ *status = 0;
+ return SimDriverStationData->allianceStationId;
+}
+
+int32_t HAL_GetJoystickAxes(int32_t joystickNum, HAL_JoystickAxes* axes) {
+ SimDriverStationData->GetJoystickAxes(joystickNum, axes);
+ return 0;
+}
+
+int32_t HAL_GetJoystickPOVs(int32_t joystickNum, HAL_JoystickPOVs* povs) {
+ SimDriverStationData->GetJoystickPOVs(joystickNum, povs);
+ return 0;
+}
+
+int32_t HAL_GetJoystickButtons(int32_t joystickNum,
+ HAL_JoystickButtons* buttons) {
+ SimDriverStationData->GetJoystickButtons(joystickNum, buttons);
+ return 0;
+}
+
+int32_t HAL_GetJoystickDescriptor(int32_t joystickNum,
+ HAL_JoystickDescriptor* desc) {
+ SimDriverStationData->GetJoystickDescriptor(joystickNum, desc);
+ return 0;
+}
+
+HAL_Bool HAL_GetJoystickIsXbox(int32_t joystickNum) {
+ HAL_JoystickDescriptor desc;
+ SimDriverStationData->GetJoystickDescriptor(joystickNum, &desc);
+ return desc.isXbox;
+}
+
+int32_t HAL_GetJoystickType(int32_t joystickNum) {
+ HAL_JoystickDescriptor desc;
+ SimDriverStationData->GetJoystickDescriptor(joystickNum, &desc);
+ return desc.type;
+}
+
+char* HAL_GetJoystickName(int32_t joystickNum) {
+ HAL_JoystickDescriptor desc;
+ SimDriverStationData->GetJoystickDescriptor(joystickNum, &desc);
+ size_t len = std::strlen(desc.name);
+ char* name = static_cast<char*>(std::malloc(len + 1));
+ std::memcpy(name, desc.name, len + 1);
+ return name;
+}
+
+void HAL_FreeJoystickName(char* name) { std::free(name); }
+
+int32_t HAL_GetJoystickAxisType(int32_t joystickNum, int32_t axis) { return 0; }
+
+int32_t HAL_SetJoystickOutputs(int32_t joystickNum, int64_t outputs,
+ int32_t leftRumble, int32_t rightRumble) {
+ SimDriverStationData->SetJoystickOutputs(joystickNum, outputs, leftRumble,
+ rightRumble);
+ return 0;
+}
+
+double HAL_GetMatchTime(int32_t* status) {
+ return SimDriverStationData->matchTime;
+}
+
+int32_t HAL_GetMatchInfo(HAL_MatchInfo* info) {
+ SimDriverStationData->GetMatchInfo(info);
+ return 0;
+}
+
+void HAL_ObserveUserProgramStarting(void) { HALSIM_SetProgramStarted(); }
+
+void HAL_ObserveUserProgramDisabled(void) {
+ // TODO
+}
+
+void HAL_ObserveUserProgramAutonomous(void) {
+ // TODO
+}
+
+void HAL_ObserveUserProgramTeleop(void) {
+ // TODO
+}
+
+void HAL_ObserveUserProgramTest(void) {
+ // TODO
+}
+
+#ifdef __APPLE__
+static pthread_key_t lastCountKey;
+static pthread_once_t lastCountKeyOnce = PTHREAD_ONCE_INIT;
+
+static void InitLastCountKey(void) {
+ pthread_key_create(&lastCountKey, std::free);
+}
+#endif
+
+static int& GetThreadLocalLastCount() {
+ // There is a rollover error condition here. At Packet# = n * (uintmax), this
+ // will return false when instead it should return true. However, this at a
+ // 20ms rate occurs once every 2.7 years of DS connected runtime, so not
+ // worth the cycles to check.
+#ifdef __APPLE__
+ pthread_once(&lastCountKeyOnce, InitLastCountKey);
+ int* lastCountPtr = static_cast<int*>(pthread_getspecific(lastCountKey));
+ if (!lastCountPtr) {
+ lastCountPtr = static_cast<int*>(std::malloc(sizeof(int)));
+ *lastCountPtr = -1;
+ pthread_setspecific(lastCountKey, lastCountPtr);
+ }
+ int& lastCount = *lastCountPtr;
+#else
+ thread_local int lastCount{-1};
+#endif
+ return lastCount;
+}
+
+HAL_Bool HAL_WaitForCachedControlDataTimeout(double timeout) {
+ int& lastCount = GetThreadLocalLastCount();
+ std::unique_lock lock(newDSDataAvailableMutex);
+ int currentCount = newDSDataAvailableCounter;
+ if (lastCount != currentCount) {
+ lastCount = currentCount;
+ return true;
+ }
+
+ if (isFinalized.load()) {
+ return false;
+ }
+
+ auto timeoutTime =
+ std::chrono::steady_clock::now() + std::chrono::duration<double>(timeout);
+
+ while (newDSDataAvailableCounter == currentCount) {
+ if (timeout > 0) {
+ auto timedOut = newDSDataAvailableCond->wait_until(lock, timeoutTime);
+ if (timedOut == std::cv_status::timeout) {
+ return false;
+ }
+ } else {
+ newDSDataAvailableCond->wait(lock);
+ }
+ }
+ return true;
+}
+
+HAL_Bool HAL_IsNewControlData(void) {
+ int& lastCount = GetThreadLocalLastCount();
+ int currentCount = 0;
+ {
+ std::scoped_lock lock(newDSDataAvailableMutex);
+ currentCount = newDSDataAvailableCounter;
+ }
+ if (lastCount == currentCount) return false;
+ lastCount = currentCount;
+ return true;
+}
+
+void HAL_WaitForDSData(void) { HAL_WaitForDSDataTimeout(0); }
+
+HAL_Bool HAL_WaitForDSDataTimeout(double timeout) {
+ if (isFinalized.load()) {
+ return false;
+ }
+ auto timeoutTime =
+ std::chrono::steady_clock::now() + std::chrono::duration<double>(timeout);
+
+ std::unique_lock lock(newDSDataAvailableMutex);
+ int currentCount = newDSDataAvailableCounter;
+ while (newDSDataAvailableCounter == currentCount) {
+ if (timeout > 0) {
+ auto timedOut = newDSDataAvailableCond->wait_until(lock, timeoutTime);
+ if (timedOut == std::cv_status::timeout) {
+ return false;
+ }
+ } else {
+ newDSDataAvailableCond->wait(lock);
+ }
+ }
+ return true;
+}
+
+// Constant number to be used for our occur handle
+constexpr int32_t refNumber = 42;
+
+static int32_t newDataOccur(uint32_t refNum) {
+ // Since we could get other values, require our specific handle
+ // to signal our threads
+ if (refNum != refNumber) return 0;
+ std::scoped_lock lock(newDSDataAvailableMutex);
+ // Nofify all threads
+ newDSDataAvailableCounter++;
+ newDSDataAvailableCond->notify_all();
+ return 0;
+}
+
+void HAL_InitializeDriverStation(void) {
+ hal::init::CheckInit();
+ static std::atomic_bool initialized{false};
+ static wpi::mutex initializeMutex;
+ // Initial check, as if it's true initialization has finished
+ if (initialized) return;
+
+ std::scoped_lock lock(initializeMutex);
+ // Second check in case another thread was waiting
+ if (initialized) return;
+
+ SimDriverStationData->ResetData();
+
+ std::atexit([]() {
+ isFinalized.store(true);
+ HAL_ReleaseDSMutex();
+ });
+
+ initialized = true;
+}
+
+void HAL_ReleaseDSMutex(void) { newDataOccur(refNumber); }
+
+} // extern "C"
diff --git a/hal/src/main/native/sim/DutyCycle.cpp b/hal/src/main/native/sim/DutyCycle.cpp
new file mode 100644
index 0000000..0e14eeb
--- /dev/null
+++ b/hal/src/main/native/sim/DutyCycle.cpp
@@ -0,0 +1,120 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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 "hal/DutyCycle.h"
+
+#include "HALInitializer.h"
+#include "PortsInternal.h"
+#include "hal/Errors.h"
+#include "hal/handles/HandlesInternal.h"
+#include "hal/handles/LimitedHandleResource.h"
+#include "mockdata/DutyCycleDataInternal.h"
+
+using namespace hal;
+
+namespace {
+struct DutyCycle {
+ uint8_t index;
+};
+struct Empty {};
+} // namespace
+
+static LimitedHandleResource<HAL_DutyCycleHandle, DutyCycle, kNumDutyCycles,
+ HAL_HandleEnum::DutyCycle>* dutyCycleHandles;
+
+namespace hal {
+namespace init {
+void InitializeDutyCycle() {
+ static LimitedHandleResource<HAL_DutyCycleHandle, DutyCycle, kNumDutyCycles,
+ HAL_HandleEnum::DutyCycle>
+ dcH;
+ dutyCycleHandles = &dcH;
+}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+HAL_DutyCycleHandle HAL_InitializeDutyCycle(HAL_Handle digitalSourceHandle,
+ HAL_AnalogTriggerType triggerType,
+ int32_t* status) {
+ hal::init::CheckInit();
+
+ HAL_DutyCycleHandle handle = dutyCycleHandles->Allocate();
+ if (handle == HAL_kInvalidHandle) {
+ *status = NO_AVAILABLE_RESOURCES;
+ return HAL_kInvalidHandle;
+ }
+
+ auto dutyCycle = dutyCycleHandles->Get(handle);
+ if (dutyCycle == nullptr) { // would only occur on thread issue
+ *status = HAL_HANDLE_ERROR;
+ return HAL_kInvalidHandle;
+ }
+
+ int16_t index = getHandleIndex(handle);
+ SimDutyCycleData[index].digitalChannel = getHandleIndex(digitalSourceHandle);
+ SimDutyCycleData[index].initialized = true;
+ SimDutyCycleData[index].simDevice = 0;
+ dutyCycle->index = index;
+ return handle;
+}
+void HAL_FreeDutyCycle(HAL_DutyCycleHandle dutyCycleHandle) {
+ auto dutyCycle = dutyCycleHandles->Get(dutyCycleHandle);
+ dutyCycleHandles->Free(dutyCycleHandle);
+ if (dutyCycle == nullptr) return;
+ SimDutyCycleData[dutyCycle->index].initialized = false;
+}
+
+void HAL_SetDutyCycleSimDevice(HAL_EncoderHandle handle,
+ HAL_SimDeviceHandle device) {
+ auto dutyCycle = dutyCycleHandles->Get(handle);
+ if (dutyCycle == nullptr) return;
+ SimDutyCycleData[dutyCycle->index].simDevice = device;
+}
+
+int32_t HAL_GetDutyCycleFrequency(HAL_DutyCycleHandle dutyCycleHandle,
+ int32_t* status) {
+ auto dutyCycle = dutyCycleHandles->Get(dutyCycleHandle);
+ if (dutyCycle == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+ return SimDutyCycleData[dutyCycle->index].frequency;
+}
+double HAL_GetDutyCycleOutput(HAL_DutyCycleHandle dutyCycleHandle,
+ int32_t* status) {
+ auto dutyCycle = dutyCycleHandles->Get(dutyCycleHandle);
+ if (dutyCycle == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+ return SimDutyCycleData[dutyCycle->index].output;
+}
+int32_t HAL_GetDutyCycleOutputRaw(HAL_DutyCycleHandle dutyCycleHandle,
+ int32_t* status) {
+ auto dutyCycle = dutyCycleHandles->Get(dutyCycleHandle);
+ if (dutyCycle == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+ return SimDutyCycleData[dutyCycle->index].output *
+ HAL_GetDutyCycleOutputScaleFactor(dutyCycleHandle, status);
+}
+int32_t HAL_GetDutyCycleOutputScaleFactor(HAL_DutyCycleHandle dutyCycleHandle,
+ int32_t* status) {
+ return 4e7 - 1;
+}
+int32_t HAL_GetDutyCycleFPGAIndex(HAL_DutyCycleHandle dutyCycleHandle,
+ int32_t* status) {
+ auto dutyCycle = dutyCycleHandles->Get(dutyCycleHandle);
+ if (dutyCycle == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return -1;
+ }
+ return dutyCycle->index;
+}
+} // extern "C"
diff --git a/hal/src/main/native/sim/Encoder.cpp b/hal/src/main/native/sim/Encoder.cpp
new file mode 100644
index 0000000..36122bf
--- /dev/null
+++ b/hal/src/main/native/sim/Encoder.cpp
@@ -0,0 +1,354 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "hal/Encoder.h"
+
+#include "CounterInternal.h"
+#include "HALInitializer.h"
+#include "PortsInternal.h"
+#include "hal/Counter.h"
+#include "hal/Errors.h"
+#include "hal/handles/HandlesInternal.h"
+#include "hal/handles/LimitedHandleResource.h"
+#include "mockdata/EncoderDataInternal.h"
+
+using namespace hal;
+
+namespace {
+struct Encoder {
+ HAL_Handle nativeHandle;
+ HAL_EncoderEncodingType encodingType;
+ double distancePerPulse;
+ uint8_t index;
+};
+struct Empty {};
+} // namespace
+
+static LimitedHandleResource<HAL_EncoderHandle, Encoder,
+ kNumEncoders + kNumCounters,
+ HAL_HandleEnum::Encoder>* encoderHandles;
+
+static LimitedHandleResource<HAL_FPGAEncoderHandle, Empty, kNumEncoders,
+ HAL_HandleEnum::FPGAEncoder>* fpgaEncoderHandles;
+
+namespace hal {
+namespace init {
+void InitializeEncoder() {
+ static LimitedHandleResource<HAL_FPGAEncoderHandle, Empty, kNumEncoders,
+ HAL_HandleEnum::FPGAEncoder>
+ feH;
+ fpgaEncoderHandles = &feH;
+ static LimitedHandleResource<HAL_EncoderHandle, Encoder,
+ kNumEncoders + kNumCounters,
+ HAL_HandleEnum::Encoder>
+ eH;
+ encoderHandles = &eH;
+}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+HAL_EncoderHandle HAL_InitializeEncoder(
+ HAL_Handle digitalSourceHandleA, HAL_AnalogTriggerType analogTriggerTypeA,
+ HAL_Handle digitalSourceHandleB, HAL_AnalogTriggerType analogTriggerTypeB,
+ HAL_Bool reverseDirection, HAL_EncoderEncodingType encodingType,
+ int32_t* status) {
+ hal::init::CheckInit();
+ HAL_Handle nativeHandle = HAL_kInvalidHandle;
+ if (encodingType == HAL_EncoderEncodingType::HAL_Encoder_k4X) {
+ // k4x, allocate encoder
+ nativeHandle = fpgaEncoderHandles->Allocate();
+ } else {
+ // k2x or k1x, allocate counter
+ nativeHandle = counterHandles->Allocate();
+ }
+ if (nativeHandle == HAL_kInvalidHandle) {
+ *status = NO_AVAILABLE_RESOURCES;
+ return HAL_kInvalidHandle;
+ }
+ auto handle = encoderHandles->Allocate();
+ if (handle == HAL_kInvalidHandle) {
+ *status = NO_AVAILABLE_RESOURCES;
+ return HAL_kInvalidHandle;
+ }
+ auto encoder = encoderHandles->Get(handle);
+ if (encoder == nullptr) { // would only occur on thread issue
+ *status = HAL_HANDLE_ERROR;
+ return HAL_kInvalidHandle;
+ }
+ int16_t index = getHandleIndex(handle);
+ SimEncoderData[index].digitalChannelA = getHandleIndex(digitalSourceHandleA);
+ SimEncoderData[index].digitalChannelB = getHandleIndex(digitalSourceHandleB);
+ SimEncoderData[index].initialized = true;
+ SimEncoderData[index].reverseDirection = reverseDirection;
+ SimEncoderData[index].simDevice = 0;
+ // TODO: Add encoding type to Sim data
+ encoder->index = index;
+ encoder->nativeHandle = nativeHandle;
+ encoder->encodingType = encodingType;
+ encoder->distancePerPulse = 1.0;
+ return handle;
+}
+
+void HAL_FreeEncoder(HAL_EncoderHandle encoderHandle, int32_t* status) {
+ auto encoder = encoderHandles->Get(encoderHandle);
+ encoderHandles->Free(encoderHandle);
+ if (encoder == nullptr) return;
+ if (isHandleType(encoder->nativeHandle, HAL_HandleEnum::FPGAEncoder)) {
+ fpgaEncoderHandles->Free(encoder->nativeHandle);
+ } else if (isHandleType(encoder->nativeHandle, HAL_HandleEnum::Counter)) {
+ counterHandles->Free(encoder->nativeHandle);
+ }
+ SimEncoderData[encoder->index].initialized = false;
+}
+
+void HAL_SetEncoderSimDevice(HAL_EncoderHandle handle,
+ HAL_SimDeviceHandle device) {
+ auto encoder = encoderHandles->Get(handle);
+ if (encoder == nullptr) return;
+ SimEncoderData[encoder->index].simDevice = device;
+}
+
+static inline int EncodingScaleFactor(Encoder* encoder) {
+ switch (encoder->encodingType) {
+ case HAL_Encoder_k1X:
+ return 1;
+ case HAL_Encoder_k2X:
+ return 2;
+ case HAL_Encoder_k4X:
+ return 4;
+ default:
+ return 0;
+ }
+}
+
+static inline double DecodingScaleFactor(Encoder* encoder) {
+ switch (encoder->encodingType) {
+ case HAL_Encoder_k1X:
+ return 1.0;
+ case HAL_Encoder_k2X:
+ return 0.5;
+ case HAL_Encoder_k4X:
+ return 0.25;
+ default:
+ return 0.0;
+ }
+}
+
+int32_t HAL_GetEncoder(HAL_EncoderHandle encoderHandle, int32_t* status) {
+ auto encoder = encoderHandles->Get(encoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+
+ return SimEncoderData[encoder->index].count;
+}
+int32_t HAL_GetEncoderRaw(HAL_EncoderHandle encoderHandle, int32_t* status) {
+ auto encoder = encoderHandles->Get(encoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+
+ return SimEncoderData[encoder->index].count /
+ DecodingScaleFactor(encoder.get());
+}
+int32_t HAL_GetEncoderEncodingScale(HAL_EncoderHandle encoderHandle,
+ int32_t* status) {
+ auto encoder = encoderHandles->Get(encoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+
+ return EncodingScaleFactor(encoder.get());
+}
+void HAL_ResetEncoder(HAL_EncoderHandle encoderHandle, int32_t* status) {
+ auto encoder = encoderHandles->Get(encoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ SimEncoderData[encoder->index].count = 0;
+ SimEncoderData[encoder->index].period = std::numeric_limits<double>::max();
+ SimEncoderData[encoder->index].reset = true;
+}
+double HAL_GetEncoderPeriod(HAL_EncoderHandle encoderHandle, int32_t* status) {
+ auto encoder = encoderHandles->Get(encoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+
+ return SimEncoderData[encoder->index].period;
+}
+void HAL_SetEncoderMaxPeriod(HAL_EncoderHandle encoderHandle, double maxPeriod,
+ int32_t* status) {
+ auto encoder = encoderHandles->Get(encoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ SimEncoderData[encoder->index].maxPeriod = maxPeriod;
+}
+HAL_Bool HAL_GetEncoderStopped(HAL_EncoderHandle encoderHandle,
+ int32_t* status) {
+ auto encoder = encoderHandles->Get(encoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+
+ return SimEncoderData[encoder->index].period >
+ SimEncoderData[encoder->index].maxPeriod;
+}
+HAL_Bool HAL_GetEncoderDirection(HAL_EncoderHandle encoderHandle,
+ int32_t* status) {
+ auto encoder = encoderHandles->Get(encoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+
+ return SimEncoderData[encoder->index].direction;
+}
+double HAL_GetEncoderDistance(HAL_EncoderHandle encoderHandle,
+ int32_t* status) {
+ auto encoder = encoderHandles->Get(encoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+
+ return SimEncoderData[encoder->index].count * encoder->distancePerPulse;
+}
+double HAL_GetEncoderRate(HAL_EncoderHandle encoderHandle, int32_t* status) {
+ auto encoder = encoderHandles->Get(encoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+
+ return encoder->distancePerPulse / SimEncoderData[encoder->index].period;
+}
+void HAL_SetEncoderMinRate(HAL_EncoderHandle encoderHandle, double minRate,
+ int32_t* status) {
+ auto encoder = encoderHandles->Get(encoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (minRate == 0.0) {
+ *status = PARAMETER_OUT_OF_RANGE;
+ return;
+ }
+
+ SimEncoderData[encoder->index].maxPeriod =
+ encoder->distancePerPulse / minRate;
+}
+void HAL_SetEncoderDistancePerPulse(HAL_EncoderHandle encoderHandle,
+ double distancePerPulse, int32_t* status) {
+ auto encoder = encoderHandles->Get(encoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ if (distancePerPulse == 0.0) {
+ *status = PARAMETER_OUT_OF_RANGE;
+ return;
+ }
+ encoder->distancePerPulse = distancePerPulse;
+ SimEncoderData[encoder->index].distancePerPulse = distancePerPulse;
+}
+void HAL_SetEncoderReverseDirection(HAL_EncoderHandle encoderHandle,
+ HAL_Bool reverseDirection,
+ int32_t* status) {
+ auto encoder = encoderHandles->Get(encoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ SimEncoderData[encoder->index].reverseDirection = reverseDirection;
+}
+void HAL_SetEncoderSamplesToAverage(HAL_EncoderHandle encoderHandle,
+ int32_t samplesToAverage, int32_t* status) {
+ auto encoder = encoderHandles->Get(encoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ SimEncoderData[encoder->index].samplesToAverage = samplesToAverage;
+}
+int32_t HAL_GetEncoderSamplesToAverage(HAL_EncoderHandle encoderHandle,
+ int32_t* status) {
+ auto encoder = encoderHandles->Get(encoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+
+ return SimEncoderData[encoder->index].samplesToAverage;
+}
+
+void HAL_SetEncoderIndexSource(HAL_EncoderHandle encoderHandle,
+ HAL_Handle digitalSourceHandle,
+ HAL_AnalogTriggerType analogTriggerType,
+ HAL_EncoderIndexingType type, int32_t* status) {
+ // Not implemented yet
+}
+
+int32_t HAL_GetEncoderFPGAIndex(HAL_EncoderHandle encoderHandle,
+ int32_t* status) {
+ auto encoder = encoderHandles->Get(encoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+
+ return encoder->index;
+}
+
+double HAL_GetEncoderDecodingScaleFactor(HAL_EncoderHandle encoderHandle,
+ int32_t* status) {
+ auto encoder = encoderHandles->Get(encoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0.0;
+ }
+
+ return DecodingScaleFactor(encoder.get());
+}
+
+double HAL_GetEncoderDistancePerPulse(HAL_EncoderHandle encoderHandle,
+ int32_t* status) {
+ auto encoder = encoderHandles->Get(encoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0.0;
+ }
+
+ return encoder->distancePerPulse;
+}
+
+HAL_EncoderEncodingType HAL_GetEncoderEncodingType(
+ HAL_EncoderHandle encoderHandle, int32_t* status) {
+ auto encoder = encoderHandles->Get(encoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return HAL_Encoder_k4X; // default to k4x
+ }
+
+ return encoder->encodingType;
+}
+} // extern "C"
diff --git a/hal/src/main/native/sim/ErrorsInternal.h b/hal/src/main/native/sim/ErrorsInternal.h
new file mode 100644
index 0000000..55372d8
--- /dev/null
+++ b/hal/src/main/native/sim/ErrorsInternal.h
@@ -0,0 +1,448 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 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
+
+typedef enum {
+ CTR_OKAY, // No Error - Function executed as expected
+ CTR_RxTimeout, // CAN frame has not been received within specified period of
+ // time.
+ CTR_TxTimeout, // Not used.
+ CTR_InvalidParamValue, // Caller passed an invalid param
+ CTR_UnexpectedArbId, // Specified CAN Id is invalid.
+ CTR_TxFailed, // Could not transmit the CAN frame.
+ CTR_SigNotUpdated, // Have not received an value response for signal.
+ CTR_BufferFull, // Caller attempted to insert data into a buffer that is
+ // full.
+} CTR_Code;
+
+// VISA Error
+#define _VI_ERROR (-2147483647L - 1)
+#define VI_ERROR_SYSTEM_ERROR (_VI_ERROR + 0x3FFF0000L)
+#define VI_ERROR_INV_OBJECT (_VI_ERROR + 0x3FFF000EL)
+#define VI_ERROR_RSRC_LOCKED (_VI_ERROR + 0x3FFF000FL)
+#define VI_ERROR_INV_EXPR (_VI_ERROR + 0x3FFF0010L)
+#define VI_ERROR_RSRC_NFOUND (_VI_ERROR + 0x3FFF0011L)
+#define VI_ERROR_INV_RSRC_NAME (_VI_ERROR + 0x3FFF0012L)
+#define VI_ERROR_INV_ACC_MODE (_VI_ERROR + 0x3FFF0013L)
+#define VI_ERROR_TMO (_VI_ERROR + 0x3FFF0015L)
+#define VI_ERROR_CLOSING_FAILED (_VI_ERROR + 0x3FFF0016L)
+#define VI_ERROR_INV_DEGREE (_VI_ERROR + 0x3FFF001BL)
+#define VI_ERROR_INV_JOB_ID (_VI_ERROR + 0x3FFF001CL)
+#define VI_ERROR_NSUP_ATTR (_VI_ERROR + 0x3FFF001DL)
+#define VI_ERROR_NSUP_ATTR_STATE (_VI_ERROR + 0x3FFF001EL)
+#define VI_ERROR_ATTR_READONLY (_VI_ERROR + 0x3FFF001FL)
+#define VI_ERROR_INV_LOCK_TYPE (_VI_ERROR + 0x3FFF0020L)
+#define VI_ERROR_INV_ACCESS_KEY (_VI_ERROR + 0x3FFF0021L)
+#define VI_ERROR_INV_EVENT (_VI_ERROR + 0x3FFF0026L)
+#define VI_ERROR_INV_MECH (_VI_ERROR + 0x3FFF0027L)
+#define VI_ERROR_HNDLR_NINSTALLED (_VI_ERROR + 0x3FFF0028L)
+#define VI_ERROR_INV_HNDLR_REF (_VI_ERROR + 0x3FFF0029L)
+#define VI_ERROR_INV_CONTEXT (_VI_ERROR + 0x3FFF002AL)
+#define VI_ERROR_QUEUE_OVERFLOW (_VI_ERROR + 0x3FFF002DL)
+#define VI_ERROR_NENABLED (_VI_ERROR + 0x3FFF002FL)
+#define VI_ERROR_ABORT (_VI_ERROR + 0x3FFF0030L)
+#define VI_ERROR_RAW_WR_PROT_VIOL (_VI_ERROR + 0x3FFF0034L)
+#define VI_ERROR_RAW_RD_PROT_VIOL (_VI_ERROR + 0x3FFF0035L)
+#define VI_ERROR_OUTP_PROT_VIOL (_VI_ERROR + 0x3FFF0036L)
+#define VI_ERROR_INP_PROT_VIOL (_VI_ERROR + 0x3FFF0037L)
+#define VI_ERROR_BERR (_VI_ERROR + 0x3FFF0038L)
+#define VI_ERROR_IN_PROGRESS (_VI_ERROR + 0x3FFF0039L)
+#define VI_ERROR_INV_SETUP (_VI_ERROR + 0x3FFF003AL)
+#define VI_ERROR_QUEUE_ERROR (_VI_ERROR + 0x3FFF003BL)
+#define VI_ERROR_ALLOC (_VI_ERROR + 0x3FFF003CL)
+#define VI_ERROR_INV_MASK (_VI_ERROR + 0x3FFF003DL)
+#define VI_ERROR_IO (_VI_ERROR + 0x3FFF003EL)
+#define VI_ERROR_INV_FMT (_VI_ERROR + 0x3FFF003FL)
+#define VI_ERROR_NSUP_FMT (_VI_ERROR + 0x3FFF0041L)
+#define VI_ERROR_LINE_IN_USE (_VI_ERROR + 0x3FFF0042L)
+#define VI_ERROR_NSUP_MODE (_VI_ERROR + 0x3FFF0046L)
+#define VI_ERROR_SRQ_NOCCURRED (_VI_ERROR + 0x3FFF004AL)
+#define VI_ERROR_INV_SPACE (_VI_ERROR + 0x3FFF004EL)
+#define VI_ERROR_INV_OFFSET (_VI_ERROR + 0x3FFF0051L)
+#define VI_ERROR_INV_WIDTH (_VI_ERROR + 0x3FFF0052L)
+#define VI_ERROR_NSUP_OFFSET (_VI_ERROR + 0x3FFF0054L)
+#define VI_ERROR_NSUP_VAR_WIDTH (_VI_ERROR + 0x3FFF0055L)
+#define VI_ERROR_WINDOW_NMAPPED (_VI_ERROR + 0x3FFF0057L)
+#define VI_ERROR_RESP_PENDING (_VI_ERROR + 0x3FFF0059L)
+#define VI_ERROR_NLISTENERS (_VI_ERROR + 0x3FFF005FL)
+#define VI_ERROR_NCIC (_VI_ERROR + 0x3FFF0060L)
+#define VI_ERROR_NSYS_CNTLR (_VI_ERROR + 0x3FFF0061L)
+#define VI_ERROR_NSUP_OPER (_VI_ERROR + 0x3FFF0067L)
+#define VI_ERROR_INTR_PENDING (_VI_ERROR + 0x3FFF0068L)
+#define VI_ERROR_ASRL_PARITY (_VI_ERROR + 0x3FFF006AL)
+#define VI_ERROR_ASRL_FRAMING (_VI_ERROR + 0x3FFF006BL)
+#define VI_ERROR_ASRL_OVERRUN (_VI_ERROR + 0x3FFF006CL)
+#define VI_ERROR_TRIG_NMAPPED (_VI_ERROR + 0x3FFF006EL)
+#define VI_ERROR_NSUP_ALIGN_OFFSET (_VI_ERROR + 0x3FFF0070L)
+#define VI_ERROR_USER_BUF (_VI_ERROR + 0x3FFF0071L)
+#define VI_ERROR_RSRC_BUSY (_VI_ERROR + 0x3FFF0072L)
+#define VI_ERROR_NSUP_WIDTH (_VI_ERROR + 0x3FFF0076L)
+#define VI_ERROR_INV_PARAMETER (_VI_ERROR + 0x3FFF0078L)
+#define VI_ERROR_INV_PROT (_VI_ERROR + 0x3FFF0079L)
+#define VI_ERROR_INV_SIZE (_VI_ERROR + 0x3FFF007BL)
+#define VI_ERROR_WINDOW_MAPPED (_VI_ERROR + 0x3FFF0080L)
+#define VI_ERROR_NIMPL_OPER (_VI_ERROR + 0x3FFF0081L)
+#define VI_ERROR_INV_LENGTH (_VI_ERROR + 0x3FFF0083L)
+#define VI_ERROR_INV_MODE (_VI_ERROR + 0x3FFF0091L)
+#define VI_ERROR_SESN_NLOCKED (_VI_ERROR + 0x3FFF009CL)
+#define VI_ERROR_MEM_NSHARED (_VI_ERROR + 0x3FFF009DL)
+#define VI_ERROR_LIBRARY_NFOUND (_VI_ERROR + 0x3FFF009EL)
+#define VI_ERROR_NSUP_INTR (_VI_ERROR + 0x3FFF009FL)
+#define VI_ERROR_INV_LINE (_VI_ERROR + 0x3FFF00A0L)
+#define VI_ERROR_FILE_ACCESS (_VI_ERROR + 0x3FFF00A1L)
+#define VI_ERROR_FILE_IO (_VI_ERROR + 0x3FFF00A2L)
+#define VI_ERROR_NSUP_LINE (_VI_ERROR + 0x3FFF00A3L)
+#define VI_ERROR_NSUP_MECH (_VI_ERROR + 0x3FFF00A4L)
+#define VI_ERROR_INTF_NUM_NCONFIG (_VI_ERROR + 0x3FFF00A5L)
+#define VI_ERROR_CONN_LOST (_VI_ERROR + 0x3FFF00A6L)
+#define VI_ERROR_MACHINE_NAVAIL (_VI_ERROR + 0x3FFF00A7L)
+#define VI_ERROR_NPERMISSION (_VI_ERROR + 0x3FFF00A8L)
+
+// FPGA Errors
+
+/**
+ * Represents the resulting status of a function call through its return value.
+ * 0 is success, negative values are errors, and positive values are warnings.
+ */
+typedef int32_t NiFpga_Status;
+
+/**
+ * No errors or warnings.
+ */
+static const NiFpga_Status NiFpga_Status_Success = 0;
+
+/**
+ * The timeout expired before the FIFO operation could complete.
+ */
+static const NiFpga_Status NiFpga_Status_FifoTimeout = -50400;
+
+/**
+ * No transfer is in progress because the transfer was aborted by the client.
+ * The operation could not be completed as specified.
+ */
+static const NiFpga_Status NiFpga_Status_TransferAborted = -50405;
+
+/**
+ * A memory allocation failed. Try again after rebooting.
+ */
+static const NiFpga_Status NiFpga_Status_MemoryFull = -52000;
+
+/**
+ * An unexpected software error occurred.
+ */
+static const NiFpga_Status NiFpga_Status_SoftwareFault = -52003;
+
+/**
+ * A parameter to a function was not valid. This could be a NULL pointer, a bad
+ * value, etc.
+ */
+static const NiFpga_Status NiFpga_Status_InvalidParameter = -52005;
+
+/**
+ * A required resource was not found. The NiFpga.* library, the RIO resource, or
+ * some other resource may be missing.
+ */
+static const NiFpga_Status NiFpga_Status_ResourceNotFound = -52006;
+
+/**
+ * A required resource was not properly initialized. This could occur if
+ * NiFpga_Initialize was not called or a required NiFpga_IrqContext was not
+ * reserved.
+ */
+static const NiFpga_Status NiFpga_Status_ResourceNotInitialized = -52010;
+
+/**
+ * A hardware failure has occurred. The operation could not be completed as
+ * specified.
+ */
+static const NiFpga_Status NiFpga_Status_HardwareFault = -52018;
+
+/**
+ * The FPGA is already running.
+ */
+static const NiFpga_Status NiFpga_Status_FpgaAlreadyRunning = -61003;
+
+/**
+ * An error occurred downloading the VI to the FPGA device. Verify that
+ * the target is connected and powered and that the resource of the target
+ * is properly configured.
+ */
+static const NiFpga_Status NiFpga_Status_DownloadError = -61018;
+
+/**
+ * The bitfile was not compiled for the specified resource's device type.
+ */
+static const NiFpga_Status NiFpga_Status_DeviceTypeMismatch = -61024;
+
+/**
+ * An error was detected in the communication between the host computer and the
+ * FPGA target.
+ */
+static const NiFpga_Status NiFpga_Status_CommunicationTimeout = -61046;
+
+/**
+ * The timeout expired before any of the IRQs were asserted.
+ */
+static const NiFpga_Status NiFpga_Status_IrqTimeout = -61060;
+
+/**
+ * The specified bitfile is invalid or corrupt.
+ */
+static const NiFpga_Status NiFpga_Status_CorruptBitfile = -61070;
+
+/**
+ * The requested FIFO depth is invalid. It is either 0 or an amount not
+ * supported by the hardware.
+ */
+static const NiFpga_Status NiFpga_Status_BadDepth = -61072;
+
+/**
+ * The number of FIFO elements is invalid. Either the number is greater than the
+ * depth of the host memory DMA FIFO, or more elements were requested for
+ * release than had been acquired.
+ */
+static const NiFpga_Status NiFpga_Status_BadReadWriteCount = -61073;
+
+/**
+ * A hardware clocking error occurred. A derived clock lost lock with its base
+ * clock during the execution of the LabVIEW FPGA VI. If any base clocks with
+ * derived clocks are referencing an external source, make sure that the
+ * external source is connected and within the supported frequency, jitter,
+ * accuracy, duty cycle, and voltage specifications. Also verify that the
+ * characteristics of the base clock match the configuration specified in the
+ * FPGA Base Clock Properties. If all base clocks with derived clocks are
+ * generated from free-running, on-board sources, please contact National
+ * Instruments technical support at ni.com/support.
+ */
+static const NiFpga_Status NiFpga_Status_ClockLostLock = -61083;
+
+/**
+ * The operation could not be performed because the FPGA is busy. Stop all
+ * activities on the FPGA before requesting this operation. If the target is in
+ * Scan Interface programming mode, put it in FPGA Interface programming mode.
+ */
+static const NiFpga_Status NiFpga_Status_FpgaBusy = -61141;
+
+/**
+ * The operation could not be performed because the FPGA is busy operating in
+ * FPGA Interface C API mode. Stop all activities on the FPGA before requesting
+ * this operation.
+ */
+static const NiFpga_Status NiFpga_Status_FpgaBusyFpgaInterfaceCApi = -61200;
+
+/**
+ * The chassis is in Scan Interface programming mode. In order to run FPGA VIs,
+ * you must go to the chassis properties page, select FPGA programming mode, and
+ * deploy settings.
+ */
+static const NiFpga_Status NiFpga_Status_FpgaBusyScanInterface = -61201;
+
+/**
+ * The operation could not be performed because the FPGA is busy operating in
+ * FPGA Interface mode. Stop all activities on the FPGA before requesting this
+ * operation.
+ */
+static const NiFpga_Status NiFpga_Status_FpgaBusyFpgaInterface = -61202;
+
+/**
+ * The operation could not be performed because the FPGA is busy operating in
+ * Interactive mode. Stop all activities on the FPGA before requesting this
+ * operation.
+ */
+static const NiFpga_Status NiFpga_Status_FpgaBusyInteractive = -61203;
+
+/**
+ * The operation could not be performed because the FPGA is busy operating in
+ * Emulation mode. Stop all activities on the FPGA before requesting this
+ * operation.
+ */
+static const NiFpga_Status NiFpga_Status_FpgaBusyEmulation = -61204;
+
+/**
+ * LabVIEW FPGA does not support the Reset method for bitfiles that allow
+ * removal of implicit enable signals in single-cycle Timed Loops.
+ */
+static const NiFpga_Status NiFpga_Status_ResetCalledWithImplicitEnableRemoval =
+ -61211;
+
+/**
+ * LabVIEW FPGA does not support the Abort method for bitfiles that allow
+ * removal of implicit enable signals in single-cycle Timed Loops.
+ */
+static const NiFpga_Status NiFpga_Status_AbortCalledWithImplicitEnableRemoval =
+ -61212;
+
+/**
+ * LabVIEW FPGA does not support Close and Reset if Last Reference for bitfiles
+ * that allow removal of implicit enable signals in single-cycle Timed Loops.
+ * Pass the NiFpga_CloseAttribute_NoResetIfLastSession attribute to NiFpga_Close
+ * instead of 0.
+ */
+static const NiFpga_Status
+ NiFpga_Status_CloseAndResetCalledWithImplicitEnableRemoval = -61213;
+
+/**
+ * For bitfiles that allow removal of implicit enable signals in single-cycle
+ * Timed Loops, LabVIEW FPGA does not support this method prior to running the
+ * bitfile.
+ */
+static const NiFpga_Status NiFpga_Status_ImplicitEnableRemovalButNotYetRun =
+ -61214;
+
+/**
+ * Bitfiles that allow removal of implicit enable signals in single-cycle Timed
+ * Loops can run only once. Download the bitfile again before re-running the VI.
+ */
+static const NiFpga_Status
+ NiFpga_Status_RunAfterStoppedCalledWithImplicitEnableRemoval = -61215;
+
+/**
+ * A gated clock has violated the handshaking protocol. If you are using
+ * external gated clocks, ensure that they follow the required clock gating
+ * protocol. If you are generating your clocks internally, please contact
+ * National Instruments Technical Support.
+ */
+static const NiFpga_Status NiFpga_Status_GatedClockHandshakingViolation =
+ -61216;
+
+/**
+ * The number of elements requested must be less than or equal to the number of
+ * unacquired elements left in the host memory DMA FIFO. There are currently
+ * fewer unacquired elements left in the FIFO than are being requested. Release
+ * some acquired elements before acquiring more elements.
+ */
+static const NiFpga_Status NiFpga_Status_ElementsNotPermissibleToBeAcquired =
+ -61219;
+
+/**
+ * The operation could not be performed because the FPGA is in configuration or
+ * discovery mode. Wait for configuration or discovery to complete and retry
+ * your operation.
+ */
+static const NiFpga_Status NiFpga_Status_FpgaBusyConfiguration = -61252;
+
+/**
+ * An unexpected internal error occurred.
+ */
+static const NiFpga_Status NiFpga_Status_InternalError = -61499;
+
+/**
+ * The NI-RIO driver was unable to allocate memory for a FIFO. This can happen
+ * when the combined depth of all DMA FIFOs exceeds the maximum depth for the
+ * controller, or when the controller runs out of system memory. You may be able
+ * to reconfigure the controller with a greater maximum FIFO depth. For more
+ * information, refer to the NI KnowledgeBase article 65OF2ERQ.
+ */
+static const NiFpga_Status NiFpga_Status_TotalDmaFifoDepthExceeded = -63003;
+
+/**
+ * Access to the remote system was denied. Use MAX to check the Remote Device
+ * Access settings under Software>>NI-RIO>>NI-RIO Settings on the remote system.
+ */
+static const NiFpga_Status NiFpga_Status_AccessDenied = -63033;
+
+/**
+ * The NI-RIO software on the host is not compatible with the software on the
+ * target. Upgrade the NI-RIO software on the host in order to connect to this
+ * target.
+ */
+static const NiFpga_Status NiFpga_Status_HostVersionMismatch = -63038;
+
+/**
+ * A connection could not be established to the specified remote device. Ensure
+ * that the device is on and accessible over the network, that NI-RIO software
+ * is installed, and that the RIO server is running and properly configured.
+ */
+static const NiFpga_Status NiFpga_Status_RpcConnectionError = -63040;
+
+/**
+ * The RPC session is invalid. The target may have reset or been rebooted. Check
+ * the network connection and retry the operation.
+ */
+static const NiFpga_Status NiFpga_Status_RpcSessionError = -63043;
+
+/**
+ * The operation could not complete because another session is accessing the
+ * FIFO. Close the other session and retry.
+ */
+static const NiFpga_Status NiFpga_Status_FifoReserved = -63082;
+
+/**
+ * A Configure FIFO, Stop FIFO, Read FIFO, or Write FIFO function was called
+ * while the host had acquired elements of the FIFO. Release all acquired
+ * elements before configuring, stopping, reading, or writing.
+ */
+static const NiFpga_Status NiFpga_Status_FifoElementsCurrentlyAcquired = -63083;
+
+/**
+ * A function was called using a misaligned address. The address must be a
+ * multiple of the size of the datatype.
+ */
+static const NiFpga_Status NiFpga_Status_MisalignedAccess = -63084;
+
+/**
+ * The FPGA Read/Write Control Function is accessing a control or indicator
+ * with data that exceeds the maximum size supported on the current target.
+ * Refer to the hardware documentation for the limitations on data types for
+ * this target.
+ */
+static const NiFpga_Status NiFpga_Status_ControlOrIndicatorTooLarge = -63085;
+
+/**
+ * A valid .lvbitx bitfile is required. If you are using a valid .lvbitx
+ * bitfile, the bitfile may not be compatible with the software you are using.
+ * Determine which version of LabVIEW was used to make the bitfile, update your
+ * software to that version or later, and try again.
+ */
+static const NiFpga_Status NiFpga_Status_BitfileReadError = -63101;
+
+/**
+ * The specified signature does not match the signature of the bitfile. If the
+ * bitfile has been recompiled, regenerate the C API and rebuild the
+ * application.
+ */
+static const NiFpga_Status NiFpga_Status_SignatureMismatch = -63106;
+
+/**
+ * The bitfile you are trying to use is incompatible with the version
+ * of NI-RIO installed on the target and/or host. Update the version
+ * of NI-RIO on the target and/or host to the same version (or later)
+ * used to compile the bitfile. Alternatively, recompile the bitfile
+ * with the same version of NI-RIO that is currently installed on the
+ * target and/or host.
+ */
+static const NiFpga_Status NiFpga_Status_IncompatibleBitfile = -63107;
+
+/**
+ * Either the supplied resource name is invalid as a RIO resource name, or the
+ * device was not found. Use MAX to find the proper resource name for the
+ * intended device.
+ */
+static const NiFpga_Status NiFpga_Status_InvalidResourceName = -63192;
+
+/**
+ * The requested feature is not supported.
+ */
+static const NiFpga_Status NiFpga_Status_FeatureNotSupported = -63193;
+
+/**
+ * The NI-RIO software on the target system is not compatible with this
+ * software. Upgrade the NI-RIO software on the target system.
+ */
+static const NiFpga_Status NiFpga_Status_VersionMismatch = -63194;
+
+/**
+ * The session is invalid or has been closed.
+ */
+static const NiFpga_Status NiFpga_Status_InvalidSession = -63195;
+
+/**
+ * The maximum number of open FPGA sessions has been reached. Close some open
+ * sessions.
+ */
+static const NiFpga_Status NiFpga_Status_OutOfHandles = -63198;
diff --git a/hal/src/main/native/sim/Extensions.cpp b/hal/src/main/native/sim/Extensions.cpp
new file mode 100644
index 0000000..951ad10
--- /dev/null
+++ b/hal/src/main/native/sim/Extensions.cpp
@@ -0,0 +1,119 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "hal/Extensions.h"
+
+#include <wpi/Path.h>
+#include <wpi/SmallString.h>
+#include <wpi/StringRef.h>
+#include <wpi/raw_ostream.h>
+
+#include "hal/HAL.h"
+
+#if defined(WIN32) || defined(_WIN32)
+#include <windows.h>
+#else
+#include <dlfcn.h>
+#endif
+
+#if defined(WIN32) || defined(_WIN32)
+#define DELIM ';'
+#define HTYPE HMODULE
+#define DLOPEN(a) LoadLibrary(a)
+#define DLSYM GetProcAddress
+#define DLCLOSE FreeLibrary
+#else
+#define DELIM ':'
+#define HTYPE void*
+#define PREFIX "lib"
+#define DLOPEN(a) dlopen(a, RTLD_LAZY)
+#define DLSYM dlsym
+#define DLCLOSE dlclose
+#endif
+
+namespace hal {
+namespace init {
+void InitializeExtensions() {}
+} // namespace init
+} // namespace hal
+
+static bool& GetShowNotFoundMessage() {
+ static bool showMsg = true;
+ return showMsg;
+}
+
+extern "C" {
+
+int HAL_LoadOneExtension(const char* library) {
+ int rc = 1; // It is expected and reasonable not to find an extra simulation
+ wpi::outs() << "HAL Extensions: Attempting to load: "
+ << wpi::sys::path::stem(library) << "\n";
+ wpi::outs().flush();
+ HTYPE handle = DLOPEN(library);
+#if !defined(WIN32) && !defined(_WIN32)
+ if (!handle) {
+ wpi::SmallString<128> libraryName("lib");
+ libraryName += library;
+#if defined(__APPLE__)
+ libraryName += ".dylib";
+#else
+ libraryName += ".so";
+#endif
+ wpi::outs() << "HAL Extensions: Trying modified name: "
+ << wpi::sys::path::stem(libraryName);
+ wpi::outs().flush();
+ handle = DLOPEN(libraryName.c_str());
+ }
+#endif
+ if (!handle) {
+ wpi::outs() << "HAL Extensions: Failed to load library\n";
+ wpi::outs().flush();
+ return rc;
+ }
+
+ auto init = reinterpret_cast<halsim_extension_init_func_t*>(
+ DLSYM(handle, "HALSIM_InitExtension"));
+
+ if (init) rc = (*init)();
+
+ if (rc != 0) {
+ wpi::outs() << "HAL Extensions: Failed to load extension\n";
+ wpi::outs().flush();
+ DLCLOSE(handle);
+ } else {
+ wpi::outs() << "HAL Extensions: Successfully loaded extension\n";
+ wpi::outs().flush();
+ }
+ return rc;
+}
+
+int HAL_LoadExtensions(void) {
+ int rc = 1;
+ wpi::SmallVector<wpi::StringRef, 2> libraries;
+ const char* e = std::getenv("HALSIM_EXTENSIONS");
+ if (!e) {
+ if (GetShowNotFoundMessage()) {
+ wpi::outs() << "HAL Extensions: No extensions found\n";
+ wpi::outs().flush();
+ }
+ return rc;
+ }
+ wpi::StringRef env{e};
+ env.split(libraries, DELIM, -1, false);
+ for (auto& libref : libraries) {
+ wpi::SmallString<128> library(libref);
+ rc = HAL_LoadOneExtension(library.c_str());
+ if (rc < 0) break;
+ }
+ return rc;
+}
+
+void HAL_SetShowExtensionsNotFoundMessages(HAL_Bool showMessage) {
+ GetShowNotFoundMessage() = showMessage;
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/sim/HAL.cpp b/hal/src/main/native/sim/HAL.cpp
new file mode 100644
index 0000000..0dda617
--- /dev/null
+++ b/hal/src/main/native/sim/HAL.cpp
@@ -0,0 +1,291 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 "hal/HAL.h"
+
+#include <wpi/mutex.h>
+#include <wpi/raw_ostream.h>
+
+#include "ErrorsInternal.h"
+#include "HALInitializer.h"
+#include "MockHooksInternal.h"
+#include "hal/DriverStation.h"
+#include "hal/Errors.h"
+#include "hal/Extensions.h"
+#include "hal/handles/HandlesInternal.h"
+#include "mockdata/RoboRioDataInternal.h"
+
+using namespace hal;
+
+namespace hal {
+namespace init {
+void InitializeHAL() {
+ InitializeAccelerometerData();
+ InitializeAddressableLEDData();
+ InitializeAnalogGyroData();
+ InitializeAnalogInData();
+ InitializeAnalogOutData();
+ InitializeAnalogTriggerData();
+ InitializeCanData();
+ InitializeCANAPI();
+ InitializeDigitalPWMData();
+ InitializeDutyCycleData();
+ InitializeDIOData();
+ InitializeDriverStationData();
+ InitializeEncoderData();
+ InitializeI2CData();
+ InitializePCMData();
+ InitializePDPData();
+ InitializePWMData();
+ InitializeRelayData();
+ InitializeRoboRioData();
+ InitializeSimDeviceData();
+ InitializeSPIAccelerometerData();
+ InitializeSPIData();
+ InitializeAccelerometer();
+ InitializeAddressableLED();
+ InitializeAnalogAccumulator();
+ InitializeAnalogGyro();
+ InitializeAnalogInput();
+ InitializeAnalogInternal();
+ InitializeAnalogOutput();
+ InitializeCAN();
+ InitializeCompressor();
+ InitializeConstants();
+ InitializeCounter();
+ InitializeDigitalInternal();
+ InitializeDIO();
+ InitializeDutyCycle();
+ InitializeDriverStation();
+ InitializeEncoder();
+ InitializeExtensions();
+ InitializeI2C();
+ InitializeInterrupts();
+ InitializeMain();
+ InitializeMockHooks();
+ InitializeNotifier();
+ InitializePDP();
+ InitializePorts();
+ InitializePower();
+ InitializePWM();
+ InitializeRelay();
+ InitializeSerialPort();
+ InitializeSimDevice();
+ InitializeSolenoid();
+ InitializeSPI();
+ InitializeThreads();
+}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+
+HAL_PortHandle HAL_GetPort(int32_t channel) {
+ // Dont allow a number that wouldn't fit in a uint8_t
+ if (channel < 0 || channel >= 255) return HAL_kInvalidHandle;
+ return createPortHandle(channel, 1);
+}
+
+HAL_PortHandle HAL_GetPortWithModule(int32_t module, int32_t channel) {
+ // Dont allow a number that wouldn't fit in a uint8_t
+ if (channel < 0 || channel >= 255) return HAL_kInvalidHandle;
+ if (module < 0 || module >= 255) return HAL_kInvalidHandle;
+ return createPortHandle(channel, module);
+}
+
+const char* HAL_GetErrorMessage(int32_t code) {
+ switch (code) {
+ case 0:
+ return "";
+ case CTR_RxTimeout:
+ return CTR_RxTimeout_MESSAGE;
+ case CTR_TxTimeout:
+ return CTR_TxTimeout_MESSAGE;
+ case CTR_InvalidParamValue:
+ return CTR_InvalidParamValue_MESSAGE;
+ case CTR_UnexpectedArbId:
+ return CTR_UnexpectedArbId_MESSAGE;
+ case CTR_TxFailed:
+ return CTR_TxFailed_MESSAGE;
+ case CTR_SigNotUpdated:
+ return CTR_SigNotUpdated_MESSAGE;
+ case NiFpga_Status_FifoTimeout:
+ return NiFpga_Status_FifoTimeout_MESSAGE;
+ case NiFpga_Status_TransferAborted:
+ return NiFpga_Status_TransferAborted_MESSAGE;
+ case NiFpga_Status_MemoryFull:
+ return NiFpga_Status_MemoryFull_MESSAGE;
+ case NiFpga_Status_SoftwareFault:
+ return NiFpga_Status_SoftwareFault_MESSAGE;
+ case NiFpga_Status_InvalidParameter:
+ return NiFpga_Status_InvalidParameter_MESSAGE;
+ case NiFpga_Status_ResourceNotFound:
+ return NiFpga_Status_ResourceNotFound_MESSAGE;
+ case NiFpga_Status_ResourceNotInitialized:
+ return NiFpga_Status_ResourceNotInitialized_MESSAGE;
+ case NiFpga_Status_HardwareFault:
+ return NiFpga_Status_HardwareFault_MESSAGE;
+ case NiFpga_Status_IrqTimeout:
+ return NiFpga_Status_IrqTimeout_MESSAGE;
+ case SAMPLE_RATE_TOO_HIGH:
+ return SAMPLE_RATE_TOO_HIGH_MESSAGE;
+ case VOLTAGE_OUT_OF_RANGE:
+ return VOLTAGE_OUT_OF_RANGE_MESSAGE;
+ case LOOP_TIMING_ERROR:
+ return LOOP_TIMING_ERROR_MESSAGE;
+ case SPI_WRITE_NO_MOSI:
+ return SPI_WRITE_NO_MOSI_MESSAGE;
+ case SPI_READ_NO_MISO:
+ return SPI_READ_NO_MISO_MESSAGE;
+ case SPI_READ_NO_DATA:
+ return SPI_READ_NO_DATA_MESSAGE;
+ case INCOMPATIBLE_STATE:
+ return INCOMPATIBLE_STATE_MESSAGE;
+ case NO_AVAILABLE_RESOURCES:
+ return NO_AVAILABLE_RESOURCES_MESSAGE;
+ case RESOURCE_IS_ALLOCATED:
+ return RESOURCE_IS_ALLOCATED_MESSAGE;
+ case RESOURCE_OUT_OF_RANGE:
+ return RESOURCE_OUT_OF_RANGE_MESSAGE;
+ case HAL_INVALID_ACCUMULATOR_CHANNEL:
+ return HAL_INVALID_ACCUMULATOR_CHANNEL_MESSAGE;
+ case HAL_HANDLE_ERROR:
+ return HAL_HANDLE_ERROR_MESSAGE;
+ case NULL_PARAMETER:
+ return NULL_PARAMETER_MESSAGE;
+ case ANALOG_TRIGGER_LIMIT_ORDER_ERROR:
+ return ANALOG_TRIGGER_LIMIT_ORDER_ERROR_MESSAGE;
+ case ANALOG_TRIGGER_PULSE_OUTPUT_ERROR:
+ return ANALOG_TRIGGER_PULSE_OUTPUT_ERROR_MESSAGE;
+ case PARAMETER_OUT_OF_RANGE:
+ return PARAMETER_OUT_OF_RANGE_MESSAGE;
+ case HAL_COUNTER_NOT_SUPPORTED:
+ return HAL_COUNTER_NOT_SUPPORTED_MESSAGE;
+ case HAL_ERR_CANSessionMux_InvalidBuffer:
+ return ERR_CANSessionMux_InvalidBuffer_MESSAGE;
+ case HAL_ERR_CANSessionMux_MessageNotFound:
+ return ERR_CANSessionMux_MessageNotFound_MESSAGE;
+ case HAL_WARN_CANSessionMux_NoToken:
+ return WARN_CANSessionMux_NoToken_MESSAGE;
+ case HAL_ERR_CANSessionMux_NotAllowed:
+ return ERR_CANSessionMux_NotAllowed_MESSAGE;
+ case HAL_ERR_CANSessionMux_NotInitialized:
+ return ERR_CANSessionMux_NotInitialized_MESSAGE;
+ case VI_ERROR_SYSTEM_ERROR:
+ return VI_ERROR_SYSTEM_ERROR_MESSAGE;
+ case VI_ERROR_INV_OBJECT:
+ return VI_ERROR_INV_OBJECT_MESSAGE;
+ case VI_ERROR_RSRC_LOCKED:
+ return VI_ERROR_RSRC_LOCKED_MESSAGE;
+ case VI_ERROR_RSRC_NFOUND:
+ return VI_ERROR_RSRC_NFOUND_MESSAGE;
+ case VI_ERROR_INV_RSRC_NAME:
+ return VI_ERROR_INV_RSRC_NAME_MESSAGE;
+ case VI_ERROR_QUEUE_OVERFLOW:
+ return VI_ERROR_QUEUE_OVERFLOW_MESSAGE;
+ case VI_ERROR_IO:
+ return VI_ERROR_IO_MESSAGE;
+ case VI_ERROR_ASRL_PARITY:
+ return VI_ERROR_ASRL_PARITY_MESSAGE;
+ case VI_ERROR_ASRL_FRAMING:
+ return VI_ERROR_ASRL_FRAMING_MESSAGE;
+ case VI_ERROR_ASRL_OVERRUN:
+ return VI_ERROR_ASRL_OVERRUN_MESSAGE;
+ case VI_ERROR_RSRC_BUSY:
+ return VI_ERROR_RSRC_BUSY_MESSAGE;
+ case VI_ERROR_INV_PARAMETER:
+ return VI_ERROR_INV_PARAMETER_MESSAGE;
+ case HAL_PWM_SCALE_ERROR:
+ return HAL_PWM_SCALE_ERROR_MESSAGE;
+ case HAL_CAN_TIMEOUT:
+ return HAL_CAN_TIMEOUT_MESSAGE;
+ case HAL_SIM_NOT_SUPPORTED:
+ return HAL_SIM_NOT_SUPPORTED_MESSAGE;
+ case HAL_CAN_BUFFER_OVERRUN:
+ return HAL_CAN_BUFFER_OVERRUN_MESSAGE;
+ case HAL_LED_CHANNEL_ERROR:
+ return HAL_LED_CHANNEL_ERROR_MESSAGE;
+ default:
+ return "Unknown error status";
+ }
+}
+
+HAL_RuntimeType HAL_GetRuntimeType(void) { return HAL_Mock; }
+
+int32_t HAL_GetFPGAVersion(int32_t* status) {
+ return 2018; // Automatically script this at some point
+}
+
+int64_t HAL_GetFPGARevision(int32_t* status) {
+ return 0; // TODO: Find a better number to return;
+}
+
+uint64_t HAL_GetFPGATime(int32_t* status) { return hal::GetFPGATime(); }
+
+uint64_t HAL_ExpandFPGATime(uint32_t unexpanded_lower, int32_t* status) {
+ // Capture the current FPGA time. This will give us the upper half of the
+ // clock.
+ uint64_t fpga_time = HAL_GetFPGATime(status);
+ if (*status != 0) return 0;
+
+ // Now, we need to detect the case where the lower bits rolled over after we
+ // sampled. In that case, the upper bits will be 1 bigger than they should
+ // be.
+
+ // Break it into lower and upper portions.
+ uint32_t lower = fpga_time & 0xffffffffull;
+ uint64_t upper = (fpga_time >> 32) & 0xffffffff;
+
+ // The time was sampled *before* the current time, so roll it back.
+ if (lower < unexpanded_lower) {
+ --upper;
+ }
+
+ return (upper << 32) + static_cast<uint64_t>(unexpanded_lower);
+}
+
+HAL_Bool HAL_GetFPGAButton(int32_t* status) {
+ return SimRoboRioData[0].fpgaButton;
+}
+
+HAL_Bool HAL_GetSystemActive(int32_t* status) {
+ return true; // Figure out if we need to handle this
+}
+
+HAL_Bool HAL_GetBrownedOut(int32_t* status) {
+ return false; // Figure out if we need to detect a brownout condition
+}
+
+HAL_Bool HAL_Initialize(int32_t timeout, int32_t mode) {
+ static std::atomic_bool initialized{false};
+ static wpi::mutex initializeMutex;
+ // Initial check, as if it's true initialization has finished
+ if (initialized) return true;
+
+ std::scoped_lock lock(initializeMutex);
+ // Second check in case another thread was waiting
+ if (initialized) return true;
+
+ hal::init::InitializeHAL();
+
+ hal::init::HAL_IsInitialized.store(true);
+
+ wpi::outs().SetUnbuffered();
+ if (HAL_LoadExtensions() < 0) return false;
+ hal::RestartTiming();
+ HAL_InitializeDriverStation();
+
+ initialized = true;
+ return true; // Add initialization if we need to at a later point
+}
+
+int64_t HAL_Report(int32_t resource, int32_t instanceNumber, int32_t context,
+ const char* feature) {
+ return 0; // Do nothing for now
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/sim/HALInitializer.cpp b/hal/src/main/native/sim/HALInitializer.cpp
new file mode 100644
index 0000000..a0456d4
--- /dev/null
+++ b/hal/src/main/native/sim/HALInitializer.cpp
@@ -0,0 +1,17 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 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 "HALInitializer.h"
+
+#include "hal/HAL.h"
+
+namespace hal {
+namespace init {
+std::atomic_bool HAL_IsInitialized{false};
+void RunInitialize() { HAL_Initialize(500, 0); }
+} // namespace init
+} // namespace hal
diff --git a/hal/src/main/native/sim/HALInitializer.h b/hal/src/main/native/sim/HALInitializer.h
new file mode 100644
index 0000000..c08df73
--- /dev/null
+++ b/hal/src/main/native/sim/HALInitializer.h
@@ -0,0 +1,78 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 <atomic>
+
+namespace hal {
+namespace init {
+extern std::atomic_bool HAL_IsInitialized;
+extern void RunInitialize();
+static inline void CheckInit() {
+ if (HAL_IsInitialized.load(std::memory_order_relaxed)) return;
+ RunInitialize();
+}
+
+extern void InitializeAccelerometerData();
+extern void InitializeAddressableLEDData();
+extern void InitializeAnalogGyroData();
+extern void InitializeAnalogInData();
+extern void InitializeAnalogOutData();
+extern void InitializeAnalogTriggerData();
+extern void InitializeCanData();
+extern void InitializeCANAPI();
+extern void InitializeDigitalPWMData();
+extern void InitializeDutyCycleData();
+extern void InitializeDIOData();
+extern void InitializeDutyCycle();
+extern void InitializeDriverStationData();
+extern void InitializeEncoderData();
+extern void InitializeI2CData();
+extern void InitializePCMData();
+extern void InitializePDPData();
+extern void InitializePWMData();
+extern void InitializeRelayData();
+extern void InitializeRoboRioData();
+extern void InitializeSimDeviceData();
+extern void InitializeSPIAccelerometerData();
+extern void InitializeSPIData();
+extern void InitializeAccelerometer();
+extern void InitializeAddressableLED();
+extern void InitializeAnalogAccumulator();
+extern void InitializeAnalogGyro();
+extern void InitializeAnalogInput();
+extern void InitializeAnalogInternal();
+extern void InitializeAnalogOutput();
+extern void InitializeCAN();
+extern void InitializeCompressor();
+extern void InitializeConstants();
+extern void InitializeCounter();
+extern void InitializeDigitalInternal();
+extern void InitializeDIO();
+extern void InitializeDriverStation();
+extern void InitializeEncoder();
+extern void InitializeExtensions();
+extern void InitializeHAL();
+extern void InitializeI2C();
+extern void InitializeInterrupts();
+extern void InitializeMain();
+extern void InitializeMockHooks();
+extern void InitializeNotifier();
+extern void InitializePDP();
+extern void InitializePorts();
+extern void InitializePower();
+extern void InitializePWM();
+extern void InitializeRelay();
+extern void InitializeSerialPort();
+extern void InitializeSimDevice();
+extern void InitializeSolenoid();
+extern void InitializeSPI();
+extern void InitializeThreads();
+
+} // namespace init
+} // namespace hal
diff --git a/hal/src/main/native/sim/I2C.cpp b/hal/src/main/native/sim/I2C.cpp
new file mode 100644
index 0000000..fe4952f
--- /dev/null
+++ b/hal/src/main/native/sim/I2C.cpp
@@ -0,0 +1,44 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 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 "hal/I2C.h"
+
+#include "HALInitializer.h"
+#include "mockdata/I2CDataInternal.h"
+
+using namespace hal;
+
+namespace hal {
+namespace init {
+void InitializeI2C() {}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+void HAL_InitializeI2C(HAL_I2CPort port, int32_t* status) {
+ hal::init::CheckInit();
+ SimI2CData[port].initialized = true;
+}
+int32_t HAL_TransactionI2C(HAL_I2CPort port, int32_t deviceAddress,
+ const uint8_t* dataToSend, int32_t sendSize,
+ uint8_t* dataReceived, int32_t receiveSize) {
+ SimI2CData[port].Write(deviceAddress, dataToSend, sendSize);
+ SimI2CData[port].Read(deviceAddress, dataReceived, receiveSize);
+ return 0;
+}
+int32_t HAL_WriteI2C(HAL_I2CPort port, int32_t deviceAddress,
+ const uint8_t* dataToSend, int32_t sendSize) {
+ SimI2CData[port].Write(deviceAddress, dataToSend, sendSize);
+ return 0;
+}
+int32_t HAL_ReadI2C(HAL_I2CPort port, int32_t deviceAddress, uint8_t* buffer,
+ int32_t count) {
+ SimI2CData[port].Read(deviceAddress, buffer, count);
+ return 0;
+}
+void HAL_CloseI2C(HAL_I2CPort port) { SimI2CData[port].initialized = false; }
+} // extern "C"
diff --git a/hal/src/main/native/sim/Interrupts.cpp b/hal/src/main/native/sim/Interrupts.cpp
new file mode 100644
index 0000000..a7dd285
--- /dev/null
+++ b/hal/src/main/native/sim/Interrupts.cpp
@@ -0,0 +1,569 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "hal/Interrupts.h"
+
+#include <memory>
+
+#include <wpi/condition_variable.h>
+
+#include "AnalogInternal.h"
+#include "DigitalInternal.h"
+#include "ErrorsInternal.h"
+#include "HALInitializer.h"
+#include "MockHooksInternal.h"
+#include "PortsInternal.h"
+#include "hal/AnalogTrigger.h"
+#include "hal/Errors.h"
+#include "hal/Value.h"
+#include "hal/handles/HandlesInternal.h"
+#include "hal/handles/LimitedHandleResource.h"
+#include "hal/handles/UnlimitedHandleResource.h"
+#include "mockdata/AnalogInDataInternal.h"
+#include "mockdata/DIODataInternal.h"
+
+#ifdef _WIN32
+#pragma warning(disable : 4996 4018 6297 26451 4334)
+#endif
+
+using namespace hal;
+
+enum WaitResult {
+ Timeout = 0x0,
+ RisingEdge = 0x1,
+ FallingEdge = 0x100,
+ Both = 0x101,
+};
+
+namespace {
+struct Interrupt {
+ bool isAnalog;
+ HAL_Handle portHandle;
+ uint8_t index;
+ HAL_AnalogTriggerType trigType;
+ bool watcher;
+ int64_t risingTimestamp;
+ int64_t fallingTimestamp;
+ bool previousState;
+ bool fireOnUp;
+ bool fireOnDown;
+ int32_t callbackId;
+
+ void* callbackParam;
+ HAL_InterruptHandlerFunction callbackFunction;
+};
+
+struct SynchronousWaitData {
+ HAL_InterruptHandle interruptHandle{HAL_kInvalidHandle};
+ wpi::condition_variable waitCond;
+ HAL_Bool waitPredicate{false};
+};
+} // namespace
+
+static LimitedHandleResource<HAL_InterruptHandle, Interrupt, kNumInterrupts,
+ HAL_HandleEnum::Interrupt>* interruptHandles;
+
+typedef HAL_Handle SynchronousWaitDataHandle;
+static UnlimitedHandleResource<SynchronousWaitDataHandle, SynchronousWaitData,
+ HAL_HandleEnum::Vendor>*
+ synchronousInterruptHandles;
+
+namespace hal {
+namespace init {
+void InitializeInterrupts() {
+ static LimitedHandleResource<HAL_InterruptHandle, Interrupt, kNumInterrupts,
+ HAL_HandleEnum::Interrupt>
+ iH;
+ interruptHandles = &iH;
+ static UnlimitedHandleResource<SynchronousWaitDataHandle, SynchronousWaitData,
+ HAL_HandleEnum::Vendor>
+ siH;
+ synchronousInterruptHandles = &siH;
+}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+HAL_InterruptHandle HAL_InitializeInterrupts(HAL_Bool watcher,
+ int32_t* status) {
+ hal::init::CheckInit();
+ HAL_InterruptHandle handle = interruptHandles->Allocate();
+ if (handle == HAL_kInvalidHandle) {
+ *status = NO_AVAILABLE_RESOURCES;
+ return HAL_kInvalidHandle;
+ }
+ auto anInterrupt = interruptHandles->Get(handle);
+ if (anInterrupt == nullptr) { // would only occur on thread issue.
+ *status = HAL_HANDLE_ERROR;
+ return HAL_kInvalidHandle;
+ }
+
+ anInterrupt->index = getHandleIndex(handle);
+ anInterrupt->callbackId = -1;
+
+ anInterrupt->watcher = watcher;
+
+ return handle;
+}
+void* HAL_CleanInterrupts(HAL_InterruptHandle interruptHandle,
+ int32_t* status) {
+ HAL_DisableInterrupts(interruptHandle, status);
+ auto anInterrupt = interruptHandles->Get(interruptHandle);
+ interruptHandles->Free(interruptHandle);
+ if (anInterrupt == nullptr) {
+ return nullptr;
+ }
+ return anInterrupt->callbackParam;
+}
+
+static void ProcessInterruptDigitalSynchronous(const char* name, void* param,
+ const struct HAL_Value* value) {
+ // void* is a SynchronousWaitDataHandle.
+ // convert to uintptr_t first, then to handle
+ uintptr_t handleTmp = reinterpret_cast<uintptr_t>(param);
+ SynchronousWaitDataHandle handle =
+ static_cast<SynchronousWaitDataHandle>(handleTmp);
+ auto interruptData = synchronousInterruptHandles->Get(handle);
+ if (interruptData == nullptr) return;
+ auto interrupt = interruptHandles->Get(interruptData->interruptHandle);
+ if (interrupt == nullptr) return;
+ // Have a valid interrupt
+ if (value->type != HAL_Type::HAL_BOOLEAN) return;
+ bool retVal = value->data.v_boolean;
+ // If no change in interrupt, return;
+ if (retVal == interrupt->previousState) return;
+ // If its a falling change, and we dont fire on falling return
+ if (interrupt->previousState && !interrupt->fireOnDown) return;
+ // If its a rising change, and we dont fire on rising return.
+ if (!interrupt->previousState && !interrupt->fireOnUp) return;
+
+ interruptData->waitPredicate = true;
+
+ // Pulse interrupt
+ interruptData->waitCond.notify_all();
+}
+
+static double GetAnalogTriggerValue(HAL_Handle triggerHandle,
+ HAL_AnalogTriggerType type,
+ int32_t* status) {
+ return HAL_GetAnalogTriggerOutput(triggerHandle, type, status);
+}
+
+static void ProcessInterruptAnalogSynchronous(const char* name, void* param,
+ const struct HAL_Value* value) {
+ // void* is a SynchronousWaitDataHandle.
+ // convert to uintptr_t first, then to handle
+ uintptr_t handleTmp = reinterpret_cast<uintptr_t>(param);
+ SynchronousWaitDataHandle handle =
+ static_cast<SynchronousWaitDataHandle>(handleTmp);
+ auto interruptData = synchronousInterruptHandles->Get(handle);
+ if (interruptData == nullptr) return;
+ auto interrupt = interruptHandles->Get(interruptData->interruptHandle);
+ if (interrupt == nullptr) return;
+ // Have a valid interrupt
+ if (value->type != HAL_Type::HAL_DOUBLE) return;
+ int32_t status = 0;
+ bool retVal = GetAnalogTriggerValue(interrupt->portHandle,
+ interrupt->trigType, &status);
+ if (status != 0) {
+ // Interrupt and Cancel
+ interruptData->waitPredicate = true;
+ // Pulse interrupt
+ interruptData->waitCond.notify_all();
+ }
+ // If no change in interrupt, return;
+ if (retVal == interrupt->previousState) return;
+ // If its a falling change, and we dont fire on falling return
+ if (interrupt->previousState && !interrupt->fireOnDown) return;
+ // If its a rising change, and we dont fire on rising return.
+ if (!interrupt->previousState && !interrupt->fireOnUp) return;
+
+ interruptData->waitPredicate = true;
+
+ // Pulse interrupt
+ interruptData->waitCond.notify_all();
+}
+
+static int64_t WaitForInterruptDigital(HAL_InterruptHandle handle,
+ Interrupt* interrupt, double timeout,
+ bool ignorePrevious) {
+ auto data = std::make_shared<SynchronousWaitData>();
+
+ auto dataHandle = synchronousInterruptHandles->Allocate(data);
+ if (dataHandle == HAL_kInvalidHandle) {
+ // Error allocating data
+ return WaitResult::Timeout;
+ }
+
+ // auto data = synchronousInterruptHandles->Get(dataHandle);
+ data->waitPredicate = false;
+ data->interruptHandle = handle;
+
+ int32_t status = 0;
+
+ int32_t digitalIndex = GetDigitalInputChannel(interrupt->portHandle, &status);
+
+ if (status != 0) return WaitResult::Timeout;
+
+ interrupt->previousState = SimDIOData[digitalIndex].value;
+
+ int32_t uid = SimDIOData[digitalIndex].value.RegisterCallback(
+ &ProcessInterruptDigitalSynchronous,
+ reinterpret_cast<void*>(static_cast<uintptr_t>(dataHandle)), false);
+
+ bool timedOut = false;
+
+ wpi::mutex waitMutex;
+
+ auto timeoutTime =
+ std::chrono::steady_clock::now() + std::chrono::duration<double>(timeout);
+
+ {
+ std::unique_lock lock(waitMutex);
+ while (!data->waitPredicate) {
+ if (data->waitCond.wait_until(lock, timeoutTime) ==
+ std::cv_status::timeout) {
+ timedOut = true;
+ break;
+ }
+ }
+ }
+
+ // Cancel our callback
+ SimDIOData[digitalIndex].value.CancelCallback(uid);
+ (void)synchronousInterruptHandles->Free(dataHandle);
+
+ // Check for what to return
+ if (timedOut) return WaitResult::Timeout;
+ // True => false, Falling
+ if (interrupt->previousState) {
+ // Set our return value and our timestamps
+ interrupt->fallingTimestamp = hal::GetFPGATime();
+ return 1 << (8 + interrupt->index);
+ } else {
+ interrupt->risingTimestamp = hal::GetFPGATime();
+ return 1 << (interrupt->index);
+ }
+}
+
+static int64_t WaitForInterruptAnalog(HAL_InterruptHandle handle,
+ Interrupt* interrupt, double timeout,
+ bool ignorePrevious) {
+ auto data = std::make_shared<SynchronousWaitData>();
+
+ auto dataHandle = synchronousInterruptHandles->Allocate(data);
+ if (dataHandle == HAL_kInvalidHandle) {
+ // Error allocating data
+ return WaitResult::Timeout;
+ }
+
+ data->waitPredicate = false;
+ data->interruptHandle = handle;
+
+ int32_t status = 0;
+ interrupt->previousState = GetAnalogTriggerValue(
+ interrupt->portHandle, interrupt->trigType, &status);
+
+ if (status != 0) return WaitResult::Timeout;
+
+ int32_t analogIndex =
+ GetAnalogTriggerInputIndex(interrupt->portHandle, &status);
+
+ if (status != 0) return WaitResult::Timeout;
+
+ int32_t uid = SimAnalogInData[analogIndex].voltage.RegisterCallback(
+ &ProcessInterruptAnalogSynchronous,
+ reinterpret_cast<void*>(static_cast<uintptr_t>(dataHandle)), false);
+
+ bool timedOut = false;
+
+ wpi::mutex waitMutex;
+
+ auto timeoutTime =
+ std::chrono::steady_clock::now() + std::chrono::duration<double>(timeout);
+
+ {
+ std::unique_lock lock(waitMutex);
+ while (!data->waitPredicate) {
+ if (data->waitCond.wait_until(lock, timeoutTime) ==
+ std::cv_status::timeout) {
+ timedOut = true;
+ break;
+ }
+ }
+ }
+
+ // Cancel our callback
+ SimAnalogInData[analogIndex].voltage.CancelCallback(uid);
+ (void)synchronousInterruptHandles->Free(dataHandle);
+
+ // Check for what to return
+ if (timedOut) return WaitResult::Timeout;
+ // True => false, Falling
+ if (interrupt->previousState) {
+ // Set our return value and our timestamps
+ interrupt->fallingTimestamp = hal::GetFPGATime();
+ return 1 << (8 + interrupt->index);
+ } else {
+ interrupt->risingTimestamp = hal::GetFPGATime();
+ return 1 << (interrupt->index);
+ }
+}
+
+int64_t HAL_WaitForInterrupt(HAL_InterruptHandle interruptHandle,
+ double timeout, HAL_Bool ignorePrevious,
+ int32_t* status) {
+ auto interrupt = interruptHandles->Get(interruptHandle);
+ if (interrupt == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return WaitResult::Timeout;
+ }
+
+ // Check to make sure we are actually an interrupt in synchronous mode
+ if (!interrupt->watcher) {
+ *status = NiFpga_Status_InvalidParameter;
+ return WaitResult::Timeout;
+ }
+
+ if (interrupt->isAnalog) {
+ return WaitForInterruptAnalog(interruptHandle, interrupt.get(), timeout,
+ ignorePrevious);
+ } else {
+ return WaitForInterruptDigital(interruptHandle, interrupt.get(), timeout,
+ ignorePrevious);
+ }
+}
+
+static void ProcessInterruptDigitalAsynchronous(const char* name, void* param,
+ const struct HAL_Value* value) {
+ // void* is a HAL handle
+ // convert to uintptr_t first, then to handle
+ uintptr_t handleTmp = reinterpret_cast<uintptr_t>(param);
+ HAL_InterruptHandle handle = static_cast<HAL_InterruptHandle>(handleTmp);
+ auto interrupt = interruptHandles->Get(handle);
+ if (interrupt == nullptr) return;
+ // Have a valid interrupt
+ if (value->type != HAL_Type::HAL_BOOLEAN) return;
+ bool retVal = value->data.v_boolean;
+ // If no change in interrupt, return;
+ if (retVal == interrupt->previousState) return;
+ int32_t mask = 0;
+ if (interrupt->previousState) {
+ interrupt->previousState = retVal;
+ interrupt->fallingTimestamp = hal::GetFPGATime();
+ mask = 1 << (8 + interrupt->index);
+ if (!interrupt->fireOnDown) return;
+ } else {
+ interrupt->previousState = retVal;
+ interrupt->risingTimestamp = hal::GetFPGATime();
+ mask = 1 << (interrupt->index);
+ if (!interrupt->fireOnUp) return;
+ }
+
+ // run callback
+ auto callback = interrupt->callbackFunction;
+ if (callback == nullptr) return;
+ callback(mask, interrupt->callbackParam);
+}
+
+static void ProcessInterruptAnalogAsynchronous(const char* name, void* param,
+ const struct HAL_Value* value) {
+ // void* is a HAL handle
+ // convert to intptr_t first, then to handle
+ uintptr_t handleTmp = reinterpret_cast<uintptr_t>(param);
+ HAL_InterruptHandle handle = static_cast<HAL_InterruptHandle>(handleTmp);
+ auto interrupt = interruptHandles->Get(handle);
+ if (interrupt == nullptr) return;
+ // Have a valid interrupt
+ if (value->type != HAL_Type::HAL_DOUBLE) return;
+ int32_t status = 0;
+ bool retVal = GetAnalogTriggerValue(interrupt->portHandle,
+ interrupt->trigType, &status);
+ if (status != 0) return;
+ // If no change in interrupt, return;
+ if (retVal == interrupt->previousState) return;
+ int mask = 0;
+ if (interrupt->previousState) {
+ interrupt->previousState = retVal;
+ interrupt->fallingTimestamp = hal::GetFPGATime();
+ if (!interrupt->fireOnDown) return;
+ mask = 1 << (8 + interrupt->index);
+ } else {
+ interrupt->previousState = retVal;
+ interrupt->risingTimestamp = hal::GetFPGATime();
+ if (!interrupt->fireOnUp) return;
+ mask = 1 << (interrupt->index);
+ }
+
+ // run callback
+ auto callback = interrupt->callbackFunction;
+ if (callback == nullptr) return;
+ callback(mask, interrupt->callbackParam);
+}
+
+static void EnableInterruptsDigital(HAL_InterruptHandle handle,
+ Interrupt* interrupt) {
+ int32_t status = 0;
+ int32_t digitalIndex = GetDigitalInputChannel(interrupt->portHandle, &status);
+ if (status != 0) return;
+
+ interrupt->previousState = SimDIOData[digitalIndex].value;
+
+ int32_t uid = SimDIOData[digitalIndex].value.RegisterCallback(
+ &ProcessInterruptDigitalAsynchronous,
+ reinterpret_cast<void*>(static_cast<uintptr_t>(handle)), false);
+ interrupt->callbackId = uid;
+}
+
+static void EnableInterruptsAnalog(HAL_InterruptHandle handle,
+ Interrupt* interrupt) {
+ int32_t status = 0;
+ int32_t analogIndex =
+ GetAnalogTriggerInputIndex(interrupt->portHandle, &status);
+ if (status != 0) return;
+
+ status = 0;
+ interrupt->previousState = GetAnalogTriggerValue(
+ interrupt->portHandle, interrupt->trigType, &status);
+ if (status != 0) return;
+
+ int32_t uid = SimAnalogInData[analogIndex].voltage.RegisterCallback(
+ &ProcessInterruptAnalogAsynchronous,
+ reinterpret_cast<void*>(static_cast<uintptr_t>(handle)), false);
+ interrupt->callbackId = uid;
+}
+
+void HAL_EnableInterrupts(HAL_InterruptHandle interruptHandle,
+ int32_t* status) {
+ auto interrupt = interruptHandles->Get(interruptHandle);
+ if (interrupt == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ // If we have not had a callback set, error out
+ if (interrupt->callbackFunction == nullptr) {
+ *status = INCOMPATIBLE_STATE;
+ return;
+ }
+
+ // EnableInterrupts has already been called
+ if (interrupt->callbackId >= 0) {
+ // We can double enable safely.
+ return;
+ }
+
+ if (interrupt->isAnalog) {
+ EnableInterruptsAnalog(interruptHandle, interrupt.get());
+ } else {
+ EnableInterruptsDigital(interruptHandle, interrupt.get());
+ }
+}
+void HAL_DisableInterrupts(HAL_InterruptHandle interruptHandle,
+ int32_t* status) {
+ auto interrupt = interruptHandles->Get(interruptHandle);
+ if (interrupt == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ // No need to disable if we are already disabled
+ if (interrupt->callbackId < 0) return;
+
+ if (interrupt->isAnalog) {
+ // Do analog
+ int32_t status = 0;
+ int32_t analogIndex =
+ GetAnalogTriggerInputIndex(interrupt->portHandle, &status);
+ if (status != 0) return;
+ SimAnalogInData[analogIndex].voltage.CancelCallback(interrupt->callbackId);
+ } else {
+ int32_t status = 0;
+ int32_t digitalIndex =
+ GetDigitalInputChannel(interrupt->portHandle, &status);
+ if (status != 0) return;
+ SimDIOData[digitalIndex].value.CancelCallback(interrupt->callbackId);
+ }
+ interrupt->callbackId = -1;
+}
+int64_t HAL_ReadInterruptRisingTimestamp(HAL_InterruptHandle interruptHandle,
+ int32_t* status) {
+ auto interrupt = interruptHandles->Get(interruptHandle);
+ if (interrupt == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+
+ return interrupt->risingTimestamp;
+}
+int64_t HAL_ReadInterruptFallingTimestamp(HAL_InterruptHandle interruptHandle,
+ int32_t* status) {
+ auto interrupt = interruptHandles->Get(interruptHandle);
+ if (interrupt == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+
+ return interrupt->fallingTimestamp;
+}
+void HAL_RequestInterrupts(HAL_InterruptHandle interruptHandle,
+ HAL_Handle digitalSourceHandle,
+ HAL_AnalogTriggerType analogTriggerType,
+ int32_t* status) {
+ auto interrupt = interruptHandles->Get(interruptHandle);
+ if (interrupt == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ bool routingAnalogTrigger = false;
+ uint8_t routingChannel = 0;
+ uint8_t routingModule = 0;
+ bool success =
+ remapDigitalSource(digitalSourceHandle, analogTriggerType, routingChannel,
+ routingModule, routingAnalogTrigger);
+ if (!success) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ interrupt->isAnalog = routingAnalogTrigger;
+ interrupt->trigType = analogTriggerType;
+ interrupt->portHandle = digitalSourceHandle;
+}
+void HAL_AttachInterruptHandler(HAL_InterruptHandle interruptHandle,
+ HAL_InterruptHandlerFunction handler,
+ void* param, int32_t* status) {
+ auto interrupt = interruptHandles->Get(interruptHandle);
+ if (interrupt == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ interrupt->callbackFunction = handler;
+ interrupt->callbackParam = param;
+}
+
+void HAL_AttachInterruptHandlerThreaded(HAL_InterruptHandle interruptHandle,
+ HAL_InterruptHandlerFunction handler,
+ void* param, int32_t* status) {
+ HAL_AttachInterruptHandler(interruptHandle, handler, param, status);
+}
+
+void HAL_SetInterruptUpSourceEdge(HAL_InterruptHandle interruptHandle,
+ HAL_Bool risingEdge, HAL_Bool fallingEdge,
+ int32_t* status) {
+ auto interrupt = interruptHandles->Get(interruptHandle);
+ if (interrupt == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ interrupt->fireOnDown = fallingEdge;
+ interrupt->fireOnUp = risingEdge;
+}
+} // extern "C"
diff --git a/hal/src/main/native/sim/MockHooks.cpp b/hal/src/main/native/sim/MockHooks.cpp
new file mode 100644
index 0000000..59086ae
--- /dev/null
+++ b/hal/src/main/native/sim/MockHooks.cpp
@@ -0,0 +1,98 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 <atomic>
+#include <chrono>
+#include <cstdio>
+#include <thread>
+
+#include <wpi/timestamp.h>
+
+#include "MockHooksInternal.h"
+#include "NotifierInternal.h"
+
+static std::atomic<bool> programStarted{false};
+
+static std::atomic<uint64_t> programStartTime{0};
+static std::atomic<uint64_t> programPauseTime{0};
+
+namespace hal {
+namespace init {
+void InitializeMockHooks() {}
+} // namespace init
+} // namespace hal
+
+namespace hal {
+void RestartTiming() {
+ programStartTime = wpi::Now();
+ if (programPauseTime != 0) programPauseTime = programStartTime.load();
+}
+
+void PauseTiming() {
+ if (programPauseTime == 0) programPauseTime = wpi::Now();
+}
+
+void ResumeTiming() {
+ if (programPauseTime != 0) {
+ programStartTime += wpi::Now() - programPauseTime;
+ programPauseTime = 0;
+ }
+}
+
+bool IsTimingPaused() { return programPauseTime != 0; }
+
+void StepTiming(uint64_t delta) {
+ if (programPauseTime != 0) programPauseTime += delta;
+}
+
+int64_t GetFPGATime() {
+ uint64_t curTime = programPauseTime;
+ if (curTime == 0) curTime = wpi::Now();
+ return curTime - programStartTime;
+}
+
+double GetFPGATimestamp() { return GetFPGATime() * 1.0e-6; }
+
+void SetProgramStarted() { programStarted = true; }
+bool GetProgramStarted() { return programStarted; }
+} // namespace hal
+
+using namespace hal;
+
+extern "C" {
+void HALSIM_WaitForProgramStart(void) {
+ int count = 0;
+ while (!programStarted) {
+ count++;
+ std::printf("Waiting for program start signal: %d\n", count);
+ std::this_thread::sleep_for(std::chrono::milliseconds(500));
+ }
+}
+
+void HALSIM_SetProgramStarted(void) { SetProgramStarted(); }
+
+HAL_Bool HALSIM_GetProgramStarted(void) { return GetProgramStarted(); }
+
+void HALSIM_RestartTiming(void) { RestartTiming(); }
+
+void HALSIM_PauseTiming(void) {
+ PauseTiming();
+ PauseNotifiers();
+}
+
+void HALSIM_ResumeTiming(void) {
+ ResumeTiming();
+ ResumeNotifiers();
+}
+
+HAL_Bool HALSIM_IsTimingPaused(void) { return IsTimingPaused(); }
+
+void HALSIM_StepTiming(uint64_t delta) {
+ StepTiming(delta);
+ WakeupNotifiers();
+}
+} // extern "C"
diff --git a/hal/src/main/native/sim/MockHooksInternal.h b/hal/src/main/native/sim/MockHooksInternal.h
new file mode 100644
index 0000000..a69e9bf
--- /dev/null
+++ b/hal/src/main/native/sim/MockHooksInternal.h
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 <stdint.h>
+
+#include "mockdata/MockHooks.h"
+
+namespace hal {
+void RestartTiming();
+
+void PauseTiming();
+
+void ResumeTiming();
+
+bool IsTimingPaused();
+
+void StepTiming(uint64_t delta);
+
+int64_t GetFPGATime();
+
+double GetFPGATimestamp();
+
+void SetProgramStarted();
+} // namespace hal
diff --git a/hal/src/main/native/sim/Notifier.cpp b/hal/src/main/native/sim/Notifier.cpp
new file mode 100644
index 0000000..211f7b2
--- /dev/null
+++ b/hal/src/main/native/sim/Notifier.cpp
@@ -0,0 +1,223 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 "hal/Notifier.h"
+
+#include <atomic>
+#include <chrono>
+#include <cstdio>
+#include <cstring>
+#include <string>
+
+#include <wpi/condition_variable.h>
+#include <wpi/mutex.h>
+#include <wpi/timestamp.h>
+
+#include "HALInitializer.h"
+#include "NotifierInternal.h"
+#include "hal/HAL.h"
+#include "hal/cpp/fpga_clock.h"
+#include "hal/handles/UnlimitedHandleResource.h"
+#include "mockdata/NotifierData.h"
+
+namespace {
+struct Notifier {
+ std::string name;
+ uint64_t waitTime;
+ bool active = true;
+ bool running = false;
+ wpi::mutex mutex;
+ wpi::condition_variable cond;
+};
+} // namespace
+
+using namespace hal;
+
+class NotifierHandleContainer
+ : public UnlimitedHandleResource<HAL_NotifierHandle, Notifier,
+ HAL_HandleEnum::Notifier> {
+ public:
+ ~NotifierHandleContainer() {
+ ForEach([](HAL_NotifierHandle handle, Notifier* notifier) {
+ {
+ std::scoped_lock lock(notifier->mutex);
+ notifier->active = false;
+ notifier->running = false;
+ }
+ notifier->cond.notify_all(); // wake up any waiting threads
+ });
+ }
+};
+
+static NotifierHandleContainer* notifierHandles;
+static std::atomic<bool> notifiersPaused{false};
+
+namespace hal {
+namespace init {
+void InitializeNotifier() {
+ static NotifierHandleContainer nH;
+ notifierHandles = &nH;
+}
+} // namespace init
+
+void PauseNotifiers() { notifiersPaused = true; }
+
+void ResumeNotifiers() {
+ notifiersPaused = false;
+ WakeupNotifiers();
+}
+
+void WakeupNotifiers() {
+ notifierHandles->ForEach([](HAL_NotifierHandle handle, Notifier* notifier) {
+ notifier->cond.notify_all();
+ });
+}
+} // namespace hal
+
+extern "C" {
+
+HAL_NotifierHandle HAL_InitializeNotifier(int32_t* status) {
+ hal::init::CheckInit();
+ std::shared_ptr<Notifier> notifier = std::make_shared<Notifier>();
+ HAL_NotifierHandle handle = notifierHandles->Allocate(notifier);
+ if (handle == HAL_kInvalidHandle) {
+ *status = HAL_HANDLE_ERROR;
+ return HAL_kInvalidHandle;
+ }
+ return handle;
+}
+
+void HAL_SetNotifierName(HAL_NotifierHandle notifierHandle, const char* name,
+ int32_t* status) {
+ auto notifier = notifierHandles->Get(notifierHandle);
+ if (!notifier) return;
+ std::scoped_lock lock(notifier->mutex);
+ notifier->name = name;
+}
+
+void HAL_StopNotifier(HAL_NotifierHandle notifierHandle, int32_t* status) {
+ auto notifier = notifierHandles->Get(notifierHandle);
+ if (!notifier) return;
+
+ {
+ std::scoped_lock lock(notifier->mutex);
+ notifier->active = false;
+ notifier->running = false;
+ }
+ notifier->cond.notify_all();
+}
+
+void HAL_CleanNotifier(HAL_NotifierHandle notifierHandle, int32_t* status) {
+ auto notifier = notifierHandles->Free(notifierHandle);
+ if (!notifier) return;
+
+ // Just in case HAL_StopNotifier() wasn't called...
+ {
+ std::scoped_lock lock(notifier->mutex);
+ notifier->active = false;
+ notifier->running = false;
+ }
+ notifier->cond.notify_all();
+}
+
+void HAL_UpdateNotifierAlarm(HAL_NotifierHandle notifierHandle,
+ uint64_t triggerTime, int32_t* status) {
+ auto notifier = notifierHandles->Get(notifierHandle);
+ if (!notifier) return;
+
+ {
+ std::scoped_lock lock(notifier->mutex);
+ notifier->waitTime = triggerTime;
+ notifier->running = true;
+ }
+
+ // We wake up any waiters to change how long they're sleeping for
+ notifier->cond.notify_all();
+}
+
+void HAL_CancelNotifierAlarm(HAL_NotifierHandle notifierHandle,
+ int32_t* status) {
+ auto notifier = notifierHandles->Get(notifierHandle);
+ if (!notifier) return;
+
+ {
+ std::scoped_lock lock(notifier->mutex);
+ notifier->running = false;
+ }
+}
+
+uint64_t HAL_WaitForNotifierAlarm(HAL_NotifierHandle notifierHandle,
+ int32_t* status) {
+ auto notifier = notifierHandles->Get(notifierHandle);
+ if (!notifier) return 0;
+
+ std::unique_lock lock(notifier->mutex);
+ while (notifier->active) {
+ double waitTime;
+ if (!notifier->running || notifiersPaused) {
+ waitTime = (HAL_GetFPGATime(status) * 1e-6) + 1000.0;
+ // If not running, wait 1000 seconds
+ } else {
+ waitTime = notifier->waitTime * 1e-6;
+ }
+
+ auto timeoutTime =
+ hal::fpga_clock::epoch() + std::chrono::duration<double>(waitTime);
+ notifier->cond.wait_until(lock, timeoutTime);
+ if (!notifier->running) continue;
+ if (!notifier->active) break;
+ uint64_t curTime = HAL_GetFPGATime(status);
+ if (curTime < notifier->waitTime) continue;
+ notifier->running = false;
+ return curTime;
+ }
+ return 0;
+}
+
+uint64_t HALSIM_GetNextNotifierTimeout(void) {
+ uint64_t timeout = UINT64_MAX;
+ notifierHandles->ForEach([&](HAL_NotifierHandle, Notifier* notifier) {
+ std::scoped_lock lock(notifier->mutex);
+ if (notifier->active && notifier->running && timeout > notifier->waitTime)
+ timeout = notifier->waitTime;
+ });
+ return timeout;
+}
+
+int32_t HALSIM_GetNumNotifiers(void) {
+ int32_t count = 0;
+ notifierHandles->ForEach([&](HAL_NotifierHandle, Notifier* notifier) {
+ std::scoped_lock lock(notifier->mutex);
+ if (notifier->active) ++count;
+ });
+ return count;
+}
+
+int32_t HALSIM_GetNotifierInfo(struct HALSIM_NotifierInfo* arr, int32_t size) {
+ int32_t num = 0;
+ notifierHandles->ForEach([&](HAL_NotifierHandle handle, Notifier* notifier) {
+ std::scoped_lock lock(notifier->mutex);
+ if (!notifier->active) return;
+ if (num < size) {
+ arr[num].handle = handle;
+ if (notifier->name.empty()) {
+ std::snprintf(arr[num].name, sizeof(arr[num].name), "Notifier%d",
+ static_cast<int>(getHandleIndex(handle)));
+ } else {
+ std::strncpy(arr[num].name, notifier->name.c_str(),
+ sizeof(arr[num].name));
+ arr[num].name[sizeof(arr[num].name) - 1] = '\0';
+ }
+ arr[num].timeout = notifier->waitTime;
+ arr[num].running = notifier->running;
+ }
+ ++num;
+ });
+ return num;
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/sim/NotifierInternal.h b/hal/src/main/native/sim/NotifierInternal.h
new file mode 100644
index 0000000..84232d2
--- /dev/null
+++ b/hal/src/main/native/sim/NotifierInternal.h
@@ -0,0 +1,14 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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
+
+namespace hal {
+void PauseNotifiers();
+void ResumeNotifiers();
+void WakeupNotifiers();
+} // namespace hal
diff --git a/hal/src/main/native/sim/PDP.cpp b/hal/src/main/native/sim/PDP.cpp
new file mode 100644
index 0000000..dbe09d4
--- /dev/null
+++ b/hal/src/main/native/sim/PDP.cpp
@@ -0,0 +1,104 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "hal/PDP.h"
+
+#include "CANAPIInternal.h"
+#include "HALInitializer.h"
+#include "PortsInternal.h"
+#include "hal/CANAPI.h"
+#include "hal/handles/IndexedHandleResource.h"
+#include "mockdata/PDPDataInternal.h"
+
+using namespace hal;
+
+static constexpr HAL_CANManufacturer manufacturer =
+ HAL_CANManufacturer::HAL_CAN_Man_kCTRE;
+
+static constexpr HAL_CANDeviceType deviceType =
+ HAL_CANDeviceType::HAL_CAN_Dev_kPowerDistribution;
+
+namespace hal {
+namespace init {
+void InitializePDP() {}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+HAL_PDPHandle HAL_InitializePDP(int32_t module, int32_t* status) {
+ if (!HAL_CheckPDPModule(module)) {
+ *status = PARAMETER_OUT_OF_RANGE;
+ return HAL_kInvalidHandle;
+ }
+ hal::init::CheckInit();
+ SimPDPData[module].initialized = true;
+ auto handle = HAL_InitializeCAN(manufacturer, module, deviceType, status);
+
+ if (*status != 0) {
+ HAL_CleanCAN(handle);
+ return HAL_kInvalidHandle;
+ }
+
+ return handle;
+}
+
+HAL_Bool HAL_CheckPDPModule(int32_t module) {
+ return module < kNumPDPModules && module >= 0;
+}
+
+HAL_Bool HAL_CheckPDPChannel(int32_t channel) {
+ return channel < kNumPDPChannels && channel >= 0;
+}
+
+void HAL_CleanPDP(HAL_PDPHandle handle) { HAL_CleanCAN(handle); }
+
+double HAL_GetPDPTemperature(HAL_PDPHandle handle, int32_t* status) {
+ auto module = hal::can::GetCANModuleFromHandle(handle, status);
+ if (*status != 0) {
+ return 0.0;
+ }
+ return SimPDPData[module].temperature;
+}
+double HAL_GetPDPVoltage(HAL_PDPHandle handle, int32_t* status) {
+ auto module = hal::can::GetCANModuleFromHandle(handle, status);
+ if (*status != 0) {
+ return 0.0;
+ }
+ return SimPDPData[module].voltage;
+}
+double HAL_GetPDPChannelCurrent(HAL_PDPHandle handle, int32_t channel,
+ int32_t* status) {
+ auto module = hal::can::GetCANModuleFromHandle(handle, status);
+ if (*status != 0) {
+ return 0.0;
+ }
+ return SimPDPData[module].current[channel];
+}
+void HAL_GetPDPAllChannelCurrents(HAL_PDPHandle handle, double* currents,
+ int32_t* status) {
+ auto module = hal::can::GetCANModuleFromHandle(handle, status);
+ if (*status != 0) {
+ return;
+ }
+
+ auto& data = SimPDPData[module];
+ for (int i = 0; i < kNumPDPChannels; i++) {
+ currents[i] = data.current[i];
+ }
+}
+double HAL_GetPDPTotalCurrent(HAL_PDPHandle handle, int32_t* status) {
+ return 0.0;
+}
+double HAL_GetPDPTotalPower(HAL_PDPHandle handle, int32_t* status) {
+ return 0.0;
+}
+double HAL_GetPDPTotalEnergy(HAL_PDPHandle handle, int32_t* status) {
+ return 0.0;
+}
+void HAL_ResetPDPTotalEnergy(HAL_PDPHandle handle, int32_t* status) {}
+void HAL_ClearPDPStickyFaults(HAL_PDPHandle handle, int32_t* status) {}
+} // extern "C"
diff --git a/hal/src/main/native/sim/PWM.cpp b/hal/src/main/native/sim/PWM.cpp
new file mode 100644
index 0000000..228b540
--- /dev/null
+++ b/hal/src/main/native/sim/PWM.cpp
@@ -0,0 +1,302 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 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 "hal/PWM.h"
+
+#include "ConstantsInternal.h"
+#include "DigitalInternal.h"
+#include "HALInitializer.h"
+#include "PortsInternal.h"
+#include "hal/handles/HandlesInternal.h"
+#include "mockdata/PWMDataInternal.h"
+
+using namespace hal;
+
+namespace hal {
+namespace init {
+void InitializePWM() {}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+
+HAL_DigitalHandle HAL_InitializePWMPort(HAL_PortHandle portHandle,
+ int32_t* status) {
+ hal::init::CheckInit();
+ if (*status != 0) return HAL_kInvalidHandle;
+
+ int16_t channel = getPortHandleChannel(portHandle);
+ if (channel == InvalidHandleIndex) {
+ *status = PARAMETER_OUT_OF_RANGE;
+ return HAL_kInvalidHandle;
+ }
+
+ uint8_t origChannel = static_cast<uint8_t>(channel);
+
+ if (origChannel < kNumPWMHeaders) {
+ channel += kNumDigitalChannels; // remap Headers to end of allocations
+ } else {
+ channel = remapMXPPWMChannel(channel) + 10; // remap MXP to proper channel
+ }
+
+ auto handle =
+ digitalChannelHandles->Allocate(channel, HAL_HandleEnum::PWM, status);
+
+ if (*status != 0)
+ return HAL_kInvalidHandle; // failed to allocate. Pass error back.
+
+ auto port = digitalChannelHandles->Get(handle, HAL_HandleEnum::PWM);
+ if (port == nullptr) { // would only occur on thread issue.
+ *status = HAL_HANDLE_ERROR;
+ return HAL_kInvalidHandle;
+ }
+
+ port->channel = origChannel;
+
+ SimPWMData[origChannel].initialized = true;
+
+ // Defaults to allow an always valid config.
+ HAL_SetPWMConfig(handle, 2.0, 1.501, 1.5, 1.499, 1.0, status);
+
+ return handle;
+}
+void HAL_FreePWMPort(HAL_DigitalHandle pwmPortHandle, int32_t* status) {
+ auto port = digitalChannelHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ SimPWMData[port->channel].initialized = false;
+
+ digitalChannelHandles->Free(pwmPortHandle, HAL_HandleEnum::PWM);
+}
+
+HAL_Bool HAL_CheckPWMChannel(int32_t channel) {
+ return channel < kNumPWMChannels && channel >= 0;
+}
+
+void HAL_SetPWMConfig(HAL_DigitalHandle pwmPortHandle, double max,
+ double deadbandMax, double center, double deadbandMin,
+ double min, int32_t* status) {
+ auto port = digitalChannelHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ // calculate the loop time in milliseconds
+ double loopTime =
+ HAL_GetPWMLoopTiming(status) / (kSystemClockTicksPerMicrosecond * 1e3);
+ if (*status != 0) return;
+
+ int32_t maxPwm = static_cast<int32_t>((max - kDefaultPwmCenter) / loopTime +
+ kDefaultPwmStepsDown - 1);
+ int32_t deadbandMaxPwm = static_cast<int32_t>(
+ (deadbandMax - kDefaultPwmCenter) / loopTime + kDefaultPwmStepsDown - 1);
+ int32_t centerPwm = static_cast<int32_t>(
+ (center - kDefaultPwmCenter) / loopTime + kDefaultPwmStepsDown - 1);
+ int32_t deadbandMinPwm = static_cast<int32_t>(
+ (deadbandMin - kDefaultPwmCenter) / loopTime + kDefaultPwmStepsDown - 1);
+ int32_t minPwm = static_cast<int32_t>((min - kDefaultPwmCenter) / loopTime +
+ kDefaultPwmStepsDown - 1);
+
+ port->maxPwm = maxPwm;
+ port->deadbandMaxPwm = deadbandMaxPwm;
+ port->deadbandMinPwm = deadbandMinPwm;
+ port->centerPwm = centerPwm;
+ port->minPwm = minPwm;
+ port->configSet = true;
+}
+
+void HAL_SetPWMConfigRaw(HAL_DigitalHandle pwmPortHandle, int32_t maxPwm,
+ int32_t deadbandMaxPwm, int32_t centerPwm,
+ int32_t deadbandMinPwm, int32_t minPwm,
+ int32_t* status) {
+ auto port = digitalChannelHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ port->maxPwm = maxPwm;
+ port->deadbandMaxPwm = deadbandMaxPwm;
+ port->deadbandMinPwm = deadbandMinPwm;
+ port->centerPwm = centerPwm;
+ port->minPwm = minPwm;
+}
+
+void HAL_GetPWMConfigRaw(HAL_DigitalHandle pwmPortHandle, int32_t* maxPwm,
+ int32_t* deadbandMaxPwm, int32_t* centerPwm,
+ int32_t* deadbandMinPwm, int32_t* minPwm,
+ int32_t* status) {
+ auto port = digitalChannelHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ *maxPwm = port->maxPwm;
+ *deadbandMaxPwm = port->deadbandMaxPwm;
+ *deadbandMinPwm = port->deadbandMinPwm;
+ *centerPwm = port->centerPwm;
+ *minPwm = port->minPwm;
+}
+
+void HAL_SetPWMEliminateDeadband(HAL_DigitalHandle pwmPortHandle,
+ HAL_Bool eliminateDeadband, int32_t* status) {
+ auto port = digitalChannelHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ port->eliminateDeadband = eliminateDeadband;
+}
+
+HAL_Bool HAL_GetPWMEliminateDeadband(HAL_DigitalHandle pwmPortHandle,
+ int32_t* status) {
+ auto port = digitalChannelHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return false;
+ }
+ return port->eliminateDeadband;
+}
+
+void HAL_SetPWMRaw(HAL_DigitalHandle pwmPortHandle, int32_t value,
+ int32_t* status) {
+ auto port = digitalChannelHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ SimPWMData[port->channel].rawValue = value;
+}
+
+void HAL_SetPWMSpeed(HAL_DigitalHandle pwmPortHandle, double speed,
+ int32_t* status) {
+ auto port = digitalChannelHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ if (!port->configSet) {
+ *status = INCOMPATIBLE_STATE;
+ return;
+ }
+
+ if (speed < -1.0) {
+ speed = -1.0;
+ } else if (speed > 1.0) {
+ speed = 1.0;
+ }
+
+ SimPWMData[port->channel].speed = speed;
+}
+
+void HAL_SetPWMPosition(HAL_DigitalHandle pwmPortHandle, double pos,
+ int32_t* status) {
+ auto port = digitalChannelHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ if (!port->configSet) {
+ *status = INCOMPATIBLE_STATE;
+ return;
+ }
+
+ if (pos < 0.0) {
+ pos = 0.0;
+ } else if (pos > 1.0) {
+ pos = 1.0;
+ }
+
+ SimPWMData[port->channel].position = pos;
+}
+
+void HAL_SetPWMDisabled(HAL_DigitalHandle pwmPortHandle, int32_t* status) {
+ auto port = digitalChannelHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ SimPWMData[port->channel].rawValue = 0;
+ SimPWMData[port->channel].position = 0;
+ SimPWMData[port->channel].speed = 0;
+}
+
+int32_t HAL_GetPWMRaw(HAL_DigitalHandle pwmPortHandle, int32_t* status) {
+ auto port = digitalChannelHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+
+ return SimPWMData[port->channel].rawValue;
+}
+
+double HAL_GetPWMSpeed(HAL_DigitalHandle pwmPortHandle, int32_t* status) {
+ auto port = digitalChannelHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+ if (!port->configSet) {
+ *status = INCOMPATIBLE_STATE;
+ return 0;
+ }
+
+ double speed = SimPWMData[port->channel].speed;
+ if (speed > 1) speed = 1;
+ if (speed < -1) speed = -1;
+ return speed;
+}
+
+double HAL_GetPWMPosition(HAL_DigitalHandle pwmPortHandle, int32_t* status) {
+ auto port = digitalChannelHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+ if (!port->configSet) {
+ *status = INCOMPATIBLE_STATE;
+ return 0;
+ }
+
+ double position = SimPWMData[port->channel].position;
+ if (position > 1) position = 1;
+ if (position < 0) position = 0;
+ return position;
+}
+
+void HAL_LatchPWMZero(HAL_DigitalHandle pwmPortHandle, int32_t* status) {
+ auto port = digitalChannelHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ SimPWMData[port->channel].zeroLatch = true;
+ SimPWMData[port->channel].zeroLatch = false;
+}
+
+void HAL_SetPWMPeriodScale(HAL_DigitalHandle pwmPortHandle, int32_t squelchMask,
+ int32_t* status) {
+ auto port = digitalChannelHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ SimPWMData[port->channel].periodScale = squelchMask;
+}
+
+int32_t HAL_GetPWMLoopTiming(int32_t* status) { return kExpectedLoopTiming; }
+
+uint64_t HAL_GetPWMCycleStartTime(int32_t* status) { return 0; }
+} // extern "C"
diff --git a/hal/src/main/native/sim/Ports.cpp b/hal/src/main/native/sim/Ports.cpp
new file mode 100644
index 0000000..2f670b3
--- /dev/null
+++ b/hal/src/main/native/sim/Ports.cpp
@@ -0,0 +1,41 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 "hal/Ports.h"
+
+#include "PortsInternal.h"
+
+using namespace hal;
+
+namespace hal {
+namespace init {
+void InitializePorts() {}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+int32_t HAL_GetNumAccumulators(void) { return kNumAccumulators; }
+int32_t HAL_GetNumAnalogTriggers(void) { return kNumAnalogTriggers; }
+int32_t HAL_GetNumAnalogInputs(void) { return kNumAnalogInputs; }
+int32_t HAL_GetNumAnalogOutputs(void) { return kNumAnalogOutputs; }
+int32_t HAL_GetNumCounters(void) { return kNumCounters; }
+int32_t HAL_GetNumDigitalHeaders(void) { return kNumDigitalHeaders; }
+int32_t HAL_GetNumPWMHeaders(void) { return kNumPWMHeaders; }
+int32_t HAL_GetNumDigitalChannels(void) { return kNumDigitalChannels; }
+int32_t HAL_GetNumPWMChannels(void) { return kNumPWMChannels; }
+int32_t HAL_GetNumDigitalPWMOutputs(void) { return kNumDigitalPWMOutputs; }
+int32_t HAL_GetNumEncoders(void) { return kNumEncoders; }
+int32_t HAL_GetNumInterrupts(void) { return kNumInterrupts; }
+int32_t HAL_GetNumRelayChannels(void) { return kNumRelayChannels; }
+int32_t HAL_GetNumRelayHeaders(void) { return kNumRelayHeaders; }
+int32_t HAL_GetNumPCMModules(void) { return kNumPCMModules; }
+int32_t HAL_GetNumSolenoidChannels(void) { return kNumSolenoidChannels; }
+int32_t HAL_GetNumPDPModules(void) { return kNumPDPModules; }
+int32_t HAL_GetNumPDPChannels(void) { return kNumPDPChannels; }
+int32_t HAL_GetNumDutyCycles(void) { return kNumDutyCycles; }
+int32_t HAL_GetNumAddressableLEDs(void) { return kNumAddressableLEDs; }
+} // extern "C"
diff --git a/hal/src/main/native/sim/PortsInternal.h b/hal/src/main/native/sim/PortsInternal.h
new file mode 100644
index 0000000..cfbf1e7
--- /dev/null
+++ b/hal/src/main/native/sim/PortsInternal.h
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 <stdint.h>
+
+namespace hal {
+constexpr int32_t kNumAccumulators = 2;
+constexpr int32_t kNumAnalogTriggers = 8;
+constexpr int32_t kNumAnalogInputs = 8;
+constexpr int32_t kNumAnalogOutputs = 2;
+constexpr int32_t kNumCounters = 8;
+constexpr int32_t kNumDigitalHeaders = 10;
+constexpr int32_t kNumPWMHeaders = 10;
+constexpr int32_t kNumDigitalChannels = 26;
+constexpr int32_t kNumPWMChannels = 20;
+constexpr int32_t kNumDigitalPWMOutputs = 6;
+constexpr int32_t kNumEncoders = 8;
+constexpr int32_t kNumInterrupts = 8;
+constexpr int32_t kNumRelayChannels = 8;
+constexpr int32_t kNumRelayHeaders = kNumRelayChannels / 2;
+constexpr int32_t kNumPCMModules = 63;
+constexpr int32_t kNumSolenoidChannels = 8;
+constexpr int32_t kNumPDPModules = 63;
+constexpr int32_t kNumPDPChannels = 16;
+constexpr int32_t kNumDutyCycles = 8;
+constexpr int32_t kNumAddressableLEDs = 1;
+} // namespace hal
diff --git a/hal/src/main/native/sim/Power.cpp b/hal/src/main/native/sim/Power.cpp
new file mode 100644
index 0000000..95bb3dd
--- /dev/null
+++ b/hal/src/main/native/sim/Power.cpp
@@ -0,0 +1,64 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 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 "hal/Power.h"
+
+#include "mockdata/RoboRioDataInternal.h"
+
+using namespace hal;
+
+namespace hal {
+namespace init {
+void InitializePower() {}
+} // namespace init
+} // namespace hal
+
+// TODO: Fix the naming in here
+extern "C" {
+double HAL_GetVinVoltage(int32_t* status) {
+ return SimRoboRioData[0].vInVoltage;
+}
+double HAL_GetVinCurrent(int32_t* status) {
+ return SimRoboRioData[0].vInCurrent;
+}
+double HAL_GetUserVoltage6V(int32_t* status) {
+ return SimRoboRioData[0].userVoltage6V;
+}
+double HAL_GetUserCurrent6V(int32_t* status) {
+ return SimRoboRioData[0].userCurrent6V;
+}
+HAL_Bool HAL_GetUserActive6V(int32_t* status) {
+ return SimRoboRioData[0].userActive6V;
+}
+int32_t HAL_GetUserCurrentFaults6V(int32_t* status) {
+ return SimRoboRioData[0].userFaults6V;
+}
+double HAL_GetUserVoltage5V(int32_t* status) {
+ return SimRoboRioData[0].userVoltage5V;
+}
+double HAL_GetUserCurrent5V(int32_t* status) {
+ return SimRoboRioData[0].userCurrent5V;
+}
+HAL_Bool HAL_GetUserActive5V(int32_t* status) {
+ return SimRoboRioData[0].userActive5V;
+}
+int32_t HAL_GetUserCurrentFaults5V(int32_t* status) {
+ return SimRoboRioData[0].userFaults5V;
+}
+double HAL_GetUserVoltage3V3(int32_t* status) {
+ return SimRoboRioData[0].userVoltage3V3;
+}
+double HAL_GetUserCurrent3V3(int32_t* status) {
+ return SimRoboRioData[0].userCurrent3V3;
+}
+HAL_Bool HAL_GetUserActive3V3(int32_t* status) {
+ return SimRoboRioData[0].userActive3V3;
+}
+int32_t HAL_GetUserCurrentFaults3V3(int32_t* status) {
+ return SimRoboRioData[0].userFaults3V3;
+}
+} // extern "C"
diff --git a/hal/src/main/native/sim/Relay.cpp b/hal/src/main/native/sim/Relay.cpp
new file mode 100644
index 0000000..bd1c8a8
--- /dev/null
+++ b/hal/src/main/native/sim/Relay.cpp
@@ -0,0 +1,121 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 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 "hal/Relay.h"
+
+#include "HALInitializer.h"
+#include "PortsInternal.h"
+#include "hal/handles/IndexedHandleResource.h"
+#include "mockdata/RelayDataInternal.h"
+
+using namespace hal;
+
+namespace {
+struct Relay {
+ uint8_t channel;
+ bool fwd;
+};
+} // namespace
+
+static IndexedHandleResource<HAL_RelayHandle, Relay, kNumRelayChannels,
+ HAL_HandleEnum::Relay>* relayHandles;
+
+namespace hal {
+namespace init {
+void InitializeRelay() {
+ static IndexedHandleResource<HAL_RelayHandle, Relay, kNumRelayChannels,
+ HAL_HandleEnum::Relay>
+ rH;
+ relayHandles = &rH;
+}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+HAL_RelayHandle HAL_InitializeRelayPort(HAL_PortHandle portHandle, HAL_Bool fwd,
+ int32_t* status) {
+ hal::init::CheckInit();
+ if (*status != 0) return HAL_kInvalidHandle;
+
+ int16_t channel = getPortHandleChannel(portHandle);
+ if (channel == InvalidHandleIndex) {
+ *status = PARAMETER_OUT_OF_RANGE;
+ return HAL_kInvalidHandle;
+ }
+
+ if (!fwd) channel += kNumRelayHeaders; // add 4 to reverse channels
+
+ auto handle = relayHandles->Allocate(channel, status);
+
+ if (*status != 0)
+ return HAL_kInvalidHandle; // failed to allocate. Pass error back.
+
+ auto port = relayHandles->Get(handle);
+ if (port == nullptr) { // would only occur on thread issue.
+ *status = HAL_HANDLE_ERROR;
+ return HAL_kInvalidHandle;
+ }
+
+ if (!fwd) {
+ // Subtract number of headers to put channel in range
+ channel -= kNumRelayHeaders;
+
+ port->fwd = false; // set to reverse
+
+ SimRelayData[channel].initializedReverse = true;
+ } else {
+ port->fwd = true; // set to forward
+ SimRelayData[channel].initializedForward = true;
+ }
+
+ port->channel = static_cast<uint8_t>(channel);
+
+ return handle;
+}
+
+void HAL_FreeRelayPort(HAL_RelayHandle relayPortHandle) {
+ auto port = relayHandles->Get(relayPortHandle);
+ relayHandles->Free(relayPortHandle);
+ if (port == nullptr) return;
+ if (port->fwd)
+ SimRelayData[port->channel].initializedForward = false;
+ else
+ SimRelayData[port->channel].initializedReverse = false;
+}
+
+HAL_Bool HAL_CheckRelayChannel(int32_t channel) {
+ // roboRIO only has 4 headers, and the FPGA has
+ // seperate functions for forward and reverse,
+ // instead of seperate channel IDs
+ return channel < kNumRelayHeaders && channel >= 0;
+}
+
+void HAL_SetRelay(HAL_RelayHandle relayPortHandle, HAL_Bool on,
+ int32_t* status) {
+ auto port = relayHandles->Get(relayPortHandle);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ if (port->fwd)
+ SimRelayData[port->channel].forward = on;
+ else
+ SimRelayData[port->channel].reverse = on;
+}
+
+HAL_Bool HAL_GetRelay(HAL_RelayHandle relayPortHandle, int32_t* status) {
+ auto port = relayHandles->Get(relayPortHandle);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return false;
+ }
+ if (port->fwd)
+ return SimRelayData[port->channel].forward;
+ else
+ return SimRelayData[port->channel].reverse;
+}
+} // extern "C"
diff --git a/hal/src/main/native/sim/SPI.cpp b/hal/src/main/native/sim/SPI.cpp
new file mode 100644
index 0000000..1c90a98
--- /dev/null
+++ b/hal/src/main/native/sim/SPI.cpp
@@ -0,0 +1,73 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "hal/SPI.h"
+
+#include "HALInitializer.h"
+#include "mockdata/SPIDataInternal.h"
+
+using namespace hal;
+
+namespace hal {
+namespace init {
+void InitializeSPI() {}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+
+void HAL_InitializeSPI(HAL_SPIPort port, int32_t* status) {
+ hal::init::CheckInit();
+ SimSPIData[port].initialized = true;
+}
+int32_t HAL_TransactionSPI(HAL_SPIPort port, const uint8_t* dataToSend,
+ uint8_t* dataReceived, int32_t size) {
+ return SimSPIData[port].Transaction(dataToSend, dataReceived, size);
+}
+int32_t HAL_WriteSPI(HAL_SPIPort port, const uint8_t* dataToSend,
+ int32_t sendSize) {
+ return SimSPIData[port].Write(dataToSend, sendSize);
+}
+int32_t HAL_ReadSPI(HAL_SPIPort port, uint8_t* buffer, int32_t count) {
+ return SimSPIData[port].Read(buffer, count);
+}
+void HAL_CloseSPI(HAL_SPIPort port) { SimSPIData[port].initialized = false; }
+void HAL_SetSPISpeed(HAL_SPIPort port, int32_t speed) {}
+void HAL_SetSPIOpts(HAL_SPIPort port, HAL_Bool msbFirst,
+ HAL_Bool sampleOnTrailing, HAL_Bool clkIdleHigh) {}
+void HAL_SetSPIChipSelectActiveHigh(HAL_SPIPort port, int32_t* status) {}
+void HAL_SetSPIChipSelectActiveLow(HAL_SPIPort port, int32_t* status) {}
+int32_t HAL_GetSPIHandle(HAL_SPIPort port) { return 0; }
+void HAL_SetSPIHandle(HAL_SPIPort port, int32_t handle) {}
+
+void HAL_InitSPIAuto(HAL_SPIPort port, int32_t bufferSize, int32_t* status) {}
+void HAL_FreeSPIAuto(HAL_SPIPort port, int32_t* status) {}
+void HAL_StartSPIAutoRate(HAL_SPIPort port, double period, int32_t* status) {}
+void HAL_StartSPIAutoTrigger(HAL_SPIPort port, HAL_Handle digitalSourceHandle,
+ HAL_AnalogTriggerType analogTriggerType,
+ HAL_Bool triggerRising, HAL_Bool triggerFalling,
+ int32_t* status) {}
+void HAL_StopSPIAuto(HAL_SPIPort port, int32_t* status) {}
+void HAL_SetSPIAutoTransmitData(HAL_SPIPort port, const uint8_t* dataToSend,
+ int32_t dataSize, int32_t zeroSize,
+ int32_t* status) {}
+void HAL_ForceSPIAutoRead(HAL_SPIPort port, int32_t* status) {}
+int32_t HAL_ReadSPIAutoReceivedData(HAL_SPIPort port, uint32_t* buffer,
+ int32_t numToRead, double timeout,
+ int32_t* status) {
+ return SimSPIData[port].ReadAutoReceivedData(buffer, numToRead, timeout,
+ status);
+}
+int32_t HAL_GetSPIAutoDroppedCount(HAL_SPIPort port, int32_t* status) {
+ return 0;
+}
+
+void HAL_ConfigureSPIAutoStall(HAL_SPIPort port, int32_t csToSclkTicks,
+ int32_t stallTicks, int32_t pow2BytesPerRead,
+ int32_t* status) {}
+
+} // extern "C"
diff --git a/hal/src/main/native/sim/SerialPort.cpp b/hal/src/main/native/sim/SerialPort.cpp
new file mode 100644
index 0000000..2df2ebe
--- /dev/null
+++ b/hal/src/main/native/sim/SerialPort.cpp
@@ -0,0 +1,87 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "hal/SerialPort.h"
+
+#include "HALInitializer.h"
+
+namespace hal {
+namespace init {
+void InitializeSerialPort() {}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+HAL_SerialPortHandle HAL_InitializeSerialPort(HAL_SerialPort port,
+ int32_t* status) {
+ hal::init::CheckInit();
+ return HAL_kInvalidHandle;
+}
+
+HAL_SerialPortHandle HAL_InitializeSerialPortDirect(HAL_SerialPort port,
+ const char* portName,
+ int32_t* status) {
+ hal::init::CheckInit();
+ return HAL_kInvalidHandle;
+}
+
+int HAL_GetSerialFD(HAL_SerialPortHandle handle, int32_t* status) { return -1; }
+
+void HAL_SetSerialBaudRate(HAL_SerialPortHandle handle, int32_t baud,
+ int32_t* status) {}
+
+void HAL_SetSerialDataBits(HAL_SerialPortHandle handle, int32_t bits,
+ int32_t* status) {}
+
+void HAL_SetSerialParity(HAL_SerialPortHandle handle, int32_t parity,
+ int32_t* status) {}
+
+void HAL_SetSerialStopBits(HAL_SerialPortHandle handle, int32_t stopBits,
+ int32_t* status) {}
+
+void HAL_SetSerialWriteMode(HAL_SerialPortHandle handle, int32_t mode,
+ int32_t* status) {}
+
+void HAL_SetSerialFlowControl(HAL_SerialPortHandle handle, int32_t flow,
+ int32_t* status) {}
+
+void HAL_SetSerialTimeout(HAL_SerialPortHandle handle, double timeout,
+ int32_t* status) {}
+
+void HAL_EnableSerialTermination(HAL_SerialPortHandle handle, char terminator,
+ int32_t* status) {}
+
+void HAL_DisableSerialTermination(HAL_SerialPortHandle handle,
+ int32_t* status) {}
+
+void HAL_SetSerialReadBufferSize(HAL_SerialPortHandle handle, int32_t size,
+ int32_t* status) {}
+
+void HAL_SetSerialWriteBufferSize(HAL_SerialPortHandle handle, int32_t size,
+ int32_t* status) {}
+
+int32_t HAL_GetSerialBytesReceived(HAL_SerialPortHandle handle,
+ int32_t* status) {
+ return 0;
+}
+
+int32_t HAL_ReadSerial(HAL_SerialPortHandle handle, char* buffer, int32_t count,
+ int32_t* status) {
+ return 0;
+}
+
+int32_t HAL_WriteSerial(HAL_SerialPortHandle handle, const char* buffer,
+ int32_t count, int32_t* status) {
+ return 0;
+}
+
+void HAL_FlushSerial(HAL_SerialPortHandle handle, int32_t* status) {}
+
+void HAL_ClearSerial(HAL_SerialPortHandle handle, int32_t* status) {}
+
+void HAL_CloseSerial(HAL_SerialPortHandle handle, int32_t* status) {}
+} // extern "C"
diff --git a/hal/src/main/native/sim/SimDevice.cpp b/hal/src/main/native/sim/SimDevice.cpp
new file mode 100644
index 0000000..a8f8f80
--- /dev/null
+++ b/hal/src/main/native/sim/SimDevice.cpp
@@ -0,0 +1,75 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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 "hal/SimDevice.h"
+
+#include <wpi/SmallString.h>
+#include <wpi/raw_ostream.h>
+
+#include "HALInitializer.h"
+#include "mockdata/SimDeviceDataInternal.h"
+
+using namespace hal;
+
+namespace hal {
+namespace init {
+void InitializeSimDevice() {}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+
+HAL_SimDeviceHandle HAL_CreateSimDevice(const char* name) {
+ hal::init::CheckInit();
+ return SimSimDeviceData->CreateDevice(name);
+}
+
+void HAL_FreeSimDevice(HAL_SimDeviceHandle handle) {
+ SimSimDeviceData->FreeDevice(handle);
+}
+
+HAL_SimValueHandle HAL_CreateSimValue(HAL_SimDeviceHandle device,
+ const char* name, HAL_Bool readonly,
+ const struct HAL_Value* initialValue) {
+ return SimSimDeviceData->CreateValue(device, name, readonly, 0, nullptr,
+ *initialValue);
+}
+
+HAL_SimValueHandle HAL_CreateSimValueEnum(HAL_SimDeviceHandle device,
+ const char* name, HAL_Bool readonly,
+ int32_t numOptions,
+ const char** options,
+ int32_t initialValue) {
+ return SimSimDeviceData->CreateValue(device, name, readonly, numOptions,
+ options, HAL_MakeEnum(initialValue));
+}
+
+void HAL_GetSimValue(HAL_SimValueHandle handle, struct HAL_Value* value) {
+ *value = SimSimDeviceData->GetValue(handle);
+}
+
+void HAL_SetSimValue(HAL_SimValueHandle handle, const struct HAL_Value* value) {
+ SimSimDeviceData->SetValue(handle, *value);
+}
+
+hal::SimDevice::SimDevice(const char* name, int index) {
+ wpi::SmallString<128> fullname;
+ wpi::raw_svector_ostream os(fullname);
+ os << name << '[' << index << ']';
+
+ m_handle = HAL_CreateSimDevice(fullname.c_str());
+}
+
+hal::SimDevice::SimDevice(const char* name, int index, int channel) {
+ wpi::SmallString<128> fullname;
+ wpi::raw_svector_ostream os(fullname);
+ os << name << '[' << index << ',' << channel << ']';
+
+ m_handle = HAL_CreateSimDevice(fullname.c_str());
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/sim/Solenoid.cpp b/hal/src/main/native/sim/Solenoid.cpp
new file mode 100644
index 0000000..46bd285
--- /dev/null
+++ b/hal/src/main/native/sim/Solenoid.cpp
@@ -0,0 +1,145 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 "hal/Solenoid.h"
+
+#include "HALInitializer.h"
+#include "PortsInternal.h"
+#include "hal/Errors.h"
+#include "hal/handles/HandlesInternal.h"
+#include "hal/handles/IndexedHandleResource.h"
+#include "mockdata/PCMDataInternal.h"
+
+namespace {
+struct Solenoid {
+ uint8_t module;
+ uint8_t channel;
+};
+} // namespace
+
+using namespace hal;
+
+static IndexedHandleResource<HAL_SolenoidHandle, Solenoid,
+ kNumPCMModules * kNumSolenoidChannels,
+ HAL_HandleEnum::Solenoid>* solenoidHandles;
+
+namespace hal {
+namespace init {
+void InitializeSolenoid() {
+ static IndexedHandleResource<HAL_SolenoidHandle, Solenoid,
+ kNumPCMModules * kNumSolenoidChannels,
+ HAL_HandleEnum::Solenoid>
+ sH;
+ solenoidHandles = &sH;
+}
+} // namespace init
+} // namespace hal
+
+extern "C" {
+HAL_SolenoidHandle HAL_InitializeSolenoidPort(HAL_PortHandle portHandle,
+ int32_t* status) {
+ hal::init::CheckInit();
+ int16_t channel = getPortHandleChannel(portHandle);
+ int16_t module = getPortHandleModule(portHandle);
+ if (channel == InvalidHandleIndex) {
+ *status = HAL_HANDLE_ERROR;
+ return HAL_kInvalidHandle;
+ }
+
+ if (!HAL_CheckSolenoidChannel(channel)) {
+ *status = RESOURCE_OUT_OF_RANGE;
+ return HAL_kInvalidHandle;
+ }
+
+ if (!HAL_CheckSolenoidModule(module)) {
+ *status = RESOURCE_OUT_OF_RANGE;
+ return HAL_kInvalidHandle;
+ }
+
+ auto handle = solenoidHandles->Allocate(
+ module * kNumSolenoidChannels + channel, status);
+ if (handle == HAL_kInvalidHandle) { // out of resources
+ *status = NO_AVAILABLE_RESOURCES;
+ return HAL_kInvalidHandle;
+ }
+ auto solenoidPort = solenoidHandles->Get(handle);
+ if (solenoidPort == nullptr) { // would only occur on thread issues
+ *status = HAL_HANDLE_ERROR;
+ return HAL_kInvalidHandle;
+ }
+ solenoidPort->module = static_cast<uint8_t>(module);
+ solenoidPort->channel = static_cast<uint8_t>(channel);
+
+ HALSIM_SetPCMSolenoidInitialized(module, channel, true);
+
+ return handle;
+}
+void HAL_FreeSolenoidPort(HAL_SolenoidHandle solenoidPortHandle) {
+ auto port = solenoidHandles->Get(solenoidPortHandle);
+ if (port == nullptr) return;
+ solenoidHandles->Free(solenoidPortHandle);
+ HALSIM_SetPCMSolenoidInitialized(port->module, port->channel, false);
+}
+HAL_Bool HAL_CheckSolenoidModule(int32_t module) {
+ return module < kNumPCMModules && module >= 0;
+}
+
+HAL_Bool HAL_CheckSolenoidChannel(int32_t channel) {
+ return channel < kNumSolenoidChannels && channel >= 0;
+}
+HAL_Bool HAL_GetSolenoid(HAL_SolenoidHandle solenoidPortHandle,
+ int32_t* status) {
+ auto port = solenoidHandles->Get(solenoidPortHandle);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return false;
+ }
+
+ return HALSIM_GetPCMSolenoidOutput(port->module, port->channel);
+}
+int32_t HAL_GetAllSolenoids(int32_t module, int32_t* status) {
+ int32_t total = 0;
+ for (int i = 0; i < kNumSolenoidChannels; i++) {
+ int32_t channel = HALSIM_GetPCMSolenoidOutput(module, i) ? 1 : 0;
+ total = total + (channel << i);
+ }
+
+ return total;
+}
+void HAL_SetSolenoid(HAL_SolenoidHandle solenoidPortHandle, HAL_Bool value,
+ int32_t* status) {
+ auto port = solenoidHandles->Get(solenoidPortHandle);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ HALSIM_SetPCMSolenoidOutput(port->module, port->channel, value);
+}
+
+void HAL_SetAllSolenoids(int32_t module, int32_t state, int32_t* status) {
+ for (int i = 0; i < kNumSolenoidChannels; i++) {
+ int set = state & 1;
+ HALSIM_SetPCMSolenoidOutput(module, i, set);
+ state >>= 1;
+ }
+}
+
+int32_t HAL_GetPCMSolenoidBlackList(int32_t module, int32_t* status) {
+ return 0;
+}
+HAL_Bool HAL_GetPCMSolenoidVoltageStickyFault(int32_t module, int32_t* status) {
+ return 0;
+}
+HAL_Bool HAL_GetPCMSolenoidVoltageFault(int32_t module, int32_t* status) {
+ return 0;
+}
+void HAL_ClearAllPCMStickyFaults(int32_t module, int32_t* status) {}
+void HAL_SetOneShotDuration(HAL_SolenoidHandle solenoidPortHandle,
+ int32_t durMS, int32_t* status) {}
+void HAL_FireOneShot(HAL_SolenoidHandle solenoidPortHandle, int32_t* status) {}
+} // extern "C"
diff --git a/hal/src/main/native/sim/Threads.cpp b/hal/src/main/native/sim/Threads.cpp
new file mode 100644
index 0000000..2aa2c4b
--- /dev/null
+++ b/hal/src/main/native/sim/Threads.cpp
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 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 "hal/Threads.h"
+
+namespace hal {
+namespace init {
+void InitializeThreads() {}
+} // namespace init
+} // namespace hal
+
+int32_t HAL_GetThreadPriority(NativeThreadHandle handle, HAL_Bool* isRealTime,
+ int32_t* status) {
+ return 0;
+}
+int32_t HAL_GetCurrentThreadPriority(HAL_Bool* isRealTime, int32_t* status) {
+ return 0;
+}
+HAL_Bool HAL_SetThreadPriority(NativeThreadHandle handle, HAL_Bool realTime,
+ int32_t priority, int32_t* status) {
+ return true;
+}
+HAL_Bool HAL_SetCurrentThreadPriority(HAL_Bool realTime, int32_t priority,
+ int32_t* status) {
+ return true;
+}
diff --git a/hal/src/main/native/sim/jni/AccelerometerDataJNI.cpp b/hal/src/main/native/sim/jni/AccelerometerDataJNI.cpp
new file mode 100644
index 0000000..8964a41
--- /dev/null
+++ b/hal/src/main/native/sim/jni/AccelerometerDataJNI.cpp
@@ -0,0 +1,279 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 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 <jni.h>
+
+#include "CallbackStore.h"
+#include "edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI.h"
+#include "mockdata/AccelerometerData.h"
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI
+ * Method: registerActiveCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI_registerActiveCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterAccelerometerActiveCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI
+ * Method: cancelActiveCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI_cancelActiveCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelAccelerometerActiveCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI
+ * Method: getActive
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI_getActive
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetAccelerometerActive(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI
+ * Method: setActive
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI_setActive
+ (JNIEnv*, jclass, jint index, jboolean value)
+{
+ HALSIM_SetAccelerometerActive(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI
+ * Method: registerRangeCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI_registerRangeCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterAccelerometerRangeCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI
+ * Method: cancelRangeCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI_cancelRangeCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelAccelerometerRangeCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI
+ * Method: getRange
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI_getRange
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetAccelerometerRange(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI
+ * Method: setRange
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI_setRange
+ (JNIEnv*, jclass, jint index, jint value)
+{
+ HALSIM_SetAccelerometerRange(index,
+ static_cast<HAL_AccelerometerRange>(value));
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI
+ * Method: registerXCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI_registerXCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterAccelerometerXCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI
+ * Method: cancelXCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI_cancelXCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelAccelerometerXCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI
+ * Method: getX
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI_getX
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetAccelerometerX(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI
+ * Method: setX
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI_setX
+ (JNIEnv*, jclass, jint index, jdouble value)
+{
+ HALSIM_SetAccelerometerX(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI
+ * Method: registerYCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI_registerYCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterAccelerometerYCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI
+ * Method: cancelYCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI_cancelYCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelAccelerometerYCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI
+ * Method: getY
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI_getY
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetAccelerometerY(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI
+ * Method: setY
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI_setY
+ (JNIEnv*, jclass, jint index, jdouble value)
+{
+ HALSIM_SetAccelerometerY(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI
+ * Method: registerZCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI_registerZCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterAccelerometerZCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI
+ * Method: cancelZCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI_cancelZCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelAccelerometerZCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI
+ * Method: getZ
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI_getZ
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetAccelerometerZ(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI
+ * Method: setZ
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI_setZ
+ (JNIEnv*, jclass, jint index, jdouble value)
+{
+ HALSIM_SetAccelerometerZ(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI
+ * Method: resetData
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI_resetData
+ (JNIEnv*, jclass, jint index)
+{
+ HALSIM_ResetAccelerometerData(index);
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/sim/jni/AddressableLEDDataJNI.cpp b/hal/src/main/native/sim/jni/AddressableLEDDataJNI.cpp
new file mode 100644
index 0000000..530eae2
--- /dev/null
+++ b/hal/src/main/native/sim/jni/AddressableLEDDataJNI.cpp
@@ -0,0 +1,293 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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 <jni.h>
+
+#include "CallbackStore.h"
+#include "ConstBufferCallbackStore.h"
+#include "edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI.h"
+#include "mockdata/AddressableLEDData.h"
+
+static_assert(sizeof(jbyte) * 4 == sizeof(HAL_AddressableLEDData));
+
+using namespace wpi::java;
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
+ * Method: registerInitializedCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_registerInitializedCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(
+ env, index, callback, initialNotify,
+ &HALSIM_RegisterAddressableLEDInitializedCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
+ * Method: cancelInitializedCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_cancelInitializedCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelAddressableLEDInitializedCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
+ * Method: getInitialized
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_getInitialized
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetAddressableLEDInitialized(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
+ * Method: setInitialized
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_setInitialized
+ (JNIEnv*, jclass, jint index, jboolean value)
+{
+ HALSIM_SetAddressableLEDInitialized(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
+ * Method: registerOutputPortCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_registerOutputPortCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(
+ env, index, callback, initialNotify,
+ &HALSIM_RegisterAddressableLEDOutputPortCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
+ * Method: cancelOutputPortCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_cancelOutputPortCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelAddressableLEDOutputPortCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
+ * Method: getOutputPort
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_getOutputPort
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetAddressableLEDOutputPort(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
+ * Method: setOutputPort
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_setOutputPort
+ (JNIEnv*, jclass, jint index, jint value)
+{
+ HALSIM_SetAddressableLEDOutputPort(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
+ * Method: registerLengthCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_registerLengthCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterAddressableLEDLengthCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
+ * Method: cancelLengthCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_cancelLengthCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelAddressableLEDLengthCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
+ * Method: getLength
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_getLength
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetAddressableLEDLength(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
+ * Method: setLength
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_setLength
+ (JNIEnv*, jclass, jint index, jint value)
+{
+ HALSIM_SetAddressableLEDLength(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
+ * Method: registerRunningCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_registerRunningCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterAddressableLEDRunningCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
+ * Method: cancelRunningCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_cancelRunningCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelAddressableLEDRunningCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
+ * Method: getRunning
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_getRunning
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetAddressableLEDRunning(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
+ * Method: setRunning
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_setRunning
+ (JNIEnv*, jclass, jint index, jboolean value)
+{
+ HALSIM_SetAddressableLEDRunning(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
+ * Method: registerDataCallback
+ * Signature: (ILjava/lang/Object;)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_registerDataCallback
+ (JNIEnv* env, jclass, jint index, jobject callback)
+{
+ return sim::AllocateConstBufferCallback(
+ env, index, callback, &HALSIM_RegisterAddressableLEDDataCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
+ * Method: cancelDataCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_cancelDataCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ sim::FreeConstBufferCallback(env, handle, index,
+ &HALSIM_CancelAddressableLEDDataCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
+ * Method: getData
+ * Signature: (I)[B
+ */
+JNIEXPORT jbyteArray JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_getData
+ (JNIEnv* env, jclass, jint index)
+{
+ auto data =
+ std::make_unique<HAL_AddressableLEDData[]>(HAL_kAddressableLEDMaxLength);
+ int32_t length = HALSIM_GetAddressableLEDData(index, data.get());
+ return MakeJByteArray(
+ env, wpi::ArrayRef(reinterpret_cast<jbyte*>(data.get()), length * 4));
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
+ * Method: setData
+ * Signature: (I[B)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_setData
+ (JNIEnv* env, jclass, jint index, jbyteArray arr)
+{
+ JByteArrayRef jArrRef{env, arr};
+ auto arrRef = jArrRef.array();
+ HALSIM_SetAddressableLEDData(
+ index, reinterpret_cast<const HAL_AddressableLEDData*>(arrRef.data()),
+ arrRef.size() / 4);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
+ * Method: resetData
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_resetData
+ (JNIEnv*, jclass, jint index)
+{
+ HALSIM_ResetAddressableLEDData(index);
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/sim/jni/AnalogGyroDataJNI.cpp b/hal/src/main/native/sim/jni/AnalogGyroDataJNI.cpp
new file mode 100644
index 0000000..08d18de
--- /dev/null
+++ b/hal/src/main/native/sim/jni/AnalogGyroDataJNI.cpp
@@ -0,0 +1,178 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 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 <jni.h>
+
+#include "CallbackStore.h"
+#include "edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI.h"
+#include "mockdata/AnalogGyroData.h"
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI
+ * Method: registerAngleCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI_registerAngleCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterAnalogGyroAngleCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI
+ * Method: cancelAngleCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI_cancelAngleCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelAnalogGyroAngleCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI
+ * Method: getAngle
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI_getAngle
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetAnalogGyroAngle(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI
+ * Method: setAngle
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI_setAngle
+ (JNIEnv*, jclass, jint index, jdouble value)
+{
+ HALSIM_SetAnalogGyroAngle(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI
+ * Method: registerRateCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI_registerRateCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterAnalogGyroRateCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI
+ * Method: cancelRateCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI_cancelRateCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelAnalogGyroRateCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI
+ * Method: getRate
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI_getRate
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetAnalogGyroRate(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI
+ * Method: setRate
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI_setRate
+ (JNIEnv*, jclass, jint index, jdouble value)
+{
+ HALSIM_SetAnalogGyroRate(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI
+ * Method: registerInitializedCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI_registerInitializedCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterAnalogGyroInitializedCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI
+ * Method: cancelInitializedCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI_cancelInitializedCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelAnalogGyroInitializedCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI
+ * Method: getInitialized
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI_getInitialized
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetAnalogGyroInitialized(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI
+ * Method: setInitialized
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI_setInitialized
+ (JNIEnv*, jclass, jint index, jboolean value)
+{
+ HALSIM_SetAnalogGyroInitialized(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI
+ * Method: resetData
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI_resetData
+ (JNIEnv*, jclass, jint index)
+{
+ HALSIM_ResetAnalogGyroData(index);
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/sim/jni/AnalogInDataJNI.cpp b/hal/src/main/native/sim/jni/AnalogInDataJNI.cpp
new file mode 100644
index 0000000..c6cee7b
--- /dev/null
+++ b/hal/src/main/native/sim/jni/AnalogInDataJNI.cpp
@@ -0,0 +1,483 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 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 <jni.h>
+
+#include "CallbackStore.h"
+#include "edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI.h"
+#include "mockdata/AnalogInData.h"
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
+ * Method: registerInitializedCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_registerInitializedCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterAnalogInInitializedCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
+ * Method: cancelInitializedCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_cancelInitializedCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelAnalogInInitializedCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
+ * Method: getInitialized
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_getInitialized
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetAnalogInInitialized(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
+ * Method: setInitialized
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_setInitialized
+ (JNIEnv*, jclass, jint index, jboolean value)
+{
+ HALSIM_SetAnalogInInitialized(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
+ * Method: registerAverageBitsCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_registerAverageBitsCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterAnalogInAverageBitsCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
+ * Method: cancelAverageBitsCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_cancelAverageBitsCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelAnalogInAverageBitsCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
+ * Method: getAverageBits
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_getAverageBits
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetAnalogInAverageBits(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
+ * Method: setAverageBits
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_setAverageBits
+ (JNIEnv*, jclass, jint index, jint value)
+{
+ HALSIM_SetAnalogInAverageBits(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
+ * Method: registerOversampleBitsCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_registerOversampleBitsCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterAnalogInOversampleBitsCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
+ * Method: cancelOversampleBitsCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_cancelOversampleBitsCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelAnalogInOversampleBitsCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
+ * Method: getOversampleBits
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_getOversampleBits
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetAnalogInOversampleBits(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
+ * Method: setOversampleBits
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_setOversampleBits
+ (JNIEnv*, jclass, jint index, jint value)
+{
+ HALSIM_SetAnalogInOversampleBits(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
+ * Method: registerVoltageCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_registerVoltageCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterAnalogInVoltageCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
+ * Method: cancelVoltageCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_cancelVoltageCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelAnalogInVoltageCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
+ * Method: getVoltage
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_getVoltage
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetAnalogInVoltage(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
+ * Method: setVoltage
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_setVoltage
+ (JNIEnv*, jclass, jint index, jdouble value)
+{
+ HALSIM_SetAnalogInVoltage(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
+ * Method: registerAccumulatorInitializedCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_registerAccumulatorInitializedCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(
+ env, index, callback, initialNotify,
+ &HALSIM_RegisterAnalogInAccumulatorInitializedCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
+ * Method: cancelAccumulatorInitializedCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_cancelAccumulatorInitializedCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(
+ env, handle, index, &HALSIM_CancelAnalogInAccumulatorInitializedCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
+ * Method: getAccumulatorInitialized
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_getAccumulatorInitialized
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetAnalogInAccumulatorInitialized(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
+ * Method: setAccumulatorInitialized
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_setAccumulatorInitialized
+ (JNIEnv*, jclass, jint index, jboolean value)
+{
+ HALSIM_SetAnalogInAccumulatorInitialized(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
+ * Method: registerAccumulatorValueCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_registerAccumulatorValueCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(
+ env, index, callback, initialNotify,
+ &HALSIM_RegisterAnalogInAccumulatorValueCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
+ * Method: cancelAccumulatorValueCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_cancelAccumulatorValueCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelAnalogInAccumulatorValueCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
+ * Method: getAccumulatorValue
+ * Signature: (I)J
+ */
+JNIEXPORT jlong JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_getAccumulatorValue
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetAnalogInAccumulatorValue(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
+ * Method: setAccumulatorValue
+ * Signature: (IJ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_setAccumulatorValue
+ (JNIEnv*, jclass, jint index, jlong value)
+{
+ HALSIM_SetAnalogInAccumulatorValue(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
+ * Method: registerAccumulatorCountCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_registerAccumulatorCountCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(
+ env, index, callback, initialNotify,
+ &HALSIM_RegisterAnalogInAccumulatorCountCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
+ * Method: cancelAccumulatorCountCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_cancelAccumulatorCountCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelAnalogInAccumulatorCountCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
+ * Method: getAccumulatorCount
+ * Signature: (I)J
+ */
+JNIEXPORT jlong JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_getAccumulatorCount
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetAnalogInAccumulatorCount(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
+ * Method: setAccumulatorCount
+ * Signature: (IJ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_setAccumulatorCount
+ (JNIEnv*, jclass, jint index, jlong value)
+{
+ HALSIM_SetAnalogInAccumulatorCount(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
+ * Method: registerAccumulatorCenterCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_registerAccumulatorCenterCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(
+ env, index, callback, initialNotify,
+ &HALSIM_RegisterAnalogInAccumulatorCenterCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
+ * Method: cancelAccumulatorCenterCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_cancelAccumulatorCenterCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelAnalogInAccumulatorCenterCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
+ * Method: getAccumulatorCenter
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_getAccumulatorCenter
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetAnalogInAccumulatorCenter(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
+ * Method: setAccumulatorCenter
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_setAccumulatorCenter
+ (JNIEnv*, jclass, jint index, jint value)
+{
+ HALSIM_SetAnalogInAccumulatorCenter(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
+ * Method: registerAccumulatorDeadbandCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_registerAccumulatorDeadbandCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(
+ env, index, callback, initialNotify,
+ &HALSIM_RegisterAnalogInAccumulatorDeadbandCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
+ * Method: cancelAccumulatorDeadbandCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_cancelAccumulatorDeadbandCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelAnalogInAccumulatorDeadbandCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
+ * Method: getAccumulatorDeadband
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_getAccumulatorDeadband
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetAnalogInAccumulatorDeadband(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
+ * Method: setAccumulatorDeadband
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_setAccumulatorDeadband
+ (JNIEnv*, jclass, jint index, jint value)
+{
+ HALSIM_SetAnalogInAccumulatorDeadband(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
+ * Method: resetData
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_resetData
+ (JNIEnv*, jclass, jint index)
+{
+ HALSIM_ResetAnalogInData(index);
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/sim/jni/AnalogOutDataJNI.cpp b/hal/src/main/native/sim/jni/AnalogOutDataJNI.cpp
new file mode 100644
index 0000000..af9d6d6
--- /dev/null
+++ b/hal/src/main/native/sim/jni/AnalogOutDataJNI.cpp
@@ -0,0 +1,128 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 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 <jni.h>
+
+#include "CallbackStore.h"
+#include "edu_wpi_first_hal_sim_mockdata_AnalogOutDataJNI.h"
+#include "mockdata/AnalogOutData.h"
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogOutDataJNI
+ * Method: registerVoltageCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogOutDataJNI_registerVoltageCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterAnalogOutVoltageCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogOutDataJNI
+ * Method: cancelVoltageCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogOutDataJNI_cancelVoltageCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelAnalogOutVoltageCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogOutDataJNI
+ * Method: getVoltage
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogOutDataJNI_getVoltage
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetAnalogOutVoltage(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogOutDataJNI
+ * Method: setVoltage
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogOutDataJNI_setVoltage
+ (JNIEnv*, jclass, jint index, jdouble value)
+{
+ HALSIM_SetAnalogOutVoltage(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogOutDataJNI
+ * Method: registerInitializedCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogOutDataJNI_registerInitializedCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterAnalogOutInitializedCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogOutDataJNI
+ * Method: cancelInitializedCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogOutDataJNI_cancelInitializedCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelAnalogOutInitializedCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogOutDataJNI
+ * Method: getInitialized
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogOutDataJNI_getInitialized
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetAnalogOutInitialized(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogOutDataJNI
+ * Method: setInitialized
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogOutDataJNI_setInitialized
+ (JNIEnv*, jclass, jint index, jboolean value)
+{
+ HALSIM_SetAnalogOutInitialized(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogOutDataJNI
+ * Method: resetData
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogOutDataJNI_resetData
+ (JNIEnv*, jclass, jint index)
+{
+ HALSIM_ResetAnalogOutData(index);
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/sim/jni/AnalogTriggerDataJNI.cpp b/hal/src/main/native/sim/jni/AnalogTriggerDataJNI.cpp
new file mode 100644
index 0000000..66af737
--- /dev/null
+++ b/hal/src/main/native/sim/jni/AnalogTriggerDataJNI.cpp
@@ -0,0 +1,181 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 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 <jni.h>
+
+#include "CallbackStore.h"
+#include "edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI.h"
+#include "mockdata/AnalogTriggerData.h"
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI
+ * Method: registerInitializedCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI_registerInitializedCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(
+ env, index, callback, initialNotify,
+ &HALSIM_RegisterAnalogTriggerInitializedCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI
+ * Method: cancelInitializedCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI_cancelInitializedCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelAnalogTriggerInitializedCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI
+ * Method: getInitialized
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI_getInitialized
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetAnalogTriggerInitialized(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI
+ * Method: setInitialized
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI_setInitialized
+ (JNIEnv*, jclass, jint index, jboolean value)
+{
+ HALSIM_SetAnalogTriggerInitialized(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI
+ * Method: registerTriggerLowerBoundCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI_registerTriggerLowerBoundCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(
+ env, index, callback, initialNotify,
+ &HALSIM_RegisterAnalogTriggerTriggerLowerBoundCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI
+ * Method: cancelTriggerLowerBoundCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI_cancelTriggerLowerBoundCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(
+ env, handle, index, &HALSIM_CancelAnalogTriggerTriggerLowerBoundCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI
+ * Method: getTriggerLowerBound
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI_getTriggerLowerBound
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetAnalogTriggerTriggerLowerBound(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI
+ * Method: setTriggerLowerBound
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI_setTriggerLowerBound
+ (JNIEnv*, jclass, jint index, jdouble value)
+{
+ HALSIM_SetAnalogTriggerTriggerLowerBound(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI
+ * Method: registerTriggerUpperBoundCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI_registerTriggerUpperBoundCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(
+ env, index, callback, initialNotify,
+ &HALSIM_RegisterAnalogTriggerTriggerUpperBoundCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI
+ * Method: cancelTriggerUpperBoundCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI_cancelTriggerUpperBoundCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(
+ env, handle, index, &HALSIM_CancelAnalogTriggerTriggerUpperBoundCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI
+ * Method: getTriggerUpperBound
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI_getTriggerUpperBound
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetAnalogTriggerTriggerUpperBound(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI
+ * Method: setTriggerUpperBound
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI_setTriggerUpperBound
+ (JNIEnv*, jclass, jint index, jdouble value)
+{
+ HALSIM_SetAnalogTriggerTriggerUpperBound(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI
+ * Method: resetData
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI_resetData
+ (JNIEnv*, jclass, jint index)
+{
+ HALSIM_ResetAnalogTriggerData(index);
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/sim/jni/BufferCallbackStore.cpp b/hal/src/main/native/sim/jni/BufferCallbackStore.cpp
new file mode 100644
index 0000000..3c9941e
--- /dev/null
+++ b/hal/src/main/native/sim/jni/BufferCallbackStore.cpp
@@ -0,0 +1,125 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 "BufferCallbackStore.h"
+
+#include <jni.h>
+
+#include <wpi/jni_util.h>
+
+#include "SimulatorJNI.h"
+#include "hal/Types.h"
+#include "hal/Value.h"
+#include "hal/handles/UnlimitedHandleResource.h"
+#include "mockdata/NotifyListener.h"
+
+using namespace wpi::java;
+using namespace sim;
+
+static hal::UnlimitedHandleResource<SIM_JniHandle, BufferCallbackStore,
+ hal::HAL_HandleEnum::SimulationJni>*
+ callbackHandles;
+
+namespace sim {
+void InitializeBufferStore() {
+ static hal::UnlimitedHandleResource<SIM_JniHandle, BufferCallbackStore,
+ hal::HAL_HandleEnum::SimulationJni>
+ cb;
+ callbackHandles = &cb;
+}
+} // namespace sim
+
+void BufferCallbackStore::create(JNIEnv* env, jobject obj) {
+ m_call = JGlobal<jobject>(env, obj);
+}
+
+void BufferCallbackStore::performCallback(const char* name, uint8_t* buffer,
+ uint32_t length) {
+ JNIEnv* env;
+ JavaVM* vm = sim::GetJVM();
+ bool didAttachThread = false;
+ int tryGetEnv = vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6);
+ if (tryGetEnv == JNI_EDETACHED) {
+ // Thread not attached
+ didAttachThread = true;
+ if (vm->AttachCurrentThread(reinterpret_cast<void**>(&env), nullptr) != 0) {
+ // Failed to attach, log and return
+ wpi::outs() << "Failed to attach\n";
+ wpi::outs().flush();
+ return;
+ }
+ } else if (tryGetEnv == JNI_EVERSION) {
+ wpi::outs() << "Invalid JVM Version requested\n";
+ wpi::outs().flush();
+ }
+
+ auto toCallbackArr =
+ MakeJByteArray(env, wpi::StringRef{reinterpret_cast<const char*>(buffer),
+ static_cast<size_t>(length)});
+
+ env->CallVoidMethod(m_call, sim::GetBufferCallback(), MakeJString(env, name),
+ toCallbackArr, (jint)length);
+
+ jbyte* fromCallbackArr = reinterpret_cast<jbyte*>(
+ env->GetPrimitiveArrayCritical(toCallbackArr, nullptr));
+
+ for (size_t i = 0; i < length; i++) {
+ buffer[i] = fromCallbackArr[i];
+ }
+
+ env->ReleasePrimitiveArrayCritical(toCallbackArr, fromCallbackArr, JNI_ABORT);
+
+ if (env->ExceptionCheck()) {
+ env->ExceptionDescribe();
+ }
+
+ if (didAttachThread) {
+ vm->DetachCurrentThread();
+ }
+}
+
+void BufferCallbackStore::free(JNIEnv* env) { m_call.free(env); }
+
+SIM_JniHandle sim::AllocateBufferCallback(
+ JNIEnv* env, jint index, jobject callback,
+ RegisterBufferCallbackFunc createCallback) {
+ auto callbackStore = std::make_shared<BufferCallbackStore>();
+
+ auto handle = callbackHandles->Allocate(callbackStore);
+
+ if (handle == HAL_kInvalidHandle) {
+ return -1;
+ }
+
+ uintptr_t handleAsPtr = static_cast<uintptr_t>(handle);
+ void* handleAsVoidPtr = reinterpret_cast<void*>(handleAsPtr);
+
+ callbackStore->create(env, callback);
+
+ auto callbackFunc = [](const char* name, void* param, uint8_t* buffer,
+ uint32_t length) {
+ uintptr_t handleTmp = reinterpret_cast<uintptr_t>(param);
+ SIM_JniHandle handle = static_cast<SIM_JniHandle>(handleTmp);
+ auto data = callbackHandles->Get(handle);
+ if (!data) return;
+
+ data->performCallback(name, buffer, length);
+ };
+
+ auto id = createCallback(index, callbackFunc, handleAsVoidPtr);
+
+ callbackStore->setCallbackId(id);
+
+ return handle;
+}
+
+void sim::FreeBufferCallback(JNIEnv* env, SIM_JniHandle handle, jint index,
+ FreeBufferCallbackFunc freeCallback) {
+ auto callback = callbackHandles->Free(handle);
+ freeCallback(index, callback->getCallbackId());
+ callback->free(env);
+}
diff --git a/hal/src/main/native/sim/jni/BufferCallbackStore.h b/hal/src/main/native/sim/jni/BufferCallbackStore.h
new file mode 100644
index 0000000..6b472ac
--- /dev/null
+++ b/hal/src/main/native/sim/jni/BufferCallbackStore.h
@@ -0,0 +1,45 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 <jni.h>
+
+#include <wpi/jni_util.h>
+
+#include "SimulatorJNI.h"
+#include "hal/Types.h"
+#include "hal/Value.h"
+#include "hal/handles/UnlimitedHandleResource.h"
+#include "mockdata/NotifyListener.h"
+
+namespace sim {
+class BufferCallbackStore {
+ public:
+ void create(JNIEnv* env, jobject obj);
+ void performCallback(const char* name, uint8_t* buffer, uint32_t length);
+ void free(JNIEnv* env);
+ void setCallbackId(int32_t id) { callbackId = id; }
+ int32_t getCallbackId() { return callbackId; }
+
+ private:
+ wpi::java::JGlobal<jobject> m_call;
+ int32_t callbackId;
+};
+
+void InitializeBufferStore();
+
+typedef int32_t (*RegisterBufferCallbackFunc)(int32_t index,
+ HAL_BufferCallback callback,
+ void* param);
+typedef void (*FreeBufferCallbackFunc)(int32_t index, int32_t uid);
+
+SIM_JniHandle AllocateBufferCallback(JNIEnv* env, jint index, jobject callback,
+ RegisterBufferCallbackFunc createCallback);
+void FreeBufferCallback(JNIEnv* env, SIM_JniHandle handle, jint index,
+ FreeBufferCallbackFunc freeCallback);
+} // namespace sim
diff --git a/hal/src/main/native/sim/jni/CallbackStore.cpp b/hal/src/main/native/sim/jni/CallbackStore.cpp
new file mode 100644
index 0000000..318ab1e
--- /dev/null
+++ b/hal/src/main/native/sim/jni/CallbackStore.cpp
@@ -0,0 +1,194 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 "CallbackStore.h"
+
+#include <jni.h>
+
+#include <wpi/jni_util.h>
+
+#include "SimulatorJNI.h"
+#include "hal/Types.h"
+#include "hal/Value.h"
+#include "hal/handles/UnlimitedHandleResource.h"
+#include "mockdata/NotifyListener.h"
+
+using namespace wpi::java;
+using namespace sim;
+
+static hal::UnlimitedHandleResource<SIM_JniHandle, CallbackStore,
+ hal::HAL_HandleEnum::SimulationJni>*
+ callbackHandles;
+
+namespace sim {
+void InitializeStore() {
+ static hal::UnlimitedHandleResource<SIM_JniHandle, CallbackStore,
+ hal::HAL_HandleEnum::SimulationJni>
+ cb;
+ callbackHandles = &cb;
+}
+} // namespace sim
+
+void CallbackStore::create(JNIEnv* env, jobject obj) {
+ m_call = JGlobal<jobject>(env, obj);
+}
+
+void CallbackStore::performCallback(const char* name, const HAL_Value* value) {
+ JNIEnv* env;
+ JavaVM* vm = sim::GetJVM();
+ bool didAttachThread = false;
+ int tryGetEnv = vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6);
+ if (tryGetEnv == JNI_EDETACHED) {
+ // Thread not attached
+ didAttachThread = true;
+ if (vm->AttachCurrentThread(reinterpret_cast<void**>(&env), nullptr) != 0) {
+ // Failed to attach, log and return
+ wpi::outs() << "Failed to attach\n";
+ wpi::outs().flush();
+ return;
+ }
+ } else if (tryGetEnv == JNI_EVERSION) {
+ wpi::outs() << "Invalid JVM Version requested\n";
+ wpi::outs().flush();
+ }
+
+ env->CallVoidMethod(m_call, sim::GetNotifyCallback(), MakeJString(env, name),
+ (jint)value->type, (jlong)value->data.v_long,
+ (jdouble)value->data.v_double);
+
+ if (env->ExceptionCheck()) {
+ env->ExceptionDescribe();
+ }
+
+ if (didAttachThread) {
+ vm->DetachCurrentThread();
+ }
+}
+
+void CallbackStore::free(JNIEnv* env) { m_call.free(env); }
+
+SIM_JniHandle sim::AllocateCallback(JNIEnv* env, jint index, jobject callback,
+ jboolean initialNotify,
+ RegisterCallbackFunc createCallback) {
+ auto callbackStore = std::make_shared<CallbackStore>();
+
+ auto handle = callbackHandles->Allocate(callbackStore);
+
+ if (handle == HAL_kInvalidHandle) {
+ return -1;
+ }
+
+ uintptr_t handleAsPtr = static_cast<uintptr_t>(handle);
+ void* handleAsVoidPtr = reinterpret_cast<void*>(handleAsPtr);
+
+ callbackStore->create(env, callback);
+
+ auto callbackFunc = [](const char* name, void* param,
+ const HAL_Value* value) {
+ uintptr_t handleTmp = reinterpret_cast<uintptr_t>(param);
+ SIM_JniHandle handle = static_cast<SIM_JniHandle>(handleTmp);
+ auto data = callbackHandles->Get(handle);
+ if (!data) return;
+
+ data->performCallback(name, value);
+ };
+
+ auto id = createCallback(index, callbackFunc, handleAsVoidPtr, initialNotify);
+
+ callbackStore->setCallbackId(id);
+
+ return handle;
+}
+
+void sim::FreeCallback(JNIEnv* env, SIM_JniHandle handle, jint index,
+ FreeCallbackFunc freeCallback) {
+ auto callback = callbackHandles->Free(handle);
+ freeCallback(index, callback->getCallbackId());
+ callback->free(env);
+}
+
+SIM_JniHandle sim::AllocateChannelCallback(
+ JNIEnv* env, jint index, jint channel, jobject callback,
+ jboolean initialNotify, RegisterChannelCallbackFunc createCallback) {
+ auto callbackStore = std::make_shared<CallbackStore>();
+
+ auto handle = callbackHandles->Allocate(callbackStore);
+
+ if (handle == HAL_kInvalidHandle) {
+ return -1;
+ }
+
+ uintptr_t handleAsPtr = static_cast<uintptr_t>(handle);
+ void* handleAsVoidPtr = reinterpret_cast<void*>(handleAsPtr);
+
+ callbackStore->create(env, callback);
+
+ auto callbackFunc = [](const char* name, void* param,
+ const HAL_Value* value) {
+ uintptr_t handleTmp = reinterpret_cast<uintptr_t>(param);
+ SIM_JniHandle handle = static_cast<SIM_JniHandle>(handleTmp);
+ auto data = callbackHandles->Get(handle);
+ if (!data) return;
+
+ data->performCallback(name, value);
+ };
+
+ auto id = createCallback(index, channel, callbackFunc, handleAsVoidPtr,
+ initialNotify);
+
+ callbackStore->setCallbackId(id);
+
+ return handle;
+}
+
+void sim::FreeChannelCallback(JNIEnv* env, SIM_JniHandle handle, jint index,
+ jint channel,
+ FreeChannelCallbackFunc freeCallback) {
+ auto callback = callbackHandles->Free(handle);
+ freeCallback(index, channel, callback->getCallbackId());
+ callback->free(env);
+}
+
+SIM_JniHandle sim::AllocateCallbackNoIndex(
+ JNIEnv* env, jobject callback, jboolean initialNotify,
+ RegisterCallbackNoIndexFunc createCallback) {
+ auto callbackStore = std::make_shared<CallbackStore>();
+
+ auto handle = callbackHandles->Allocate(callbackStore);
+
+ if (handle == HAL_kInvalidHandle) {
+ return -1;
+ }
+
+ uintptr_t handleAsPtr = static_cast<uintptr_t>(handle);
+ void* handleAsVoidPtr = reinterpret_cast<void*>(handleAsPtr);
+
+ callbackStore->create(env, callback);
+
+ auto callbackFunc = [](const char* name, void* param,
+ const HAL_Value* value) {
+ uintptr_t handleTmp = reinterpret_cast<uintptr_t>(param);
+ SIM_JniHandle handle = static_cast<SIM_JniHandle>(handleTmp);
+ auto data = callbackHandles->Get(handle);
+ if (!data) return;
+
+ data->performCallback(name, value);
+ };
+
+ auto id = createCallback(callbackFunc, handleAsVoidPtr, initialNotify);
+
+ callbackStore->setCallbackId(id);
+
+ return handle;
+}
+
+void sim::FreeCallbackNoIndex(JNIEnv* env, SIM_JniHandle handle,
+ FreeCallbackNoIndexFunc freeCallback) {
+ auto callback = callbackHandles->Free(handle);
+ freeCallback(callback->getCallbackId());
+ callback->free(env);
+}
diff --git a/hal/src/main/native/sim/jni/CallbackStore.h b/hal/src/main/native/sim/jni/CallbackStore.h
new file mode 100644
index 0000000..eacf314
--- /dev/null
+++ b/hal/src/main/native/sim/jni/CallbackStore.h
@@ -0,0 +1,66 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 <jni.h>
+
+#include <wpi/jni_util.h>
+
+#include "SimulatorJNI.h"
+#include "hal/Types.h"
+#include "hal/Value.h"
+#include "hal/handles/UnlimitedHandleResource.h"
+#include "mockdata/NotifyListener.h"
+
+namespace sim {
+class CallbackStore {
+ public:
+ void create(JNIEnv* env, jobject obj);
+ void performCallback(const char* name, const HAL_Value* value);
+ void free(JNIEnv* env);
+ void setCallbackId(int32_t id) { callbackId = id; }
+ int32_t getCallbackId() { return callbackId; }
+
+ private:
+ wpi::java::JGlobal<jobject> m_call;
+ int32_t callbackId;
+};
+
+void InitializeStore();
+
+typedef int32_t (*RegisterCallbackFunc)(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param, HAL_Bool initialNotify);
+typedef void (*FreeCallbackFunc)(int32_t index, int32_t uid);
+typedef int32_t (*RegisterChannelCallbackFunc)(int32_t index, int32_t channel,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+typedef void (*FreeChannelCallbackFunc)(int32_t index, int32_t channel,
+ int32_t uid);
+typedef int32_t (*RegisterCallbackNoIndexFunc)(HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify);
+typedef void (*FreeCallbackNoIndexFunc)(int32_t uid);
+
+SIM_JniHandle AllocateCallback(JNIEnv* env, jint index, jobject callback,
+ jboolean initialNotify,
+ RegisterCallbackFunc createCallback);
+SIM_JniHandle AllocateChannelCallback(
+ JNIEnv* env, jint index, jint channel, jobject callback,
+ jboolean initialNotify, RegisterChannelCallbackFunc createCallback);
+SIM_JniHandle AllocateCallbackNoIndex(
+ JNIEnv* env, jobject callback, jboolean initialNotify,
+ RegisterCallbackNoIndexFunc createCallback);
+void FreeCallback(JNIEnv* env, SIM_JniHandle handle, jint index,
+ FreeCallbackFunc freeCallback);
+void FreeChannelCallback(JNIEnv* env, SIM_JniHandle handle, jint index,
+ jint channel, FreeChannelCallbackFunc freeCallback);
+void FreeCallbackNoIndex(JNIEnv* env, SIM_JniHandle handle,
+ FreeCallbackNoIndexFunc freeCallback);
+} // namespace sim
diff --git a/hal/src/main/native/sim/jni/ConstBufferCallbackStore.cpp b/hal/src/main/native/sim/jni/ConstBufferCallbackStore.cpp
new file mode 100644
index 0000000..d681983
--- /dev/null
+++ b/hal/src/main/native/sim/jni/ConstBufferCallbackStore.cpp
@@ -0,0 +1,117 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 "ConstBufferCallbackStore.h"
+
+#include <jni.h>
+
+#include <wpi/jni_util.h>
+
+#include "SimulatorJNI.h"
+#include "hal/Types.h"
+#include "hal/Value.h"
+#include "hal/handles/UnlimitedHandleResource.h"
+#include "mockdata/NotifyListener.h"
+
+using namespace wpi::java;
+using namespace sim;
+
+static hal::UnlimitedHandleResource<SIM_JniHandle, ConstBufferCallbackStore,
+ hal::HAL_HandleEnum::SimulationJni>*
+ callbackHandles;
+
+namespace sim {
+void InitializeConstBufferStore() {
+ static hal::UnlimitedHandleResource<SIM_JniHandle, ConstBufferCallbackStore,
+ hal::HAL_HandleEnum::SimulationJni>
+ cb;
+ callbackHandles = &cb;
+}
+} // namespace sim
+
+void ConstBufferCallbackStore::create(JNIEnv* env, jobject obj) {
+ m_call = JGlobal<jobject>(env, obj);
+}
+
+void ConstBufferCallbackStore::performCallback(const char* name,
+ const uint8_t* buffer,
+ uint32_t length) {
+ JNIEnv* env;
+ JavaVM* vm = sim::GetJVM();
+ bool didAttachThread = false;
+ int tryGetEnv = vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6);
+ if (tryGetEnv == JNI_EDETACHED) {
+ // Thread not attached
+ didAttachThread = true;
+ if (vm->AttachCurrentThread(reinterpret_cast<void**>(&env), nullptr) != 0) {
+ // Failed to attach, log and return
+ wpi::outs() << "Failed to attach\n";
+ wpi::outs().flush();
+ return;
+ }
+ } else if (tryGetEnv == JNI_EVERSION) {
+ wpi::outs() << "Invalid JVM Version requested\n";
+ wpi::outs().flush();
+ }
+
+ auto toCallbackArr =
+ MakeJByteArray(env, wpi::StringRef{reinterpret_cast<const char*>(buffer),
+ static_cast<size_t>(length)});
+
+ env->CallVoidMethod(m_call, sim::GetConstBufferCallback(),
+ MakeJString(env, name), toCallbackArr, (jint)length);
+
+ if (env->ExceptionCheck()) {
+ env->ExceptionDescribe();
+ }
+
+ if (didAttachThread) {
+ vm->DetachCurrentThread();
+ }
+}
+
+void ConstBufferCallbackStore::free(JNIEnv* env) { m_call.free(env); }
+
+SIM_JniHandle sim::AllocateConstBufferCallback(
+ JNIEnv* env, jint index, jobject callback,
+ RegisterConstBufferCallbackFunc createCallback) {
+ auto callbackStore = std::make_shared<ConstBufferCallbackStore>();
+
+ auto handle = callbackHandles->Allocate(callbackStore);
+
+ if (handle == HAL_kInvalidHandle) {
+ return -1;
+ }
+
+ uintptr_t handleAsPtr = static_cast<uintptr_t>(handle);
+ void* handleAsVoidPtr = reinterpret_cast<void*>(handleAsPtr);
+
+ callbackStore->create(env, callback);
+
+ auto callbackFunc = [](const char* name, void* param, const uint8_t* buffer,
+ uint32_t length) {
+ uintptr_t handleTmp = reinterpret_cast<uintptr_t>(param);
+ SIM_JniHandle handle = static_cast<SIM_JniHandle>(handleTmp);
+ auto data = callbackHandles->Get(handle);
+ if (!data) return;
+
+ data->performCallback(name, buffer, length);
+ };
+
+ auto id = createCallback(index, callbackFunc, handleAsVoidPtr);
+
+ callbackStore->setCallbackId(id);
+
+ return handle;
+}
+
+void sim::FreeConstBufferCallback(JNIEnv* env, SIM_JniHandle handle, jint index,
+ FreeConstBufferCallbackFunc freeCallback) {
+ auto callback = callbackHandles->Free(handle);
+ freeCallback(index, callback->getCallbackId());
+ callback->free(env);
+}
diff --git a/hal/src/main/native/sim/jni/ConstBufferCallbackStore.h b/hal/src/main/native/sim/jni/ConstBufferCallbackStore.h
new file mode 100644
index 0000000..2164a74
--- /dev/null
+++ b/hal/src/main/native/sim/jni/ConstBufferCallbackStore.h
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 <jni.h>
+
+#include <wpi/jni_util.h>
+
+#include "SimulatorJNI.h"
+#include "hal/Types.h"
+#include "hal/Value.h"
+#include "hal/handles/UnlimitedHandleResource.h"
+#include "mockdata/NotifyListener.h"
+
+namespace sim {
+class ConstBufferCallbackStore {
+ public:
+ void create(JNIEnv* env, jobject obj);
+ void performCallback(const char* name, const uint8_t* buffer,
+ uint32_t length);
+ void free(JNIEnv* env);
+ void setCallbackId(int32_t id) { callbackId = id; }
+ int32_t getCallbackId() { return callbackId; }
+
+ private:
+ wpi::java::JGlobal<jobject> m_call;
+ int32_t callbackId;
+};
+
+void InitializeConstBufferStore();
+
+typedef int32_t (*RegisterConstBufferCallbackFunc)(
+ int32_t index, HAL_ConstBufferCallback callback, void* param);
+typedef void (*FreeConstBufferCallbackFunc)(int32_t index, int32_t uid);
+
+SIM_JniHandle AllocateConstBufferCallback(
+ JNIEnv* env, jint index, jobject callback,
+ RegisterConstBufferCallbackFunc createCallback);
+void FreeConstBufferCallback(JNIEnv* env, SIM_JniHandle handle, jint index,
+ FreeConstBufferCallbackFunc freeCallback);
+} // namespace sim
diff --git a/hal/src/main/native/sim/jni/DIODataJNI.cpp b/hal/src/main/native/sim/jni/DIODataJNI.cpp
new file mode 100644
index 0000000..2ad55f9
--- /dev/null
+++ b/hal/src/main/native/sim/jni/DIODataJNI.cpp
@@ -0,0 +1,277 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 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 <jni.h>
+
+#include "CallbackStore.h"
+#include "edu_wpi_first_hal_sim_mockdata_DIODataJNI.h"
+#include "mockdata/DIOData.h"
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DIODataJNI
+ * Method: registerInitializedCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DIODataJNI_registerInitializedCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterDIOInitializedCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DIODataJNI
+ * Method: cancelInitializedCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DIODataJNI_cancelInitializedCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelDIOInitializedCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DIODataJNI
+ * Method: getInitialized
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DIODataJNI_getInitialized
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetDIOInitialized(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DIODataJNI
+ * Method: setInitialized
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DIODataJNI_setInitialized
+ (JNIEnv*, jclass, jint index, jboolean value)
+{
+ HALSIM_SetDIOInitialized(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DIODataJNI
+ * Method: registerValueCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DIODataJNI_registerValueCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterDIOValueCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DIODataJNI
+ * Method: cancelValueCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DIODataJNI_cancelValueCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index, &HALSIM_CancelDIOValueCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DIODataJNI
+ * Method: getValue
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DIODataJNI_getValue
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetDIOValue(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DIODataJNI
+ * Method: setValue
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DIODataJNI_setValue
+ (JNIEnv*, jclass, jint index, jboolean value)
+{
+ HALSIM_SetDIOValue(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DIODataJNI
+ * Method: registerPulseLengthCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DIODataJNI_registerPulseLengthCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterDIOPulseLengthCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DIODataJNI
+ * Method: cancelPulseLengthCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DIODataJNI_cancelPulseLengthCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelDIOPulseLengthCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DIODataJNI
+ * Method: getPulseLength
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DIODataJNI_getPulseLength
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetDIOPulseLength(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DIODataJNI
+ * Method: setPulseLength
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DIODataJNI_setPulseLength
+ (JNIEnv*, jclass, jint index, jdouble value)
+{
+ HALSIM_SetDIOPulseLength(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DIODataJNI
+ * Method: registerIsInputCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DIODataJNI_registerIsInputCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterDIOIsInputCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DIODataJNI
+ * Method: cancelIsInputCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DIODataJNI_cancelIsInputCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelDIOIsInputCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DIODataJNI
+ * Method: getIsInput
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DIODataJNI_getIsInput
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetDIOIsInput(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DIODataJNI
+ * Method: setIsInput
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DIODataJNI_setIsInput
+ (JNIEnv*, jclass, jint index, jboolean value)
+{
+ HALSIM_SetDIOIsInput(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DIODataJNI
+ * Method: registerFilterIndexCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DIODataJNI_registerFilterIndexCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterDIOFilterIndexCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DIODataJNI
+ * Method: cancelFilterIndexCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DIODataJNI_cancelFilterIndexCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelDIOFilterIndexCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DIODataJNI
+ * Method: getFilterIndex
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DIODataJNI_getFilterIndex
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetDIOFilterIndex(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DIODataJNI
+ * Method: setFilterIndex
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DIODataJNI_setFilterIndex
+ (JNIEnv*, jclass, jint index, jint value)
+{
+ HALSIM_SetDIOFilterIndex(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DIODataJNI
+ * Method: resetData
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DIODataJNI_resetData
+ (JNIEnv*, jclass, jint index)
+{
+ HALSIM_ResetDIOData(index);
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/sim/jni/DigitalPWMDataJNI.cpp b/hal/src/main/native/sim/jni/DigitalPWMDataJNI.cpp
new file mode 100644
index 0000000..0800917
--- /dev/null
+++ b/hal/src/main/native/sim/jni/DigitalPWMDataJNI.cpp
@@ -0,0 +1,178 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 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 <jni.h>
+
+#include "CallbackStore.h"
+#include "edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI.h"
+#include "mockdata/DigitalPWMData.h"
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI
+ * Method: registerInitializedCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI_registerInitializedCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterDigitalPWMInitializedCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI
+ * Method: cancelInitializedCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI_cancelInitializedCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelDigitalPWMInitializedCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI
+ * Method: getInitialized
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI_getInitialized
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetDigitalPWMInitialized(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI
+ * Method: setInitialized
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI_setInitialized
+ (JNIEnv*, jclass, jint index, jboolean value)
+{
+ HALSIM_SetDigitalPWMInitialized(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI
+ * Method: registerDutyCycleCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI_registerDutyCycleCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterDigitalPWMDutyCycleCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI
+ * Method: cancelDutyCycleCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI_cancelDutyCycleCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelDigitalPWMDutyCycleCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI
+ * Method: getDutyCycle
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI_getDutyCycle
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetDigitalPWMDutyCycle(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI
+ * Method: setDutyCycle
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI_setDutyCycle
+ (JNIEnv*, jclass, jint index, jdouble value)
+{
+ HALSIM_SetDigitalPWMDutyCycle(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI
+ * Method: registerPinCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI_registerPinCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterDigitalPWMPinCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI
+ * Method: cancelPinCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI_cancelPinCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelDigitalPWMPinCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI
+ * Method: getPin
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI_getPin
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetDigitalPWMPin(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI
+ * Method: setPin
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI_setPin
+ (JNIEnv*, jclass, jint index, jint value)
+{
+ HALSIM_SetDigitalPWMPin(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI
+ * Method: resetData
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI_resetData
+ (JNIEnv*, jclass, jint index)
+{
+ HALSIM_ResetDigitalPWMData(index);
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/sim/jni/DriverStationDataJNI.cpp b/hal/src/main/native/sim/jni/DriverStationDataJNI.cpp
new file mode 100644
index 0000000..82433b3
--- /dev/null
+++ b/hal/src/main/native/sim/jni/DriverStationDataJNI.cpp
@@ -0,0 +1,481 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 <jni.h>
+
+#include <cstring>
+
+#include <wpi/jni_util.h>
+
+#include "CallbackStore.h"
+#include "edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI.h"
+#include "mockdata/DriverStationData.h"
+#include "mockdata/MockHooks.h"
+
+using namespace wpi::java;
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
+ * Method: registerEnabledCallback
+ * Signature: (Ljava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_registerEnabledCallback
+ (JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallbackNoIndex(
+ env, callback, initialNotify,
+ &HALSIM_RegisterDriverStationEnabledCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
+ * Method: cancelEnabledCallback
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_cancelEnabledCallback
+ (JNIEnv* env, jclass, jint handle)
+{
+ return sim::FreeCallbackNoIndex(env, handle,
+ &HALSIM_CancelDriverStationEnabledCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
+ * Method: getEnabled
+ * Signature: ()Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_getEnabled
+ (JNIEnv*, jclass)
+{
+ return HALSIM_GetDriverStationEnabled();
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
+ * Method: setEnabled
+ * Signature: (Z)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_setEnabled
+ (JNIEnv*, jclass, jboolean value)
+{
+ HALSIM_SetDriverStationEnabled(value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
+ * Method: registerAutonomousCallback
+ * Signature: (Ljava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_registerAutonomousCallback
+ (JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallbackNoIndex(
+ env, callback, initialNotify,
+ &HALSIM_RegisterDriverStationAutonomousCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
+ * Method: cancelAutonomousCallback
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_cancelAutonomousCallback
+ (JNIEnv* env, jclass, jint handle)
+{
+ return sim::FreeCallbackNoIndex(
+ env, handle, &HALSIM_CancelDriverStationAutonomousCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
+ * Method: getAutonomous
+ * Signature: ()Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_getAutonomous
+ (JNIEnv*, jclass)
+{
+ return HALSIM_GetDriverStationAutonomous();
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
+ * Method: setAutonomous
+ * Signature: (Z)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_setAutonomous
+ (JNIEnv*, jclass, jboolean value)
+{
+ HALSIM_SetDriverStationAutonomous(value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
+ * Method: registerTestCallback
+ * Signature: (Ljava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_registerTestCallback
+ (JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallbackNoIndex(
+ env, callback, initialNotify, &HALSIM_RegisterDriverStationTestCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
+ * Method: cancelTestCallback
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_cancelTestCallback
+ (JNIEnv* env, jclass, jint handle)
+{
+ return sim::FreeCallbackNoIndex(env, handle,
+ &HALSIM_CancelDriverStationTestCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
+ * Method: getTest
+ * Signature: ()Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_getTest
+ (JNIEnv*, jclass)
+{
+ return HALSIM_GetDriverStationTest();
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
+ * Method: setTest
+ * Signature: (Z)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_setTest
+ (JNIEnv*, jclass, jboolean value)
+{
+ HALSIM_SetDriverStationTest(value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
+ * Method: registerEStopCallback
+ * Signature: (Ljava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_registerEStopCallback
+ (JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallbackNoIndex(
+ env, callback, initialNotify, &HALSIM_RegisterDriverStationEStopCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
+ * Method: cancelEStopCallback
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_cancelEStopCallback
+ (JNIEnv* env, jclass, jint handle)
+{
+ return sim::FreeCallbackNoIndex(env, handle,
+ &HALSIM_CancelDriverStationEStopCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
+ * Method: getEStop
+ * Signature: ()Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_getEStop
+ (JNIEnv*, jclass)
+{
+ return HALSIM_GetDriverStationEStop();
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
+ * Method: setEStop
+ * Signature: (Z)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_setEStop
+ (JNIEnv*, jclass, jboolean value)
+{
+ HALSIM_SetDriverStationEStop(value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
+ * Method: registerFmsAttachedCallback
+ * Signature: (Ljava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_registerFmsAttachedCallback
+ (JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallbackNoIndex(
+ env, callback, initialNotify,
+ &HALSIM_RegisterDriverStationFmsAttachedCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
+ * Method: cancelFmsAttachedCallback
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_cancelFmsAttachedCallback
+ (JNIEnv* env, jclass, jint handle)
+{
+ return sim::FreeCallbackNoIndex(
+ env, handle, &HALSIM_CancelDriverStationFmsAttachedCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
+ * Method: getFmsAttached
+ * Signature: ()Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_getFmsAttached
+ (JNIEnv*, jclass)
+{
+ return HALSIM_GetDriverStationFmsAttached();
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
+ * Method: setFmsAttached
+ * Signature: (Z)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_setFmsAttached
+ (JNIEnv*, jclass, jboolean value)
+{
+ HALSIM_SetDriverStationFmsAttached(value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
+ * Method: registerDsAttachedCallback
+ * Signature: (Ljava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_registerDsAttachedCallback
+ (JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallbackNoIndex(
+ env, callback, initialNotify,
+ &HALSIM_RegisterDriverStationDsAttachedCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
+ * Method: cancelDsAttachedCallback
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_cancelDsAttachedCallback
+ (JNIEnv* env, jclass, jint handle)
+{
+ return sim::FreeCallbackNoIndex(
+ env, handle, &HALSIM_CancelDriverStationDsAttachedCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
+ * Method: getDsAttached
+ * Signature: ()Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_getDsAttached
+ (JNIEnv*, jclass)
+{
+ return HALSIM_GetDriverStationDsAttached();
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
+ * Method: setDsAttached
+ * Signature: (Z)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_setDsAttached
+ (JNIEnv*, jclass, jboolean value)
+{
+ HALSIM_SetDriverStationDsAttached(value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
+ * Method: setJoystickAxes
+ * Signature: (B[F)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_setJoystickAxes
+ (JNIEnv* env, jclass, jbyte joystickNum, jfloatArray axesArray)
+{
+ HAL_JoystickAxes axes;
+ {
+ wpi::java::JFloatArrayRef jArrayRef(env, axesArray);
+ auto arrayRef = jArrayRef.array();
+ auto arraySize = arrayRef.size();
+ int maxCount =
+ arraySize < HAL_kMaxJoystickAxes ? arraySize : HAL_kMaxJoystickAxes;
+ axes.count = maxCount;
+ for (int i = 0; i < maxCount; i++) {
+ axes.axes[i] = arrayRef[i];
+ }
+ }
+ HALSIM_SetJoystickAxes(joystickNum, &axes);
+ return;
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
+ * Method: setJoystickPOVs
+ * Signature: (B[S)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_setJoystickPOVs
+ (JNIEnv* env, jclass, jbyte joystickNum, jshortArray povsArray)
+{
+ HAL_JoystickPOVs povs;
+ {
+ wpi::java::JShortArrayRef jArrayRef(env, povsArray);
+ auto arrayRef = jArrayRef.array();
+ auto arraySize = arrayRef.size();
+ int maxCount =
+ arraySize < HAL_kMaxJoystickPOVs ? arraySize : HAL_kMaxJoystickPOVs;
+ povs.count = maxCount;
+ for (int i = 0; i < maxCount; i++) {
+ povs.povs[i] = arrayRef[i];
+ }
+ }
+ HALSIM_SetJoystickPOVs(joystickNum, &povs);
+ return;
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
+ * Method: setJoystickButtons
+ * Signature: (BII)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_setJoystickButtons
+ (JNIEnv* env, jclass, jbyte joystickNum, jint buttons, jint count)
+{
+ if (count > 32) {
+ count = 32;
+ }
+ HAL_JoystickButtons joystickButtons;
+ joystickButtons.count = count;
+ joystickButtons.buttons = buttons;
+ HALSIM_SetJoystickButtons(joystickNum, &joystickButtons);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
+ * Method: setMatchInfo
+ * Signature: (Ljava/lang/String;Ljava/lang/String;III)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_setMatchInfo
+ (JNIEnv* env, jclass, jstring eventName, jstring gameSpecificMessage,
+ jint matchNumber, jint replayNumber, jint matchType)
+{
+ JStringRef eventNameRef{env, eventName};
+ JStringRef gameSpecificMessageRef{env, gameSpecificMessage};
+
+ HAL_MatchInfo halMatchInfo;
+ std::snprintf(halMatchInfo.eventName, sizeof(halMatchInfo.eventName), "%s",
+ eventNameRef.c_str());
+ std::snprintf(reinterpret_cast<char*>(halMatchInfo.gameSpecificMessage),
+ sizeof(halMatchInfo.gameSpecificMessage), "%s",
+ gameSpecificMessageRef.c_str());
+ halMatchInfo.gameSpecificMessageSize = gameSpecificMessageRef.size();
+ halMatchInfo.matchType = (HAL_MatchType)matchType;
+ halMatchInfo.matchNumber = matchNumber;
+ halMatchInfo.replayNumber = replayNumber;
+ HALSIM_SetMatchInfo(&halMatchInfo);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
+ * Method: registerAllCallbacks
+ * Signature: (Ljava/lang/Object;Z)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_registerAllCallbacks
+ (JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
+{
+ sim::AllocateCallbackNoIndex(
+ env, callback, initialNotify,
+ [](HAL_NotifyCallback cb, void* param, HAL_Bool in) {
+ HALSIM_RegisterDriverStationAllCallbacks(cb, param, in);
+ return 0;
+ });
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
+ * Method: notifyNewData
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_notifyNewData
+ (JNIEnv*, jclass)
+{
+ HALSIM_NotifyDriverStationNewData();
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
+ * Method: setSendError
+ * Signature: (Z)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_setSendError
+ (JNIEnv*, jclass, jboolean shouldSend)
+{
+ if (shouldSend) {
+ HALSIM_SetSendError(nullptr);
+ } else {
+ HALSIM_SetSendError([](HAL_Bool isError, int32_t errorCode,
+ HAL_Bool isLVCode, const char* details,
+ const char* location, const char* callStack,
+ HAL_Bool printMsg) { return 1; });
+ }
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
+ * Method: resetData
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_resetData
+ (JNIEnv*, jclass)
+{
+ HALSIM_ResetDriverStationData();
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/sim/jni/DutyCycleDataJNI.cpp b/hal/src/main/native/sim/jni/DutyCycleDataJNI.cpp
new file mode 100644
index 0000000..d746ce1
--- /dev/null
+++ b/hal/src/main/native/sim/jni/DutyCycleDataJNI.cpp
@@ -0,0 +1,178 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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 <jni.h>
+
+#include "CallbackStore.h"
+#include "edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI.h"
+#include "mockdata/DutyCycleData.h"
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI
+ * Method: registerInitializedCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI_registerInitializedCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterDutyCycleInitializedCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI
+ * Method: cancelInitializedCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI_cancelInitializedCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelDutyCycleInitializedCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI
+ * Method: getInitialized
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI_getInitialized
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetDutyCycleInitialized(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI
+ * Method: setInitialized
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI_setInitialized
+ (JNIEnv*, jclass, jint index, jboolean value)
+{
+ HALSIM_SetDutyCycleInitialized(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI
+ * Method: registerFrequencyCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI_registerFrequencyCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterDutyCycleFrequencyCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI
+ * Method: cancelFrequencyCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI_cancelFrequencyCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelDutyCycleFrequencyCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI
+ * Method: getFrequency
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI_getFrequency
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetDutyCycleFrequency(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI
+ * Method: setFrequency
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI_setFrequency
+ (JNIEnv*, jclass, jint index, jint value)
+{
+ HALSIM_SetDutyCycleFrequency(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI
+ * Method: registerOutputCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI_registerOutputCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterDutyCycleOutputCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI
+ * Method: cancelOutputCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI_cancelOutputCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelDutyCycleOutputCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI
+ * Method: getOutput
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI_getOutput
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetDutyCycleOutput(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI
+ * Method: setOutput
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI_setOutput
+ (JNIEnv*, jclass, jint index, jdouble value)
+{
+ HALSIM_SetDutyCycleOutput(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI
+ * Method: resetData
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI_resetData
+ (JNIEnv*, jclass, jint index)
+{
+ HALSIM_ResetDutyCycleData(index);
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/sim/jni/EncoderDataJNI.cpp b/hal/src/main/native/sim/jni/EncoderDataJNI.cpp
new file mode 100644
index 0000000..8992b0c
--- /dev/null
+++ b/hal/src/main/native/sim/jni/EncoderDataJNI.cpp
@@ -0,0 +1,428 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 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 <jni.h>
+
+#include "CallbackStore.h"
+#include "edu_wpi_first_hal_sim_mockdata_EncoderDataJNI.h"
+#include "mockdata/EncoderData.h"
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
+ * Method: registerInitializedCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_registerInitializedCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterEncoderInitializedCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
+ * Method: cancelInitializedCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_cancelInitializedCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelEncoderInitializedCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
+ * Method: getInitialized
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_getInitialized
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetEncoderInitialized(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
+ * Method: setInitialized
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_setInitialized
+ (JNIEnv*, jclass, jint index, jboolean value)
+{
+ HALSIM_SetEncoderInitialized(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
+ * Method: registerCountCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_registerCountCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterEncoderCountCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
+ * Method: cancelCountCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_cancelCountCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelEncoderCountCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
+ * Method: getCount
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_getCount
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetEncoderCount(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
+ * Method: setCount
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_setCount
+ (JNIEnv*, jclass, jint index, jint value)
+{
+ HALSIM_SetEncoderCount(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
+ * Method: registerPeriodCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_registerPeriodCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterEncoderPeriodCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
+ * Method: cancelPeriodCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_cancelPeriodCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelEncoderPeriodCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
+ * Method: getPeriod
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_getPeriod
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetEncoderPeriod(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
+ * Method: setPeriod
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_setPeriod
+ (JNIEnv*, jclass, jint index, jdouble value)
+{
+ HALSIM_SetEncoderPeriod(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
+ * Method: registerResetCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_registerResetCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterEncoderResetCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
+ * Method: cancelResetCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_cancelResetCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelEncoderResetCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
+ * Method: getReset
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_getReset
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetEncoderReset(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
+ * Method: setReset
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_setReset
+ (JNIEnv*, jclass, jint index, jboolean value)
+{
+ HALSIM_SetEncoderReset(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
+ * Method: registerMaxPeriodCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_registerMaxPeriodCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterEncoderMaxPeriodCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
+ * Method: cancelMaxPeriodCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_cancelMaxPeriodCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelEncoderMaxPeriodCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
+ * Method: getMaxPeriod
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_getMaxPeriod
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetEncoderMaxPeriod(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
+ * Method: setMaxPeriod
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_setMaxPeriod
+ (JNIEnv*, jclass, jint index, jdouble value)
+{
+ HALSIM_SetEncoderMaxPeriod(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
+ * Method: registerDirectionCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_registerDirectionCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterEncoderDirectionCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
+ * Method: cancelDirectionCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_cancelDirectionCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelEncoderDirectionCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
+ * Method: getDirection
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_getDirection
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetEncoderDirection(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
+ * Method: setDirection
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_setDirection
+ (JNIEnv*, jclass, jint index, jboolean value)
+{
+ HALSIM_SetEncoderDirection(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
+ * Method: registerReverseDirectionCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_registerReverseDirectionCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterEncoderReverseDirectionCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
+ * Method: cancelReverseDirectionCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_cancelReverseDirectionCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelEncoderReverseDirectionCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
+ * Method: getReverseDirection
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_getReverseDirection
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetEncoderReverseDirection(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
+ * Method: setReverseDirection
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_setReverseDirection
+ (JNIEnv*, jclass, jint index, jboolean value)
+{
+ HALSIM_SetEncoderReverseDirection(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
+ * Method: registerSamplesToAverageCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_registerSamplesToAverageCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterEncoderSamplesToAverageCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
+ * Method: cancelSamplesToAverageCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_cancelSamplesToAverageCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelEncoderSamplesToAverageCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
+ * Method: getSamplesToAverage
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_getSamplesToAverage
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetEncoderSamplesToAverage(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
+ * Method: setSamplesToAverage
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_setSamplesToAverage
+ (JNIEnv*, jclass, jint index, jint value)
+{
+ HALSIM_SetEncoderSamplesToAverage(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
+ * Method: resetData
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_resetData
+ (JNIEnv*, jclass, jint index)
+{
+ HALSIM_ResetEncoderData(index);
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/sim/jni/I2CDataJNI.cpp b/hal/src/main/native/sim/jni/I2CDataJNI.cpp
new file mode 100644
index 0000000..14b3292
--- /dev/null
+++ b/hal/src/main/native/sim/jni/I2CDataJNI.cpp
@@ -0,0 +1,131 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 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 <jni.h>
+
+#include "BufferCallbackStore.h"
+#include "CallbackStore.h"
+#include "ConstBufferCallbackStore.h"
+#include "edu_wpi_first_hal_sim_mockdata_I2CDataJNI.h"
+#include "mockdata/I2CData.h"
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_I2CDataJNI
+ * Method: registerInitializedCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_I2CDataJNI_registerInitializedCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterI2CInitializedCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_I2CDataJNI
+ * Method: cancelInitializedCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_I2CDataJNI_cancelInitializedCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelI2CInitializedCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_I2CDataJNI
+ * Method: getInitialized
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_I2CDataJNI_getInitialized
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetI2CInitialized(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_I2CDataJNI
+ * Method: setInitialized
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_I2CDataJNI_setInitialized
+ (JNIEnv*, jclass, jint index, jboolean value)
+{
+ HALSIM_SetI2CInitialized(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_I2CDataJNI
+ * Method: registerReadCallback
+ * Signature: (ILjava/lang/Object;)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_I2CDataJNI_registerReadCallback
+ (JNIEnv* env, jclass, jint index, jobject callback)
+{
+ return sim::AllocateBufferCallback(env, index, callback,
+ &HALSIM_RegisterI2CReadCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_I2CDataJNI
+ * Method: cancelReadCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_I2CDataJNI_cancelReadCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ sim::FreeBufferCallback(env, handle, index, &HALSIM_CancelI2CReadCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_I2CDataJNI
+ * Method: registerWriteCallback
+ * Signature: (ILjava/lang/Object;)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_I2CDataJNI_registerWriteCallback
+ (JNIEnv* env, jclass, jint index, jobject callback)
+{
+ return sim::AllocateConstBufferCallback(env, index, callback,
+ &HALSIM_RegisterI2CWriteCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_I2CDataJNI
+ * Method: cancelWriteCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_I2CDataJNI_cancelWriteCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ sim::FreeConstBufferCallback(env, handle, index,
+ &HALSIM_CancelI2CWriteCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_I2CDataJNI
+ * Method: resetData
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_I2CDataJNI_resetData
+ (JNIEnv*, jclass, jint index)
+{
+ HALSIM_ResetI2CData(index);
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/sim/jni/NotifierDataJNI.cpp b/hal/src/main/native/sim/jni/NotifierDataJNI.cpp
new file mode 100644
index 0000000..b59b6e0
--- /dev/null
+++ b/hal/src/main/native/sim/jni/NotifierDataJNI.cpp
@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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 "edu_wpi_first_hal_sim_mockdata_NotifierDataJNI.h"
+#include "mockdata/NotifierData.h"
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_NotifierDataJNI
+ * Method: getNextTimeout
+ * Signature: ()J
+ */
+JNIEXPORT jlong JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_NotifierDataJNI_getNextTimeout
+ (JNIEnv*, jclass)
+{
+ return HALSIM_GetNextNotifierTimeout();
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_NotifierDataJNI
+ * Method: getNumNotifiers
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_NotifierDataJNI_getNumNotifiers
+ (JNIEnv*, jclass)
+{
+ return HALSIM_GetNumNotifiers();
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/sim/jni/PCMDataJNI.cpp b/hal/src/main/native/sim/jni/PCMDataJNI.cpp
new file mode 100644
index 0000000..de6f738
--- /dev/null
+++ b/hal/src/main/native/sim/jni/PCMDataJNI.cpp
@@ -0,0 +1,419 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 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 <jni.h>
+
+#include "CallbackStore.h"
+#include "edu_wpi_first_hal_sim_mockdata_PCMDataJNI.h"
+#include "mockdata/PCMData.h"
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PCMDataJNI
+ * Method: registerSolenoidInitializedCallback
+ * Signature: (IILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_registerSolenoidInitializedCallback
+ (JNIEnv* env, jclass, jint index, jint channel, jobject callback,
+ jboolean initialNotify)
+{
+ return sim::AllocateChannelCallback(
+ env, index, channel, callback, initialNotify,
+ &HALSIM_RegisterPCMSolenoidInitializedCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PCMDataJNI
+ * Method: cancelSolenoidInitializedCallback
+ * Signature: (III)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_cancelSolenoidInitializedCallback
+ (JNIEnv* env, jclass, jint index, jint channel, jint handle)
+{
+ return sim::FreeChannelCallback(env, handle, index, channel,
+ &HALSIM_CancelPCMSolenoidInitializedCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PCMDataJNI
+ * Method: getSolenoidInitialized
+ * Signature: (II)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_getSolenoidInitialized
+ (JNIEnv*, jclass, jint index, jint channel)
+{
+ return HALSIM_GetPCMSolenoidInitialized(index, channel);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PCMDataJNI
+ * Method: setSolenoidInitialized
+ * Signature: (IIZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_setSolenoidInitialized
+ (JNIEnv*, jclass, jint index, jint channel, jboolean value)
+{
+ HALSIM_SetPCMSolenoidInitialized(index, channel, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PCMDataJNI
+ * Method: registerSolenoidOutputCallback
+ * Signature: (IILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_registerSolenoidOutputCallback
+ (JNIEnv* env, jclass, jint index, jint channel, jobject callback,
+ jboolean initialNotify)
+{
+ return sim::AllocateChannelCallback(
+ env, index, channel, callback, initialNotify,
+ &HALSIM_RegisterPCMSolenoidOutputCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PCMDataJNI
+ * Method: cancelSolenoidOutputCallback
+ * Signature: (III)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_cancelSolenoidOutputCallback
+ (JNIEnv* env, jclass, jint index, jint channel, jint handle)
+{
+ return sim::FreeChannelCallback(env, handle, index, channel,
+ &HALSIM_CancelPCMSolenoidOutputCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PCMDataJNI
+ * Method: getSolenoidOutput
+ * Signature: (II)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_getSolenoidOutput
+ (JNIEnv*, jclass, jint index, jint channel)
+{
+ return HALSIM_GetPCMSolenoidOutput(index, channel);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PCMDataJNI
+ * Method: setSolenoidOutput
+ * Signature: (IIZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_setSolenoidOutput
+ (JNIEnv*, jclass, jint index, jint channel, jboolean value)
+{
+ HALSIM_SetPCMSolenoidOutput(index, channel, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PCMDataJNI
+ * Method: registerCompressorInitializedCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_registerCompressorInitializedCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(
+ env, index, callback, initialNotify,
+ &HALSIM_RegisterPCMCompressorInitializedCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PCMDataJNI
+ * Method: cancelCompressorInitializedCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_cancelCompressorInitializedCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelPCMCompressorInitializedCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PCMDataJNI
+ * Method: getCompressorInitialized
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_getCompressorInitialized
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetPCMCompressorInitialized(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PCMDataJNI
+ * Method: setCompressorInitialized
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_setCompressorInitialized
+ (JNIEnv*, jclass, jint index, jboolean value)
+{
+ HALSIM_SetPCMCompressorInitialized(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PCMDataJNI
+ * Method: registerCompressorOnCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_registerCompressorOnCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterPCMCompressorOnCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PCMDataJNI
+ * Method: cancelCompressorOnCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_cancelCompressorOnCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelPCMCompressorOnCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PCMDataJNI
+ * Method: getCompressorOn
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_getCompressorOn
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetPCMCompressorOn(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PCMDataJNI
+ * Method: setCompressorOn
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_setCompressorOn
+ (JNIEnv*, jclass, jint index, jboolean value)
+{
+ HALSIM_SetPCMCompressorOn(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PCMDataJNI
+ * Method: registerClosedLoopEnabledCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_registerClosedLoopEnabledCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterPCMClosedLoopEnabledCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PCMDataJNI
+ * Method: cancelClosedLoopEnabledCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_cancelClosedLoopEnabledCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelPCMClosedLoopEnabledCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PCMDataJNI
+ * Method: getClosedLoopEnabled
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_getClosedLoopEnabled
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetPCMClosedLoopEnabled(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PCMDataJNI
+ * Method: setClosedLoopEnabled
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_setClosedLoopEnabled
+ (JNIEnv*, jclass, jint index, jboolean value)
+{
+ HALSIM_SetPCMClosedLoopEnabled(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PCMDataJNI
+ * Method: registerPressureSwitchCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_registerPressureSwitchCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterPCMPressureSwitchCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PCMDataJNI
+ * Method: cancelPressureSwitchCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_cancelPressureSwitchCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelPCMPressureSwitchCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PCMDataJNI
+ * Method: getPressureSwitch
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_getPressureSwitch
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetPCMPressureSwitch(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PCMDataJNI
+ * Method: setPressureSwitch
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_setPressureSwitch
+ (JNIEnv*, jclass, jint index, jboolean value)
+{
+ HALSIM_SetPCMPressureSwitch(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PCMDataJNI
+ * Method: registerCompressorCurrentCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_registerCompressorCurrentCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterPCMCompressorCurrentCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PCMDataJNI
+ * Method: cancelCompressorCurrentCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_cancelCompressorCurrentCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelPCMCompressorCurrentCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PCMDataJNI
+ * Method: getCompressorCurrent
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_getCompressorCurrent
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetPCMCompressorCurrent(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PCMDataJNI
+ * Method: setCompressorCurrent
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_setCompressorCurrent
+ (JNIEnv*, jclass, jint index, jdouble value)
+{
+ HALSIM_SetPCMCompressorCurrent(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PCMDataJNI
+ * Method: registerAllNonSolenoidCallbacks
+ * Signature: (ILjava/lang/Object;Z)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_registerAllNonSolenoidCallbacks
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ sim::AllocateCallback(
+ env, index, callback, initialNotify,
+ [](int32_t index, HAL_NotifyCallback cb, void* param, HAL_Bool in) {
+ HALSIM_RegisterPCMAllNonSolenoidCallbacks(index, cb, param, in);
+ return 0;
+ });
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PCMDataJNI
+ * Method: registerAllSolenoidCallbacks
+ * Signature: (IILjava/lang/Object;Z)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_registerAllSolenoidCallbacks
+ (JNIEnv* env, jclass, jint index, jint channel, jobject callback,
+ jboolean initialNotify)
+{
+ sim::AllocateChannelCallback(
+ env, index, channel, callback, initialNotify,
+ [](int32_t index, int32_t channel, HAL_NotifyCallback cb, void* param,
+ HAL_Bool in) {
+ HALSIM_RegisterPCMAllSolenoidCallbacks(index, channel, cb, param, in);
+ return 0;
+ });
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PCMDataJNI
+ * Method: resetData
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_resetData
+ (JNIEnv*, jclass, jint index)
+{
+ HALSIM_ResetPCMData(index);
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/sim/jni/PDPDataJNI.cpp b/hal/src/main/native/sim/jni/PDPDataJNI.cpp
new file mode 100644
index 0000000..5d39e87
--- /dev/null
+++ b/hal/src/main/native/sim/jni/PDPDataJNI.cpp
@@ -0,0 +1,230 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 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 <jni.h>
+
+#include "CallbackStore.h"
+#include "edu_wpi_first_hal_sim_mockdata_PDPDataJNI.h"
+#include "mockdata/PDPData.h"
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PDPDataJNI
+ * Method: registerInitializedCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PDPDataJNI_registerInitializedCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterPDPInitializedCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PDPDataJNI
+ * Method: cancelInitializedCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PDPDataJNI_cancelInitializedCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelPDPInitializedCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PDPDataJNI
+ * Method: getInitialized
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PDPDataJNI_getInitialized
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetPDPInitialized(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PDPDataJNI
+ * Method: setInitialized
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PDPDataJNI_setInitialized
+ (JNIEnv*, jclass, jint index, jboolean value)
+{
+ HALSIM_SetPDPInitialized(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PDPDataJNI
+ * Method: registerTemperatureCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PDPDataJNI_registerTemperatureCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterPDPTemperatureCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PDPDataJNI
+ * Method: cancelTemperatureCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PDPDataJNI_cancelTemperatureCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelPDPTemperatureCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PDPDataJNI
+ * Method: getTemperature
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PDPDataJNI_getTemperature
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetPDPTemperature(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PDPDataJNI
+ * Method: setTemperature
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PDPDataJNI_setTemperature
+ (JNIEnv*, jclass, jint index, jdouble value)
+{
+ HALSIM_SetPDPTemperature(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PDPDataJNI
+ * Method: registerVoltageCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PDPDataJNI_registerVoltageCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterPDPVoltageCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PDPDataJNI
+ * Method: cancelVoltageCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PDPDataJNI_cancelVoltageCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelPDPVoltageCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PDPDataJNI
+ * Method: getVoltage
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PDPDataJNI_getVoltage
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetPDPVoltage(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PDPDataJNI
+ * Method: setVoltage
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PDPDataJNI_setVoltage
+ (JNIEnv*, jclass, jint index, jdouble value)
+{
+ HALSIM_SetPDPVoltage(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PDPDataJNI
+ * Method: registerCurrentCallback
+ * Signature: (IILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PDPDataJNI_registerCurrentCallback
+ (JNIEnv* env, jclass, jint index, jint channel, jobject callback,
+ jboolean initialNotify)
+{
+ return sim::AllocateChannelCallback(env, index, channel, callback,
+ initialNotify,
+ &HALSIM_RegisterPDPCurrentCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PDPDataJNI
+ * Method: cancelCurrentCallback
+ * Signature: (III)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PDPDataJNI_cancelCurrentCallback
+ (JNIEnv* env, jclass, jint index, jint channel, jint handle)
+{
+ return sim::FreeChannelCallback(env, handle, index, channel,
+ &HALSIM_CancelPDPCurrentCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PDPDataJNI
+ * Method: getCurrent
+ * Signature: (II)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PDPDataJNI_getCurrent
+ (JNIEnv*, jclass, jint index, jint channel)
+{
+ return HALSIM_GetPDPCurrent(index, channel);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PDPDataJNI
+ * Method: setCurrent
+ * Signature: (IID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PDPDataJNI_setCurrent
+ (JNIEnv*, jclass, jint index, jint channel, jdouble value)
+{
+ HALSIM_SetPDPCurrent(index, channel, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PDPDataJNI
+ * Method: resetData
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PDPDataJNI_resetData
+ (JNIEnv*, jclass, jint index)
+{
+ HALSIM_ResetPDPData(index);
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/sim/jni/PWMDataJNI.cpp b/hal/src/main/native/sim/jni/PWMDataJNI.cpp
new file mode 100644
index 0000000..b8a7c41
--- /dev/null
+++ b/hal/src/main/native/sim/jni/PWMDataJNI.cpp
@@ -0,0 +1,327 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 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 <jni.h>
+
+#include "CallbackStore.h"
+#include "edu_wpi_first_hal_sim_mockdata_PWMDataJNI.h"
+#include "mockdata/PWMData.h"
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PWMDataJNI
+ * Method: registerInitializedCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_registerInitializedCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterPWMInitializedCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PWMDataJNI
+ * Method: cancelInitializedCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_cancelInitializedCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelPWMInitializedCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PWMDataJNI
+ * Method: getInitialized
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_getInitialized
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetPWMInitialized(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PWMDataJNI
+ * Method: setInitialized
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_setInitialized
+ (JNIEnv*, jclass, jint index, jboolean value)
+{
+ HALSIM_SetPWMInitialized(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PWMDataJNI
+ * Method: registerRawValueCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_registerRawValueCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterPWMRawValueCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PWMDataJNI
+ * Method: cancelRawValueCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_cancelRawValueCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelPWMRawValueCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PWMDataJNI
+ * Method: getRawValue
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_getRawValue
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetPWMRawValue(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PWMDataJNI
+ * Method: setRawValue
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_setRawValue
+ (JNIEnv*, jclass, jint index, jint value)
+{
+ HALSIM_SetPWMRawValue(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PWMDataJNI
+ * Method: registerSpeedCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_registerSpeedCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterPWMSpeedCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PWMDataJNI
+ * Method: cancelSpeedCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_cancelSpeedCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index, &HALSIM_CancelPWMSpeedCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PWMDataJNI
+ * Method: getSpeed
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_getSpeed
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetPWMSpeed(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PWMDataJNI
+ * Method: setSpeed
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_setSpeed
+ (JNIEnv*, jclass, jint index, jdouble value)
+{
+ HALSIM_SetPWMSpeed(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PWMDataJNI
+ * Method: registerPositionCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_registerPositionCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterPWMPositionCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PWMDataJNI
+ * Method: cancelPositionCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_cancelPositionCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelPWMPositionCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PWMDataJNI
+ * Method: getPosition
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_getPosition
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetPWMPosition(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PWMDataJNI
+ * Method: setPosition
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_setPosition
+ (JNIEnv*, jclass, jint index, jdouble value)
+{
+ HALSIM_SetPWMPosition(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PWMDataJNI
+ * Method: registerPeriodScaleCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_registerPeriodScaleCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterPWMPeriodScaleCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PWMDataJNI
+ * Method: cancelPeriodScaleCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_cancelPeriodScaleCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelPWMPeriodScaleCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PWMDataJNI
+ * Method: getPeriodScale
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_getPeriodScale
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetPWMPeriodScale(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PWMDataJNI
+ * Method: setPeriodScale
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_setPeriodScale
+ (JNIEnv*, jclass, jint index, jint value)
+{
+ HALSIM_SetPWMPeriodScale(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PWMDataJNI
+ * Method: registerZeroLatchCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_registerZeroLatchCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterPWMZeroLatchCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PWMDataJNI
+ * Method: cancelZeroLatchCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_cancelZeroLatchCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelPWMZeroLatchCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PWMDataJNI
+ * Method: getZeroLatch
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_getZeroLatch
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetPWMZeroLatch(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PWMDataJNI
+ * Method: setZeroLatch
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_setZeroLatch
+ (JNIEnv*, jclass, jint index, jboolean value)
+{
+ HALSIM_SetPWMZeroLatch(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_PWMDataJNI
+ * Method: resetData
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_resetData
+ (JNIEnv*, jclass, jint index)
+{
+ HALSIM_ResetPWMData(index);
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/sim/jni/RelayDataJNI.cpp b/hal/src/main/native/sim/jni/RelayDataJNI.cpp
new file mode 100644
index 0000000..bf85407
--- /dev/null
+++ b/hal/src/main/native/sim/jni/RelayDataJNI.cpp
@@ -0,0 +1,228 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 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 <jni.h>
+
+#include "CallbackStore.h"
+#include "edu_wpi_first_hal_sim_mockdata_RelayDataJNI.h"
+#include "mockdata/RelayData.h"
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RelayDataJNI
+ * Method: registerInitializedForwardCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RelayDataJNI_registerInitializedForwardCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterRelayInitializedForwardCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RelayDataJNI
+ * Method: cancelInitializedForwardCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RelayDataJNI_cancelInitializedForwardCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelRelayInitializedForwardCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RelayDataJNI
+ * Method: getInitializedForward
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RelayDataJNI_getInitializedForward
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetRelayInitializedForward(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RelayDataJNI
+ * Method: setInitializedForward
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RelayDataJNI_setInitializedForward
+ (JNIEnv*, jclass, jint index, jboolean value)
+{
+ HALSIM_SetRelayInitializedForward(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RelayDataJNI
+ * Method: registerInitializedReverseCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RelayDataJNI_registerInitializedReverseCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterRelayInitializedReverseCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RelayDataJNI
+ * Method: cancelInitializedReverseCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RelayDataJNI_cancelInitializedReverseCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelRelayInitializedReverseCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RelayDataJNI
+ * Method: getInitializedReverse
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RelayDataJNI_getInitializedReverse
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetRelayInitializedReverse(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RelayDataJNI
+ * Method: setInitializedReverse
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RelayDataJNI_setInitializedReverse
+ (JNIEnv*, jclass, jint index, jboolean value)
+{
+ HALSIM_SetRelayInitializedReverse(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RelayDataJNI
+ * Method: registerForwardCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RelayDataJNI_registerForwardCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterRelayForwardCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RelayDataJNI
+ * Method: cancelForwardCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RelayDataJNI_cancelForwardCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelRelayForwardCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RelayDataJNI
+ * Method: getForward
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RelayDataJNI_getForward
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetRelayForward(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RelayDataJNI
+ * Method: setForward
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RelayDataJNI_setForward
+ (JNIEnv*, jclass, jint index, jboolean value)
+{
+ HALSIM_SetRelayForward(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RelayDataJNI
+ * Method: registerReverseCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RelayDataJNI_registerReverseCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterRelayReverseCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RelayDataJNI
+ * Method: cancelReverseCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RelayDataJNI_cancelReverseCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelRelayReverseCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RelayDataJNI
+ * Method: getReverse
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RelayDataJNI_getReverse
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetRelayReverse(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RelayDataJNI
+ * Method: setReverse
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RelayDataJNI_setReverse
+ (JNIEnv*, jclass, jint index, jboolean value)
+{
+ HALSIM_SetRelayReverse(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RelayDataJNI
+ * Method: resetData
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RelayDataJNI_resetData
+ (JNIEnv*, jclass, jint index)
+{
+ HALSIM_ResetRelayData(index);
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/sim/jni/RoboRioDataJNI.cpp b/hal/src/main/native/sim/jni/RoboRioDataJNI.cpp
new file mode 100644
index 0000000..d69e895
--- /dev/null
+++ b/hal/src/main/native/sim/jni/RoboRioDataJNI.cpp
@@ -0,0 +1,778 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 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 <jni.h>
+
+#include "CallbackStore.h"
+#include "edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI.h"
+#include "mockdata/RoboRioData.h"
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: registerFPGAButtonCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_registerFPGAButtonCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterRoboRioFPGAButtonCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: cancelFPGAButtonCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_cancelFPGAButtonCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelRoboRioFPGAButtonCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: getFPGAButton
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_getFPGAButton
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetRoboRioFPGAButton(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: setFPGAButton
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_setFPGAButton
+ (JNIEnv*, jclass, jint index, jboolean value)
+{
+ HALSIM_SetRoboRioFPGAButton(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: registerVInVoltageCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_registerVInVoltageCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterRoboRioVInVoltageCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: cancelVInVoltageCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_cancelVInVoltageCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelRoboRioVInVoltageCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: getVInVoltage
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_getVInVoltage
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetRoboRioVInVoltage(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: setVInVoltage
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_setVInVoltage
+ (JNIEnv*, jclass, jint index, jdouble value)
+{
+ HALSIM_SetRoboRioVInVoltage(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: registerVInCurrentCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_registerVInCurrentCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterRoboRioVInCurrentCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: cancelVInCurrentCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_cancelVInCurrentCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelRoboRioVInCurrentCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: getVInCurrent
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_getVInCurrent
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetRoboRioVInCurrent(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: setVInCurrent
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_setVInCurrent
+ (JNIEnv*, jclass, jint index, jdouble value)
+{
+ HALSIM_SetRoboRioVInCurrent(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: registerUserVoltage6VCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_registerUserVoltage6VCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterRoboRioUserVoltage6VCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: cancelUserVoltage6VCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_cancelUserVoltage6VCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelRoboRioUserVoltage6VCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: getUserVoltage6V
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_getUserVoltage6V
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetRoboRioUserVoltage6V(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: setUserVoltage6V
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_setUserVoltage6V
+ (JNIEnv*, jclass, jint index, jdouble value)
+{
+ HALSIM_SetRoboRioUserVoltage6V(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: registerUserCurrent6VCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_registerUserCurrent6VCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterRoboRioUserCurrent6VCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: cancelUserCurrent6VCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_cancelUserCurrent6VCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelRoboRioUserCurrent6VCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: getUserCurrent6V
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_getUserCurrent6V
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetRoboRioUserCurrent6V(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: setUserCurrent6V
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_setUserCurrent6V
+ (JNIEnv*, jclass, jint index, jdouble value)
+{
+ HALSIM_SetRoboRioUserCurrent6V(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: registerUserActive6VCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_registerUserActive6VCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterRoboRioUserActive6VCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: cancelUserActive6VCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_cancelUserActive6VCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelRoboRioUserActive6VCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: getUserActive6V
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_getUserActive6V
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetRoboRioUserActive6V(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: setUserActive6V
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_setUserActive6V
+ (JNIEnv*, jclass, jint index, jboolean value)
+{
+ HALSIM_SetRoboRioUserActive6V(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: registerUserVoltage5VCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_registerUserVoltage5VCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterRoboRioUserVoltage5VCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: cancelUserVoltage5VCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_cancelUserVoltage5VCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelRoboRioUserVoltage5VCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: getUserVoltage5V
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_getUserVoltage5V
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetRoboRioUserVoltage5V(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: setUserVoltage5V
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_setUserVoltage5V
+ (JNIEnv*, jclass, jint index, jdouble value)
+{
+ HALSIM_SetRoboRioUserVoltage5V(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: registerUserCurrent5VCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_registerUserCurrent5VCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterRoboRioUserCurrent5VCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: cancelUserCurrent5VCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_cancelUserCurrent5VCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelRoboRioUserCurrent5VCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: getUserCurrent5V
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_getUserCurrent5V
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetRoboRioUserCurrent5V(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: setUserCurrent5V
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_setUserCurrent5V
+ (JNIEnv*, jclass, jint index, jdouble value)
+{
+ HALSIM_SetRoboRioUserCurrent5V(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: registerUserActive5VCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_registerUserActive5VCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterRoboRioUserActive5VCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: cancelUserActive5VCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_cancelUserActive5VCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelRoboRioUserActive5VCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: getUserActive5V
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_getUserActive5V
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetRoboRioUserActive5V(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: setUserActive5V
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_setUserActive5V
+ (JNIEnv*, jclass, jint index, jboolean value)
+{
+ HALSIM_SetRoboRioUserActive5V(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: registerUserVoltage3V3Callback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_registerUserVoltage3V3Callback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterRoboRioUserVoltage3V3Callback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: cancelUserVoltage3V3Callback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_cancelUserVoltage3V3Callback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelRoboRioUserVoltage3V3Callback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: getUserVoltage3V3
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_getUserVoltage3V3
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetRoboRioUserVoltage3V3(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: setUserVoltage3V3
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_setUserVoltage3V3
+ (JNIEnv*, jclass, jint index, jdouble value)
+{
+ HALSIM_SetRoboRioUserVoltage3V3(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: registerUserCurrent3V3Callback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_registerUserCurrent3V3Callback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterRoboRioUserCurrent3V3Callback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: cancelUserCurrent3V3Callback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_cancelUserCurrent3V3Callback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelRoboRioUserCurrent3V3Callback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: getUserCurrent3V3
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_getUserCurrent3V3
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetRoboRioUserCurrent3V3(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: setUserCurrent3V3
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_setUserCurrent3V3
+ (JNIEnv*, jclass, jint index, jdouble value)
+{
+ HALSIM_SetRoboRioUserCurrent3V3(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: registerUserActive3V3Callback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_registerUserActive3V3Callback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterRoboRioUserActive3V3Callback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: cancelUserActive3V3Callback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_cancelUserActive3V3Callback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelRoboRioUserActive3V3Callback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: getUserActive3V3
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_getUserActive3V3
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetRoboRioUserActive3V3(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: setUserActive3V3
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_setUserActive3V3
+ (JNIEnv*, jclass, jint index, jboolean value)
+{
+ HALSIM_SetRoboRioUserActive3V3(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: registerUserFaults6VCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_registerUserFaults6VCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterRoboRioUserFaults6VCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: cancelUserFaults6VCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_cancelUserFaults6VCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelRoboRioUserFaults6VCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: getUserFaults6V
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_getUserFaults6V
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetRoboRioUserFaults6V(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: setUserFaults6V
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_setUserFaults6V
+ (JNIEnv*, jclass, jint index, jint value)
+{
+ HALSIM_SetRoboRioUserFaults6V(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: registerUserFaults5VCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_registerUserFaults5VCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterRoboRioUserFaults5VCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: cancelUserFaults5VCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_cancelUserFaults5VCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelRoboRioUserFaults5VCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: getUserFaults5V
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_getUserFaults5V
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetRoboRioUserFaults5V(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: setUserFaults5V
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_setUserFaults5V
+ (JNIEnv*, jclass, jint index, jint value)
+{
+ HALSIM_SetRoboRioUserFaults5V(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: registerUserFaults3V3Callback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_registerUserFaults3V3Callback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterRoboRioUserFaults3V3Callback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: cancelUserFaults3V3Callback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_cancelUserFaults3V3Callback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelRoboRioUserFaults3V3Callback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: getUserFaults3V3
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_getUserFaults3V3
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetRoboRioUserFaults3V3(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: setUserFaults3V3
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_setUserFaults3V3
+ (JNIEnv*, jclass, jint index, jint value)
+{
+ HALSIM_SetRoboRioUserFaults3V3(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
+ * Method: resetData
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_resetData
+ (JNIEnv*, jclass, jint index)
+{
+ HALSIM_ResetRoboRioData(index);
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/sim/jni/SPIAccelerometerDataJNI.cpp b/hal/src/main/native/sim/jni/SPIAccelerometerDataJNI.cpp
new file mode 100644
index 0000000..ca12f79
--- /dev/null
+++ b/hal/src/main/native/sim/jni/SPIAccelerometerDataJNI.cpp
@@ -0,0 +1,278 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 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 <jni.h>
+
+#include "CallbackStore.h"
+#include "edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI.h"
+#include "mockdata/SPIAccelerometerData.h"
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI
+ * Method: registerActiveCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI_registerActiveCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterSPIAccelerometerActiveCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI
+ * Method: cancelActiveCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI_cancelActiveCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelSPIAccelerometerActiveCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI
+ * Method: getActive
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI_getActive
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetSPIAccelerometerActive(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI
+ * Method: setActive
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI_setActive
+ (JNIEnv*, jclass, jint index, jboolean value)
+{
+ HALSIM_SetSPIAccelerometerActive(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI
+ * Method: registerRangeCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI_registerRangeCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterSPIAccelerometerRangeCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI
+ * Method: cancelRangeCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI_cancelRangeCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelSPIAccelerometerRangeCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI
+ * Method: getRange
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI_getRange
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetSPIAccelerometerRange(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI
+ * Method: setRange
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI_setRange
+ (JNIEnv*, jclass, jint index, jint value)
+{
+ HALSIM_SetSPIAccelerometerRange(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI
+ * Method: registerXCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI_registerXCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterSPIAccelerometerXCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI
+ * Method: cancelXCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI_cancelXCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelSPIAccelerometerXCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI
+ * Method: getX
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI_getX
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetSPIAccelerometerX(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI
+ * Method: setX
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI_setX
+ (JNIEnv*, jclass, jint index, jdouble value)
+{
+ HALSIM_SetSPIAccelerometerX(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI
+ * Method: registerYCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI_registerYCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterSPIAccelerometerYCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI
+ * Method: cancelYCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI_cancelYCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelSPIAccelerometerYCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI
+ * Method: getY
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI_getY
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetSPIAccelerometerY(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI
+ * Method: setY
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI_setY
+ (JNIEnv*, jclass, jint index, jdouble value)
+{
+ HALSIM_SetSPIAccelerometerY(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI
+ * Method: registerZCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI_registerZCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterSPIAccelerometerZCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI
+ * Method: cancelZCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI_cancelZCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelSPIAccelerometerZCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI
+ * Method: getZ
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI_getZ
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetSPIAccelerometerZ(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI
+ * Method: setZ
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI_setZ
+ (JNIEnv*, jclass, jint index, jdouble value)
+{
+ HALSIM_SetSPIAccelerometerZ(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI
+ * Method: resetData
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI_resetData
+ (JNIEnv*, jclass, jint index)
+{
+ HALSIM_ResetSPIAccelerometerData(index);
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/sim/jni/SPIDataJNI.cpp b/hal/src/main/native/sim/jni/SPIDataJNI.cpp
new file mode 100644
index 0000000..4eb342c
--- /dev/null
+++ b/hal/src/main/native/sim/jni/SPIDataJNI.cpp
@@ -0,0 +1,158 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 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 <jni.h>
+
+#include "BufferCallbackStore.h"
+#include "CallbackStore.h"
+#include "ConstBufferCallbackStore.h"
+#include "SpiReadAutoReceiveBufferCallbackStore.h"
+#include "edu_wpi_first_hal_sim_mockdata_SPIDataJNI.h"
+#include "mockdata/SPIData.h"
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SPIDataJNI
+ * Method: registerInitializedCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SPIDataJNI_registerInitializedCallback
+ (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+ return sim::AllocateCallback(env, index, callback, initialNotify,
+ &HALSIM_RegisterSPIInitializedCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SPIDataJNI
+ * Method: cancelInitializedCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SPIDataJNI_cancelInitializedCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ return sim::FreeCallback(env, handle, index,
+ &HALSIM_CancelSPIInitializedCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SPIDataJNI
+ * Method: getInitialized
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SPIDataJNI_getInitialized
+ (JNIEnv*, jclass, jint index)
+{
+ return HALSIM_GetSPIInitialized(index);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SPIDataJNI
+ * Method: setInitialized
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SPIDataJNI_setInitialized
+ (JNIEnv*, jclass, jint index, jboolean value)
+{
+ HALSIM_SetSPIInitialized(index, value);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SPIDataJNI
+ * Method: registerReadCallback
+ * Signature: (ILjava/lang/Object;)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SPIDataJNI_registerReadCallback
+ (JNIEnv* env, jclass, jint index, jobject callback)
+{
+ return sim::AllocateBufferCallback(env, index, callback,
+ &HALSIM_RegisterSPIReadCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SPIDataJNI
+ * Method: cancelReadCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SPIDataJNI_cancelReadCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ sim::FreeBufferCallback(env, handle, index, &HALSIM_CancelSPIReadCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SPIDataJNI
+ * Method: registerWriteCallback
+ * Signature: (ILjava/lang/Object;)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SPIDataJNI_registerWriteCallback
+ (JNIEnv* env, jclass, jint index, jobject callback)
+{
+ return sim::AllocateConstBufferCallback(env, index, callback,
+ &HALSIM_RegisterSPIWriteCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SPIDataJNI
+ * Method: cancelWriteCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SPIDataJNI_cancelWriteCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ sim::FreeConstBufferCallback(env, handle, index,
+ &HALSIM_CancelSPIWriteCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SPIDataJNI
+ * Method: registerReadAutoReceiveBufferCallback
+ * Signature: (ILjava/lang/Object;)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SPIDataJNI_registerReadAutoReceiveBufferCallback
+ (JNIEnv* env, jclass, jint index, jobject callback)
+{
+ return sim::AllocateSpiBufferCallback(
+ env, index, callback, &HALSIM_RegisterSPIReadAutoReceivedDataCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SPIDataJNI
+ * Method: cancelReadAutoReceiveBufferCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SPIDataJNI_cancelReadAutoReceiveBufferCallback
+ (JNIEnv* env, jclass, jint index, jint handle)
+{
+ sim::FreeSpiBufferCallback(env, handle, index,
+ &HALSIM_CancelSPIReadAutoReceivedDataCallback);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SPIDataJNI
+ * Method: resetData
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SPIDataJNI_resetData
+ (JNIEnv*, jclass, jint index)
+{
+ HALSIM_ResetSPIData(index);
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/sim/jni/SimDeviceDataJNI.cpp b/hal/src/main/native/sim/jni/SimDeviceDataJNI.cpp
new file mode 100644
index 0000000..f6cd05e
--- /dev/null
+++ b/hal/src/main/native/sim/jni/SimDeviceDataJNI.cpp
@@ -0,0 +1,573 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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 "SimDeviceDataJNI.h"
+
+#include <jni.h>
+
+#include <functional>
+#include <string>
+#include <utility>
+
+#include <wpi/UidVector.h>
+#include <wpi/jni_util.h>
+
+#include "SimulatorJNI.h"
+#include "edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI.h"
+#include "mockdata/SimDeviceData.h"
+
+using namespace wpi::java;
+
+static JClass simDeviceInfoCls;
+static JClass simValueInfoCls;
+static JClass simDeviceCallbackCls;
+static JClass simValueCallbackCls;
+static jmethodID simDeviceCallbackCallback;
+static jmethodID simValueCallbackCallback;
+
+namespace {
+
+struct DeviceInfo {
+ DeviceInfo(const char* name_, HAL_SimDeviceHandle handle_)
+ : name{name_}, handle{handle_} {}
+ std::string name;
+ HAL_SimValueHandle handle;
+
+ jobject MakeJava(JNIEnv* env) const;
+ void CallJava(JNIEnv* env, jobject callobj) const;
+};
+
+struct ValueInfo {
+ ValueInfo(const char* name_, HAL_SimValueHandle handle_, bool readonly_,
+ const HAL_Value& value_)
+ : name{name_}, handle{handle_}, readonly{readonly_}, value{value_} {}
+ std::string name;
+ HAL_SimValueHandle handle;
+ bool readonly;
+ HAL_Value value;
+
+ jobject MakeJava(JNIEnv* env) const;
+ void CallJava(JNIEnv* env, jobject callobj) const;
+
+ private:
+ std::pair<jlong, jdouble> ToValue12() const;
+};
+
+} // namespace
+
+jobject DeviceInfo::MakeJava(JNIEnv* env) const {
+ static jmethodID func =
+ env->GetMethodID(simDeviceInfoCls, "<init>", "(Ljava/lang/String;I)V");
+ return env->NewObject(simDeviceInfoCls, func, MakeJString(env, name),
+ (jint)handle);
+}
+
+void DeviceInfo::CallJava(JNIEnv* env, jobject callobj) const {
+ env->CallVoidMethod(callobj, simDeviceCallbackCallback,
+ MakeJString(env, name), (jint)handle);
+}
+
+std::pair<jlong, jdouble> ValueInfo::ToValue12() const {
+ jlong value1 = 0;
+ jdouble value2 = 0.0;
+ switch (value.type) {
+ case HAL_BOOLEAN:
+ value1 = value.data.v_boolean;
+ break;
+ case HAL_DOUBLE:
+ value2 = value.data.v_double;
+ break;
+ case HAL_ENUM:
+ value1 = value.data.v_enum;
+ break;
+ case HAL_INT:
+ value1 = value.data.v_int;
+ break;
+ case HAL_LONG:
+ value1 = value.data.v_long;
+ break;
+ default:
+ break;
+ }
+ return std::pair(value1, value2);
+}
+
+jobject ValueInfo::MakeJava(JNIEnv* env) const {
+ static jmethodID func =
+ env->GetMethodID(simValueInfoCls, "<init>", "(Ljava/lang/String;IZIJD)V");
+ auto [value1, value2] = ToValue12();
+ return env->NewObject(simValueInfoCls, func, MakeJString(env, name),
+ (jint)handle, (jboolean)readonly, (jint)value.type,
+ value1, value2);
+}
+
+void ValueInfo::CallJava(JNIEnv* env, jobject callobj) const {
+ auto [value1, value2] = ToValue12();
+ env->CallVoidMethod(callobj, simValueCallbackCallback, MakeJString(env, name),
+ (jint)handle, (jboolean)readonly, (jint)value.type,
+ value1, value2);
+}
+
+namespace {
+
+class CallbackStore {
+ public:
+ explicit CallbackStore(JNIEnv* env, jobject obj) : m_call{env, obj} {}
+ ~CallbackStore() {
+ if (m_cancelCallback) m_cancelCallback();
+ }
+
+ void SetCancel(std::function<void()> cancelCallback) {
+ m_cancelCallback = std::move(cancelCallback);
+ }
+ void Free(JNIEnv* env) { m_call.free(env); }
+ jobject Get() const { return m_call; }
+
+ private:
+ wpi::java::JGlobal<jobject> m_call;
+ std::function<void()> m_cancelCallback;
+};
+
+class CallbackThreadJNI : public wpi::SafeThread {
+ public:
+ void Main();
+
+ using DeviceCalls =
+ std::vector<std::pair<std::weak_ptr<CallbackStore>, DeviceInfo>>;
+ DeviceCalls m_deviceCalls;
+ using ValueCalls =
+ std::vector<std::pair<std::weak_ptr<CallbackStore>, ValueInfo>>;
+ ValueCalls m_valueCalls;
+
+ wpi::UidVector<std::shared_ptr<CallbackStore>, 4> m_callbacks;
+};
+
+class CallbackJNI {
+ public:
+ static CallbackJNI& GetInstance() {
+ static CallbackJNI inst;
+ return inst;
+ }
+ void SendDevice(int32_t callback, DeviceInfo info);
+ void SendValue(int32_t callback, ValueInfo info);
+
+ std::pair<int32_t, std::shared_ptr<CallbackStore>> AllocateCallback(
+ JNIEnv* env, jobject obj);
+
+ void FreeCallback(JNIEnv* env, int32_t uid);
+
+ private:
+ CallbackJNI() { m_owner.Start(); }
+
+ wpi::SafeThreadOwner<CallbackThreadJNI> m_owner;
+};
+
+} // namespace
+
+void CallbackThreadJNI::Main() {
+ JNIEnv* env;
+ JavaVMAttachArgs args;
+ args.version = JNI_VERSION_1_2;
+ args.name = const_cast<char*>("SimDeviceCallback");
+ args.group = nullptr;
+ jint rs = sim::GetJVM()->AttachCurrentThreadAsDaemon(
+ reinterpret_cast<void**>(&env), &args);
+ if (rs != JNI_OK) return;
+
+ DeviceCalls deviceCalls;
+ ValueCalls valueCalls;
+
+ std::unique_lock lock(m_mutex);
+ while (m_active) {
+ m_cond.wait(lock, [&] { return !m_active; });
+ if (!m_active) break;
+
+ deviceCalls.swap(m_deviceCalls);
+ valueCalls.swap(m_valueCalls);
+
+ lock.unlock(); // don't hold mutex during callback execution
+
+ for (auto&& call : deviceCalls) {
+ if (auto store = call.first.lock()) {
+ if (jobject callobj = store->Get()) {
+ call.second.CallJava(env, callobj);
+ if (env->ExceptionCheck()) {
+ env->ExceptionDescribe();
+ env->ExceptionClear();
+ }
+ }
+ }
+ }
+
+ for (auto&& call : valueCalls) {
+ if (auto store = call.first.lock()) {
+ if (jobject callobj = store->Get()) {
+ call.second.CallJava(env, callobj);
+ if (env->ExceptionCheck()) {
+ env->ExceptionDescribe();
+ env->ExceptionClear();
+ }
+ }
+ }
+ }
+
+ deviceCalls.clear();
+ valueCalls.clear();
+
+ lock.lock();
+ }
+
+ // free global references
+ for (auto&& callback : m_callbacks) callback->Free(env);
+
+ sim::GetJVM()->DetachCurrentThread();
+}
+
+void CallbackJNI::SendDevice(int32_t callback, DeviceInfo info) {
+ auto thr = m_owner.GetThread();
+ if (!thr) return;
+ thr->m_deviceCalls.emplace_back(thr->m_callbacks[callback], std::move(info));
+ thr->m_cond.notify_one();
+}
+
+void CallbackJNI::SendValue(int32_t callback, ValueInfo info) {
+ auto thr = m_owner.GetThread();
+ if (!thr) return;
+ thr->m_valueCalls.emplace_back(thr->m_callbacks[callback], std::move(info));
+ thr->m_cond.notify_one();
+}
+
+std::pair<int32_t, std::shared_ptr<CallbackStore>>
+CallbackJNI::AllocateCallback(JNIEnv* env, jobject obj) {
+ auto thr = m_owner.GetThread();
+ if (!thr) return std::pair(0, nullptr);
+ auto store = std::make_shared<CallbackStore>(env, obj);
+ return std::pair(thr->m_callbacks.emplace_back(store) + 1, store);
+}
+
+void CallbackJNI::FreeCallback(JNIEnv* env, int32_t uid) {
+ auto thr = m_owner.GetThread();
+ if (!thr) return;
+ if (uid <= 0 || static_cast<uint32_t>(uid) >= thr->m_callbacks.size()) return;
+ --uid;
+ auto store = std::move(thr->m_callbacks[uid]);
+ thr->m_callbacks.erase(uid);
+ store->Free(env);
+}
+
+namespace sim {
+
+bool InitializeSimDeviceDataJNI(JNIEnv* env) {
+ simDeviceInfoCls = JClass(
+ env, "edu/wpi/first/hal/sim/mockdata/SimDeviceDataJNI$SimDeviceInfo");
+ if (!simDeviceInfoCls) return false;
+
+ simValueInfoCls = JClass(
+ env, "edu/wpi/first/hal/sim/mockdata/SimDeviceDataJNI$SimValueInfo");
+ if (!simValueInfoCls) return false;
+
+ simDeviceCallbackCls = JClass(env, "edu/wpi/first/hal/sim/SimDeviceCallback");
+ if (!simDeviceCallbackCls) return false;
+
+ simDeviceCallbackCallback = env->GetMethodID(simDeviceCallbackCls, "callback",
+ "(Ljava/lang/String;I)V");
+ if (!simDeviceCallbackCallback) return false;
+
+ simValueCallbackCls = JClass(env, "edu/wpi/first/hal/sim/SimValueCallback");
+ if (!simValueCallbackCls) return false;
+
+ simValueCallbackCallback = env->GetMethodID(
+ simValueCallbackCls, "callbackNative", "(Ljava/lang/String;IZIJD)V");
+ if (!simValueCallbackCallback) return false;
+
+ return true;
+}
+
+void FreeSimDeviceDataJNI(JNIEnv* env) {
+ simDeviceInfoCls.free(env);
+ simValueInfoCls.free(env);
+ simDeviceCallbackCls.free(env);
+ simValueCallbackCls.free(env);
+}
+
+} // namespace sim
+
+extern "C" {
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI
+ * Method: registerSimDeviceCreatedCallback
+ * Signature: (Ljava/lang/String;Ljava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI_registerSimDeviceCreatedCallback
+ (JNIEnv* env, jclass, jstring prefix, jobject callback,
+ jboolean initialNotify)
+{
+ auto [uid, store] =
+ CallbackJNI::GetInstance().AllocateCallback(env, callback);
+ int32_t cuid = HALSIM_RegisterSimDeviceCreatedCallback(
+ JStringRef{env, prefix}.c_str(),
+ reinterpret_cast<void*>(static_cast<intptr_t>(uid)),
+ [](const char* name, void* param, HAL_SimDeviceHandle handle) {
+ int32_t uid = reinterpret_cast<intptr_t>(param);
+ CallbackJNI::GetInstance().SendDevice(uid, DeviceInfo{name, handle});
+ },
+ initialNotify);
+ store->SetCancel([cuid] { HALSIM_CancelSimDeviceCreatedCallback(cuid); });
+ return uid;
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI
+ * Method: cancelSimDeviceCreatedCallback
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI_cancelSimDeviceCreatedCallback
+ (JNIEnv* env, jclass, jint uid)
+{
+ CallbackJNI::GetInstance().FreeCallback(env, uid);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI
+ * Method: registerSimDeviceFreedCallback
+ * Signature: (Ljava/lang/String;Ljava/lang/Object;)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI_registerSimDeviceFreedCallback
+ (JNIEnv* env, jclass, jstring prefix, jobject callback)
+{
+ auto [uid, store] =
+ CallbackJNI::GetInstance().AllocateCallback(env, callback);
+ int32_t cuid = HALSIM_RegisterSimDeviceFreedCallback(
+ JStringRef{env, prefix}.c_str(),
+ reinterpret_cast<void*>(static_cast<intptr_t>(uid)),
+ [](const char* name, void* param, HAL_SimDeviceHandle handle) {
+ int32_t uid = reinterpret_cast<intptr_t>(param);
+ CallbackJNI::GetInstance().SendDevice(uid, DeviceInfo{name, handle});
+ });
+ store->SetCancel([cuid] { HALSIM_CancelSimDeviceFreedCallback(cuid); });
+ return uid;
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI
+ * Method: cancelSimDeviceFreedCallback
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI_cancelSimDeviceFreedCallback
+ (JNIEnv* env, jclass, jint uid)
+{
+ CallbackJNI::GetInstance().FreeCallback(env, uid);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI
+ * Method: getSimDeviceHandle
+ * Signature: (Ljava/lang/String;)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI_getSimDeviceHandle
+ (JNIEnv* env, jclass, jstring name)
+{
+ return HALSIM_GetSimDeviceHandle(JStringRef{env, name}.c_str());
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI
+ * Method: getSimValueDeviceHandle
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI_getSimValueDeviceHandle
+ (JNIEnv*, jclass, jint handle)
+{
+ return HALSIM_GetSimValueDeviceHandle(handle);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI
+ * Method: enumerateSimDevices
+ * Signature: (Ljava/lang/String;)[Ljava/lang/Object;
+ */
+JNIEXPORT jobjectArray JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI_enumerateSimDevices
+ (JNIEnv* env, jclass, jstring prefix)
+{
+ // get values
+ std::vector<DeviceInfo> arr;
+ HALSIM_EnumerateSimDevices(
+ JStringRef{env, prefix}.c_str(), &arr,
+ [](const char* name, void* param, HAL_SimDeviceHandle handle) {
+ auto arr = static_cast<std::vector<DeviceInfo>*>(param);
+ arr->emplace_back(name, handle);
+ });
+
+ // convert to java
+ size_t numElems = arr.size();
+ jobjectArray jarr =
+ env->NewObjectArray(arr.size(), simDeviceInfoCls, nullptr);
+ if (!jarr) return nullptr;
+ for (size_t i = 0; i < numElems; ++i) {
+ JLocal<jobject> elem{env, arr[i].MakeJava(env)};
+ env->SetObjectArrayElement(jarr, i, elem.obj());
+ }
+ return jarr;
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI
+ * Method: registerSimValueCreatedCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI_registerSimValueCreatedCallback
+ (JNIEnv* env, jclass, jint device, jobject callback, jboolean initialNotify)
+{
+ auto [uid, store] =
+ CallbackJNI::GetInstance().AllocateCallback(env, callback);
+ int32_t cuid = HALSIM_RegisterSimValueCreatedCallback(
+ device, reinterpret_cast<void*>(static_cast<intptr_t>(uid)),
+ [](const char* name, void* param, HAL_SimValueHandle handle,
+ HAL_Bool readonly, const HAL_Value* value) {
+ int32_t uid = reinterpret_cast<intptr_t>(param);
+ CallbackJNI::GetInstance().SendValue(
+ uid, ValueInfo{name, handle, static_cast<bool>(readonly), *value});
+ },
+ initialNotify);
+ store->SetCancel([cuid] { HALSIM_CancelSimValueCreatedCallback(cuid); });
+ return uid;
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI
+ * Method: cancelSimValueCreatedCallback
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI_cancelSimValueCreatedCallback
+ (JNIEnv* env, jclass, jint uid)
+{
+ CallbackJNI::GetInstance().FreeCallback(env, uid);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI
+ * Method: registerSimValueChangedCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI_registerSimValueChangedCallback
+ (JNIEnv* env, jclass, jint handle, jobject callback, jboolean initialNotify)
+{
+ auto [uid, store] =
+ CallbackJNI::GetInstance().AllocateCallback(env, callback);
+ int32_t cuid = HALSIM_RegisterSimValueChangedCallback(
+ handle, reinterpret_cast<void*>(static_cast<intptr_t>(uid)),
+ [](const char* name, void* param, HAL_SimValueHandle handle,
+ HAL_Bool readonly, const HAL_Value* value) {
+ int32_t uid = reinterpret_cast<intptr_t>(param);
+ CallbackJNI::GetInstance().SendValue(
+ uid, ValueInfo{name, handle, static_cast<bool>(readonly), *value});
+ },
+ initialNotify);
+ store->SetCancel([cuid] { HALSIM_CancelSimValueChangedCallback(cuid); });
+ return uid;
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI
+ * Method: cancelSimValueChangedCallback
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI_cancelSimValueChangedCallback
+ (JNIEnv* env, jclass, jint uid)
+{
+ CallbackJNI::GetInstance().FreeCallback(env, uid);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI
+ * Method: getSimValueHandle
+ * Signature: (ILjava/lang/String;)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI_getSimValueHandle
+ (JNIEnv* env, jclass, jint device, jstring name)
+{
+ return HALSIM_GetSimValueHandle(device, JStringRef{env, name}.c_str());
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI
+ * Method: enumerateSimValues
+ * Signature: (I)[Ljava/lang/Object;
+ */
+JNIEXPORT jobjectArray JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI_enumerateSimValues
+ (JNIEnv* env, jclass, jint device)
+{
+ // get values
+ std::vector<ValueInfo> arr;
+ HALSIM_EnumerateSimValues(
+ device, &arr,
+ [](const char* name, void* param, HAL_SimValueHandle handle,
+ HAL_Bool readonly, const HAL_Value* value) {
+ auto arr = static_cast<std::vector<ValueInfo>*>(param);
+ arr->emplace_back(name, handle, readonly, *value);
+ });
+
+ // convert to java
+ size_t numElems = arr.size();
+ jobjectArray jarr = env->NewObjectArray(arr.size(), simValueInfoCls, nullptr);
+ if (!jarr) return nullptr;
+ for (size_t i = 0; i < numElems; ++i) {
+ JLocal<jobject> elem{env, arr[i].MakeJava(env)};
+ env->SetObjectArrayElement(jarr, i, elem.obj());
+ }
+ return jarr;
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI
+ * Method: getSimValueEnumOptions
+ * Signature: (I)[Ljava/lang/Object;
+ */
+JNIEXPORT jobjectArray JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI_getSimValueEnumOptions
+ (JNIEnv* env, jclass, jint handle)
+{
+ static JClass stringCls{env, "java/lang/String"};
+ if (!stringCls) return nullptr;
+ int32_t numElems = 0;
+ const char** elems = HALSIM_GetSimValueEnumOptions(handle, &numElems);
+ jobjectArray jarr = env->NewObjectArray(numElems, stringCls, nullptr);
+ if (!jarr) return nullptr;
+ for (int32_t i = 0; i < numElems; ++i) {
+ JLocal<jstring> elem{env, MakeJString(env, elems[i])};
+ env->SetObjectArrayElement(jarr, i, elem.obj());
+ }
+ return jarr;
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI
+ * Method: resetSimDeviceData
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI_resetSimDeviceData
+ (JNIEnv*, jclass)
+{
+ HALSIM_ResetSimDeviceData();
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/sim/jni/SimDeviceDataJNI.h b/hal/src/main/native/sim/jni/SimDeviceDataJNI.h
new file mode 100644
index 0000000..56f6d9b
--- /dev/null
+++ b/hal/src/main/native/sim/jni/SimDeviceDataJNI.h
@@ -0,0 +1,15 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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 <jni.h>
+
+namespace sim {
+bool InitializeSimDeviceDataJNI(JNIEnv* env);
+void FreeSimDeviceDataJNI(JNIEnv* env);
+} // namespace sim
diff --git a/hal/src/main/native/sim/jni/SimulatorJNI.cpp b/hal/src/main/native/sim/jni/SimulatorJNI.cpp
new file mode 100644
index 0000000..9226f91
--- /dev/null
+++ b/hal/src/main/native/sim/jni/SimulatorJNI.cpp
@@ -0,0 +1,204 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 "SimulatorJNI.h"
+
+#include <wpi/jni_util.h>
+
+#include "BufferCallbackStore.h"
+#include "CallbackStore.h"
+#include "ConstBufferCallbackStore.h"
+#include "SimDeviceDataJNI.h"
+#include "SpiReadAutoReceiveBufferCallbackStore.h"
+#include "edu_wpi_first_hal_sim_mockdata_SimulatorJNI.h"
+#include "hal/HAL.h"
+#include "hal/handles/HandlesInternal.h"
+#include "mockdata/MockHooks.h"
+
+using namespace wpi::java;
+
+static JavaVM* jvm = nullptr;
+static JClass notifyCallbackCls;
+static JClass bufferCallbackCls;
+static JClass constBufferCallbackCls;
+static JClass spiReadAutoReceiveBufferCallbackCls;
+static jmethodID notifyCallbackCallback;
+static jmethodID bufferCallbackCallback;
+static jmethodID constBufferCallbackCallback;
+static jmethodID spiReadAutoReceiveBufferCallbackCallback;
+
+namespace sim {
+jint SimOnLoad(JavaVM* vm, void* reserved) {
+ jvm = vm;
+
+ JNIEnv* env;
+ if (vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6) != JNI_OK)
+ return JNI_ERR;
+
+ notifyCallbackCls = JClass(env, "edu/wpi/first/hal/sim/NotifyCallback");
+ if (!notifyCallbackCls) return JNI_ERR;
+
+ notifyCallbackCallback = env->GetMethodID(notifyCallbackCls, "callbackNative",
+ "(Ljava/lang/String;IJD)V");
+ if (!notifyCallbackCallback) return JNI_ERR;
+
+ bufferCallbackCls = JClass(env, "edu/wpi/first/hal/sim/BufferCallback");
+ if (!bufferCallbackCls) return JNI_ERR;
+
+ bufferCallbackCallback = env->GetMethodID(bufferCallbackCls, "callback",
+ "(Ljava/lang/String;[BI)V");
+ if (!bufferCallbackCallback) return JNI_ERR;
+
+ constBufferCallbackCls =
+ JClass(env, "edu/wpi/first/hal/sim/ConstBufferCallback");
+ if (!constBufferCallbackCls) return JNI_ERR;
+
+ constBufferCallbackCallback = env->GetMethodID(
+ constBufferCallbackCls, "callback", "(Ljava/lang/String;[BI)V");
+ if (!constBufferCallbackCallback) return JNI_ERR;
+
+ spiReadAutoReceiveBufferCallbackCls =
+ JClass(env, "edu/wpi/first/hal/sim/SpiReadAutoReceiveBufferCallback");
+ if (!spiReadAutoReceiveBufferCallbackCls) return JNI_ERR;
+
+ spiReadAutoReceiveBufferCallbackCallback =
+ env->GetMethodID(spiReadAutoReceiveBufferCallbackCls, "callback",
+ "(Ljava/lang/String;[II)I");
+ if (!spiReadAutoReceiveBufferCallbackCallback) return JNI_ERR;
+
+ InitializeStore();
+ InitializeBufferStore();
+ InitializeConstBufferStore();
+ InitializeSpiBufferStore();
+ if (!InitializeSimDeviceDataJNI(env)) return JNI_ERR;
+
+ return JNI_VERSION_1_6;
+}
+
+void SimOnUnload(JavaVM* vm, void* reserved) {
+ JNIEnv* env;
+ if (vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6) != JNI_OK)
+ return;
+
+ notifyCallbackCls.free(env);
+ bufferCallbackCls.free(env);
+ constBufferCallbackCls.free(env);
+ spiReadAutoReceiveBufferCallbackCls.free(env);
+ FreeSimDeviceDataJNI(env);
+ jvm = nullptr;
+}
+
+JavaVM* GetJVM() { return jvm; }
+
+jmethodID GetNotifyCallback() { return notifyCallbackCallback; }
+
+jmethodID GetBufferCallback() { return bufferCallbackCallback; }
+
+jmethodID GetConstBufferCallback() { return constBufferCallbackCallback; }
+
+jmethodID GetSpiReadAutoReceiveBufferCallback() {
+ return spiReadAutoReceiveBufferCallbackCallback;
+}
+} // namespace sim
+
+extern "C" {
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SimulatorJNI
+ * Method: waitForProgramStart
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SimulatorJNI_waitForProgramStart
+ (JNIEnv*, jclass)
+{
+ HALSIM_WaitForProgramStart();
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SimulatorJNI
+ * Method: setProgramStarted
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SimulatorJNI_setProgramStarted
+ (JNIEnv*, jclass)
+{
+ HALSIM_SetProgramStarted();
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SimulatorJNI
+ * Method: restartTiming
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SimulatorJNI_restartTiming
+ (JNIEnv*, jclass)
+{
+ HALSIM_RestartTiming();
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SimulatorJNI
+ * Method: pauseTiming
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SimulatorJNI_pauseTiming
+ (JNIEnv*, jclass)
+{
+ HALSIM_PauseTiming();
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SimulatorJNI
+ * Method: resumeTiming
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SimulatorJNI_resumeTiming
+ (JNIEnv*, jclass)
+{
+ HALSIM_ResumeTiming();
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SimulatorJNI
+ * Method: isTimingPaused
+ * Signature: ()Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SimulatorJNI_isTimingPaused
+ (JNIEnv*, jclass)
+{
+ return HALSIM_IsTimingPaused();
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SimulatorJNI
+ * Method: stepTiming
+ * Signature: (J)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SimulatorJNI_stepTiming
+ (JNIEnv*, jclass, jlong delta)
+{
+ HALSIM_StepTiming(delta);
+}
+
+/*
+ * Class: edu_wpi_first_hal_sim_mockdata_SimulatorJNI
+ * Method: resetHandles
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SimulatorJNI_resetHandles
+ (JNIEnv*, jclass)
+{
+ hal::HandleBase::ResetGlobalHandles();
+}
+} // extern "C"
diff --git a/hal/src/main/native/sim/jni/SimulatorJNI.h b/hal/src/main/native/sim/jni/SimulatorJNI.h
new file mode 100644
index 0000000..8680396
--- /dev/null
+++ b/hal/src/main/native/sim/jni/SimulatorJNI.h
@@ -0,0 +1,22 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 "hal/Types.h"
+#include "jni.h"
+
+typedef HAL_Handle SIM_JniHandle;
+
+namespace sim {
+JavaVM* GetJVM();
+
+jmethodID GetNotifyCallback();
+jmethodID GetBufferCallback();
+jmethodID GetConstBufferCallback();
+jmethodID GetSpiReadAutoReceiveBufferCallback();
+} // namespace sim
diff --git a/hal/src/main/native/sim/jni/SpiReadAutoReceiveBufferCallbackStore.cpp b/hal/src/main/native/sim/jni/SpiReadAutoReceiveBufferCallbackStore.cpp
new file mode 100644
index 0000000..b75bb1e
--- /dev/null
+++ b/hal/src/main/native/sim/jni/SpiReadAutoReceiveBufferCallbackStore.cpp
@@ -0,0 +1,130 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 "SpiReadAutoReceiveBufferCallbackStore.h"
+
+#include <jni.h>
+
+#include <wpi/jni_util.h>
+
+#include "SimulatorJNI.h"
+#include "hal/Types.h"
+#include "hal/Value.h"
+#include "hal/handles/UnlimitedHandleResource.h"
+#include "mockdata/NotifyListener.h"
+
+using namespace wpi::java;
+using namespace sim;
+
+static hal::UnlimitedHandleResource<
+ SIM_JniHandle, SpiReadAutoReceiveBufferCallbackStore,
+ hal::HAL_HandleEnum::SimulationJni>* callbackHandles;
+
+namespace sim {
+void InitializeSpiBufferStore() {
+ static hal::UnlimitedHandleResource<SIM_JniHandle,
+ SpiReadAutoReceiveBufferCallbackStore,
+ hal::HAL_HandleEnum::SimulationJni>
+ cb;
+ callbackHandles = &cb;
+}
+} // namespace sim
+
+void SpiReadAutoReceiveBufferCallbackStore::create(JNIEnv* env, jobject obj) {
+ m_call = JGlobal<jobject>(env, obj);
+}
+
+int32_t SpiReadAutoReceiveBufferCallbackStore::performCallback(
+ const char* name, uint32_t* buffer, int32_t numToRead) {
+ JNIEnv* env;
+ JavaVM* vm = sim::GetJVM();
+ bool didAttachThread = false;
+ int tryGetEnv = vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6);
+ if (tryGetEnv == JNI_EDETACHED) {
+ // Thread not attached
+ didAttachThread = true;
+ if (vm->AttachCurrentThread(reinterpret_cast<void**>(&env), nullptr) != 0) {
+ // Failed to attach, log and return
+ wpi::outs() << "Failed to attach\n";
+ wpi::outs().flush();
+ return -1;
+ }
+ } else if (tryGetEnv == JNI_EVERSION) {
+ wpi::outs() << "Invalid JVM Version requested\n";
+ wpi::outs().flush();
+ }
+
+ auto toCallbackArr = MakeJIntArray(
+ env, wpi::ArrayRef<uint32_t>{buffer, static_cast<size_t>(numToRead)});
+
+ jint ret = env->CallIntMethod(m_call, sim::GetBufferCallback(),
+ MakeJString(env, name), toCallbackArr,
+ (jint)numToRead);
+
+ jint* fromCallbackArr = reinterpret_cast<jint*>(
+ env->GetPrimitiveArrayCritical(toCallbackArr, nullptr));
+
+ for (int i = 0; i < ret; i++) {
+ buffer[i] = fromCallbackArr[i];
+ }
+
+ env->ReleasePrimitiveArrayCritical(toCallbackArr, fromCallbackArr, JNI_ABORT);
+
+ if (env->ExceptionCheck()) {
+ env->ExceptionDescribe();
+ }
+
+ if (didAttachThread) {
+ vm->DetachCurrentThread();
+ }
+ return ret;
+}
+
+void SpiReadAutoReceiveBufferCallbackStore::free(JNIEnv* env) {
+ m_call.free(env);
+}
+
+SIM_JniHandle sim::AllocateSpiBufferCallback(
+ JNIEnv* env, jint index, jobject callback,
+ RegisterSpiBufferCallbackFunc createCallback) {
+ auto callbackStore =
+ std::make_shared<SpiReadAutoReceiveBufferCallbackStore>();
+
+ auto handle = callbackHandles->Allocate(callbackStore);
+
+ if (handle == HAL_kInvalidHandle) {
+ return -1;
+ }
+
+ uintptr_t handleAsPtr = static_cast<uintptr_t>(handle);
+ void* handleAsVoidPtr = reinterpret_cast<void*>(handleAsPtr);
+
+ callbackStore->create(env, callback);
+
+ auto callbackFunc = [](const char* name, void* param, uint32_t* buffer,
+ int32_t numToRead, int32_t* outputCount) {
+ uintptr_t handleTmp = reinterpret_cast<uintptr_t>(param);
+ SIM_JniHandle handle = static_cast<SIM_JniHandle>(handleTmp);
+ auto data = callbackHandles->Get(handle);
+ if (!data) return;
+
+ *outputCount = data->performCallback(name, buffer, numToRead);
+ };
+
+ auto id = createCallback(index, callbackFunc, handleAsVoidPtr);
+
+ callbackStore->setCallbackId(id);
+
+ return handle;
+}
+
+void sim::FreeSpiBufferCallback(JNIEnv* env, SIM_JniHandle handle, jint index,
+ FreeSpiBufferCallbackFunc freeCallback) {
+ auto callback = callbackHandles->Free(handle);
+ freeCallback(index, callback->getCallbackId());
+ callback->free(env);
+}
diff --git a/hal/src/main/native/sim/jni/SpiReadAutoReceiveBufferCallbackStore.h b/hal/src/main/native/sim/jni/SpiReadAutoReceiveBufferCallbackStore.h
new file mode 100644
index 0000000..1a03f59
--- /dev/null
+++ b/hal/src/main/native/sim/jni/SpiReadAutoReceiveBufferCallbackStore.h
@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 <jni.h>
+
+#include <wpi/jni_util.h>
+
+#include "SimulatorJNI.h"
+#include "hal/Types.h"
+#include "hal/Value.h"
+#include "hal/handles/UnlimitedHandleResource.h"
+#include "mockdata/NotifyListener.h"
+#include "mockdata/SPIData.h"
+
+namespace sim {
+class SpiReadAutoReceiveBufferCallbackStore {
+ public:
+ void create(JNIEnv* env, jobject obj);
+ int32_t performCallback(const char* name, uint32_t* buffer,
+ int32_t numToRead);
+ void free(JNIEnv* env);
+ void setCallbackId(int32_t id) { callbackId = id; }
+ int32_t getCallbackId() { return callbackId; }
+
+ private:
+ wpi::java::JGlobal<jobject> m_call;
+ int32_t callbackId;
+};
+
+void InitializeSpiBufferStore();
+
+typedef int32_t (*RegisterSpiBufferCallbackFunc)(
+ int32_t index, HAL_SpiReadAutoReceiveBufferCallback callback, void* param);
+typedef void (*FreeSpiBufferCallbackFunc)(int32_t index, int32_t uid);
+
+SIM_JniHandle AllocateSpiBufferCallback(
+ JNIEnv* env, jint index, jobject callback,
+ RegisterSpiBufferCallbackFunc createCallback);
+void FreeSpiBufferCallback(JNIEnv* env, SIM_JniHandle handle, jint index,
+ FreeSpiBufferCallbackFunc freeCallback);
+} // namespace sim
diff --git a/hal/src/main/native/sim/mockdata/AccelerometerData.cpp b/hal/src/main/native/sim/mockdata/AccelerometerData.cpp
new file mode 100644
index 0000000..66e129a
--- /dev/null
+++ b/hal/src/main/native/sim/mockdata/AccelerometerData.cpp
@@ -0,0 +1,60 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 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 "../PortsInternal.h"
+#include "AccelerometerDataInternal.h"
+
+using namespace hal;
+
+namespace hal {
+namespace init {
+void InitializeAccelerometerData() {
+ static AccelerometerData sad[1];
+ ::hal::SimAccelerometerData = sad;
+}
+} // namespace init
+} // namespace hal
+
+AccelerometerData* hal::SimAccelerometerData;
+void AccelerometerData::ResetData() {
+ active.Reset(false);
+ range.Reset(static_cast<HAL_AccelerometerRange>(0));
+ x.Reset(0.0);
+ y.Reset(0.0);
+ z.Reset(0.0);
+}
+
+extern "C" {
+void HALSIM_ResetAccelerometerData(int32_t index) {
+ SimAccelerometerData[index].ResetData();
+}
+
+#define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME) \
+ HAL_SIMDATAVALUE_DEFINE_CAPI(TYPE, HALSIM, Accelerometer##CAPINAME, \
+ SimAccelerometerData, LOWERNAME)
+
+DEFINE_CAPI(HAL_Bool, Active, active)
+DEFINE_CAPI(HAL_AccelerometerRange, Range, range)
+DEFINE_CAPI(double, X, x)
+DEFINE_CAPI(double, Y, y)
+DEFINE_CAPI(double, Z, z)
+
+#define REGISTER(NAME) \
+ SimAccelerometerData[index].NAME.RegisterCallback(callback, param, \
+ initialNotify)
+
+void HALSIM_RegisterAccelerometerAllCallbacks(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify) {
+ REGISTER(active);
+ REGISTER(range);
+ REGISTER(x);
+ REGISTER(y);
+ REGISTER(z);
+}
+} // extern "C"
diff --git a/hal/src/main/native/sim/mockdata/AccelerometerDataInternal.h b/hal/src/main/native/sim/mockdata/AccelerometerDataInternal.h
new file mode 100644
index 0000000..9db0875
--- /dev/null
+++ b/hal/src/main/native/sim/mockdata/AccelerometerDataInternal.h
@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "mockdata/AccelerometerData.h"
+#include "mockdata/SimDataValue.h"
+
+namespace hal {
+class AccelerometerData {
+ HAL_SIMDATAVALUE_DEFINE_NAME(Active)
+ HAL_SIMDATAVALUE_DEFINE_NAME(Range)
+ HAL_SIMDATAVALUE_DEFINE_NAME(X)
+ HAL_SIMDATAVALUE_DEFINE_NAME(Y)
+ HAL_SIMDATAVALUE_DEFINE_NAME(Z)
+
+ static inline HAL_Value MakeRangeValue(HAL_AccelerometerRange value) {
+ return HAL_MakeEnum(value);
+ }
+
+ public:
+ SimDataValue<HAL_Bool, HAL_MakeBoolean, GetActiveName> active{false};
+ SimDataValue<HAL_AccelerometerRange, MakeRangeValue, GetRangeName> range{
+ static_cast<HAL_AccelerometerRange>(0)};
+ SimDataValue<double, HAL_MakeDouble, GetXName> x{0.0};
+ SimDataValue<double, HAL_MakeDouble, GetYName> y{0.0};
+ SimDataValue<double, HAL_MakeDouble, GetZName> z{0.0};
+
+ virtual void ResetData();
+};
+extern AccelerometerData* SimAccelerometerData;
+} // namespace hal
diff --git a/hal/src/main/native/sim/mockdata/AddressableLEDData.cpp b/hal/src/main/native/sim/mockdata/AddressableLEDData.cpp
new file mode 100644
index 0000000..5b44eaf
--- /dev/null
+++ b/hal/src/main/native/sim/mockdata/AddressableLEDData.cpp
@@ -0,0 +1,96 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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 <algorithm>
+#include <cstring>
+
+#include "../PortsInternal.h"
+#include "AddressableLEDDataInternal.h"
+
+using namespace hal;
+
+namespace hal {
+namespace init {
+void InitializeAddressableLEDData() {
+ static AddressableLEDData sad[kNumAddressableLEDs];
+ ::hal::SimAddressableLEDData = sad;
+}
+} // namespace init
+} // namespace hal
+
+AddressableLEDData* hal::SimAddressableLEDData;
+
+void AddressableLEDData::ResetData() {
+ initialized.Reset(false);
+ outputPort.Reset(-1);
+ length.Reset(1);
+ running.Reset(false);
+ data.Reset();
+}
+
+void AddressableLEDData::SetData(const HAL_AddressableLEDData* d, int32_t len) {
+ len = (std::min)(HAL_kAddressableLEDMaxLength, len);
+ {
+ std::scoped_lock lock(m_dataMutex);
+ std::memcpy(m_data, d, len * sizeof(d[0]));
+ }
+ data(reinterpret_cast<const uint8_t*>(d), len * sizeof(d[0]));
+}
+
+int32_t AddressableLEDData::GetData(HAL_AddressableLEDData* d) {
+ std::scoped_lock lock(m_dataMutex);
+ int32_t len = length;
+ if (d) std::memcpy(d, m_data, len * sizeof(d[0]));
+ return len;
+}
+
+extern "C" {
+void HALSIM_ResetAddressableLEDData(int32_t index) {
+ SimAddressableLEDData[index].ResetData();
+}
+
+int32_t HALSIM_GetAddressableLEDData(int32_t index,
+ struct HAL_AddressableLEDData* data) {
+ return SimAddressableLEDData[index].GetData(data);
+}
+
+void HALSIM_SetAddressableLEDData(int32_t index,
+ const struct HAL_AddressableLEDData* data,
+ int32_t length) {
+ SimAddressableLEDData[index].SetData(data, length);
+}
+
+#define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME) \
+ HAL_SIMDATAVALUE_DEFINE_CAPI(TYPE, HALSIM, AddressableLED##CAPINAME, \
+ SimAddressableLEDData, LOWERNAME)
+
+DEFINE_CAPI(HAL_Bool, Initialized, initialized)
+DEFINE_CAPI(int32_t, OutputPort, outputPort)
+DEFINE_CAPI(int32_t, Length, length)
+DEFINE_CAPI(HAL_Bool, Running, running)
+
+#undef DEFINE_CAPI
+#define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME) \
+ HAL_SIMCALLBACKREGISTRY_DEFINE_CAPI(TYPE, HALSIM, AddressableLED##CAPINAME, \
+ SimAddressableLEDData, LOWERNAME)
+
+DEFINE_CAPI(HAL_ConstBufferCallback, Data, data)
+
+#define REGISTER(NAME) \
+ SimAddressableLEDData[index].NAME.RegisterCallback(callback, param, \
+ initialNotify)
+
+void HALSIM_RegisterAddressableLEDAllCallbacks(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify) {
+ REGISTER(initialized);
+ REGISTER(outputPort);
+ REGISTER(length);
+ REGISTER(running);
+}
+} // extern "C"
diff --git a/hal/src/main/native/sim/mockdata/AddressableLEDDataInternal.h b/hal/src/main/native/sim/mockdata/AddressableLEDDataInternal.h
new file mode 100644
index 0000000..9d6e215
--- /dev/null
+++ b/hal/src/main/native/sim/mockdata/AddressableLEDDataInternal.h
@@ -0,0 +1,43 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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 <vector>
+
+#include <wpi/spinlock.h>
+
+#include "mockdata/AddressableLEDData.h"
+#include "mockdata/SimCallbackRegistry.h"
+#include "mockdata/SimDataValue.h"
+
+namespace hal {
+class AddressableLEDData {
+ HAL_SIMDATAVALUE_DEFINE_NAME(Initialized)
+ HAL_SIMDATAVALUE_DEFINE_NAME(OutputPort)
+ HAL_SIMDATAVALUE_DEFINE_NAME(Length)
+ HAL_SIMDATAVALUE_DEFINE_NAME(Running)
+ HAL_SIMDATAVALUE_DEFINE_NAME(Data)
+
+ wpi::recursive_spinlock m_dataMutex;
+ HAL_AddressableLEDData m_data[HAL_kAddressableLEDMaxLength];
+
+ public:
+ void SetData(const HAL_AddressableLEDData* d, int32_t len);
+ int32_t GetData(HAL_AddressableLEDData* d);
+
+ SimDataValue<HAL_Bool, HAL_MakeBoolean, GetInitializedName> initialized{
+ false};
+ SimDataValue<int32_t, HAL_MakeInt, GetOutputPortName> outputPort{-1};
+ SimDataValue<int32_t, HAL_MakeInt, GetLengthName> length{1};
+ SimDataValue<HAL_Bool, HAL_MakeBoolean, GetRunningName> running{false};
+ SimCallbackRegistry<HAL_ConstBufferCallback, GetDataName> data;
+
+ void ResetData();
+};
+extern AddressableLEDData* SimAddressableLEDData;
+} // namespace hal
diff --git a/hal/src/main/native/sim/mockdata/AnalogGyroData.cpp b/hal/src/main/native/sim/mockdata/AnalogGyroData.cpp
new file mode 100644
index 0000000..2508d9b
--- /dev/null
+++ b/hal/src/main/native/sim/mockdata/AnalogGyroData.cpp
@@ -0,0 +1,53 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 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 "../PortsInternal.h"
+#include "AnalogGyroDataInternal.h"
+
+using namespace hal;
+
+namespace hal {
+namespace init {
+void InitializeAnalogGyroData() {
+ static AnalogGyroData agd[kNumAccumulators];
+ ::hal::SimAnalogGyroData = agd;
+}
+} // namespace init
+} // namespace hal
+
+AnalogGyroData* hal::SimAnalogGyroData;
+void AnalogGyroData::ResetData() {
+ angle.Reset(0.0);
+ rate.Reset(0.0);
+ initialized.Reset(false);
+}
+
+extern "C" {
+void HALSIM_ResetAnalogGyroData(int32_t index) {
+ SimAnalogGyroData[index].ResetData();
+}
+
+#define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME) \
+ HAL_SIMDATAVALUE_DEFINE_CAPI(TYPE, HALSIM, AnalogGyro##CAPINAME, \
+ SimAnalogGyroData, LOWERNAME)
+
+DEFINE_CAPI(double, Angle, angle)
+DEFINE_CAPI(double, Rate, rate)
+DEFINE_CAPI(HAL_Bool, Initialized, initialized)
+
+#define REGISTER(NAME) \
+ SimAnalogGyroData[index].NAME.RegisterCallback(callback, param, initialNotify)
+
+void HALSIM_RegisterAnalogGyroAllCallbacks(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify) {
+ REGISTER(angle);
+ REGISTER(rate);
+ REGISTER(initialized);
+}
+} // extern "C"
diff --git a/hal/src/main/native/sim/mockdata/AnalogGyroDataInternal.h b/hal/src/main/native/sim/mockdata/AnalogGyroDataInternal.h
new file mode 100644
index 0000000..427d2fb
--- /dev/null
+++ b/hal/src/main/native/sim/mockdata/AnalogGyroDataInternal.h
@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "mockdata/AnalogGyroData.h"
+#include "mockdata/SimDataValue.h"
+
+namespace hal {
+class AnalogGyroData {
+ HAL_SIMDATAVALUE_DEFINE_NAME(Angle)
+ HAL_SIMDATAVALUE_DEFINE_NAME(Rate)
+ HAL_SIMDATAVALUE_DEFINE_NAME(Initialized)
+
+ public:
+ SimDataValue<double, HAL_MakeDouble, GetAngleName> angle{0.0};
+ SimDataValue<double, HAL_MakeDouble, GetRateName> rate{0.0};
+ SimDataValue<HAL_Bool, HAL_MakeBoolean, GetInitializedName> initialized{
+ false};
+
+ virtual void ResetData();
+};
+extern AnalogGyroData* SimAnalogGyroData;
+} // namespace hal
diff --git a/hal/src/main/native/sim/mockdata/AnalogInData.cpp b/hal/src/main/native/sim/mockdata/AnalogInData.cpp
new file mode 100644
index 0000000..a2a871c
--- /dev/null
+++ b/hal/src/main/native/sim/mockdata/AnalogInData.cpp
@@ -0,0 +1,75 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "../PortsInternal.h"
+#include "AnalogInDataInternal.h"
+
+using namespace hal;
+
+namespace hal {
+namespace init {
+void InitializeAnalogInData() {
+ static AnalogInData sind[kNumAnalogInputs];
+ ::hal::SimAnalogInData = sind;
+}
+} // namespace init
+} // namespace hal
+
+AnalogInData* hal::SimAnalogInData;
+void AnalogInData::ResetData() {
+ initialized.Reset(false);
+ simDevice = 0;
+ averageBits.Reset(7);
+ oversampleBits.Reset(0);
+ voltage.Reset(0.0);
+ accumulatorInitialized.Reset(false);
+ accumulatorValue.Reset(0);
+ accumulatorCount.Reset(0);
+ accumulatorCenter.Reset(0);
+ accumulatorDeadband.Reset(0);
+}
+
+extern "C" {
+void HALSIM_ResetAnalogInData(int32_t index) {
+ SimAnalogInData[index].ResetData();
+}
+
+HAL_SimDeviceHandle HALSIM_GetAnalogInSimDevice(int32_t index) {
+ return SimAnalogInData[index].simDevice;
+}
+
+#define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME) \
+ HAL_SIMDATAVALUE_DEFINE_CAPI(TYPE, HALSIM, AnalogIn##CAPINAME, \
+ SimAnalogInData, LOWERNAME)
+
+DEFINE_CAPI(HAL_Bool, Initialized, initialized)
+DEFINE_CAPI(int32_t, AverageBits, averageBits)
+DEFINE_CAPI(int32_t, OversampleBits, oversampleBits)
+DEFINE_CAPI(double, Voltage, voltage)
+DEFINE_CAPI(HAL_Bool, AccumulatorInitialized, accumulatorInitialized)
+DEFINE_CAPI(int64_t, AccumulatorValue, accumulatorValue)
+DEFINE_CAPI(int64_t, AccumulatorCount, accumulatorCount)
+DEFINE_CAPI(int32_t, AccumulatorCenter, accumulatorCenter)
+DEFINE_CAPI(int32_t, AccumulatorDeadband, accumulatorDeadband)
+
+#define REGISTER(NAME) \
+ SimAnalogInData[index].NAME.RegisterCallback(callback, param, initialNotify)
+
+void HALSIM_RegisterAnalogInAllCallbacks(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param, HAL_Bool initialNotify) {
+ REGISTER(initialized);
+ REGISTER(averageBits);
+ REGISTER(oversampleBits);
+ REGISTER(voltage);
+ REGISTER(accumulatorInitialized);
+ REGISTER(accumulatorValue);
+ REGISTER(accumulatorCount);
+ REGISTER(accumulatorCenter);
+ REGISTER(accumulatorDeadband);
+}
+} // extern "C"
diff --git a/hal/src/main/native/sim/mockdata/AnalogInDataInternal.h b/hal/src/main/native/sim/mockdata/AnalogInDataInternal.h
new file mode 100644
index 0000000..cd8348d
--- /dev/null
+++ b/hal/src/main/native/sim/mockdata/AnalogInDataInternal.h
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "mockdata/AnalogInData.h"
+#include "mockdata/SimDataValue.h"
+
+namespace hal {
+class AnalogInData {
+ HAL_SIMDATAVALUE_DEFINE_NAME(Initialized)
+ HAL_SIMDATAVALUE_DEFINE_NAME(AverageBits)
+ HAL_SIMDATAVALUE_DEFINE_NAME(OversampleBits)
+ HAL_SIMDATAVALUE_DEFINE_NAME(Voltage)
+ HAL_SIMDATAVALUE_DEFINE_NAME(AccumulatorInitialized)
+ HAL_SIMDATAVALUE_DEFINE_NAME(AccumulatorValue)
+ HAL_SIMDATAVALUE_DEFINE_NAME(AccumulatorCount)
+ HAL_SIMDATAVALUE_DEFINE_NAME(AccumulatorCenter)
+ HAL_SIMDATAVALUE_DEFINE_NAME(AccumulatorDeadband)
+
+ public:
+ SimDataValue<HAL_Bool, HAL_MakeBoolean, GetInitializedName> initialized{
+ false};
+ std::atomic<HAL_SimDeviceHandle> simDevice;
+ SimDataValue<int32_t, HAL_MakeInt, GetAverageBitsName> averageBits{7};
+ SimDataValue<int32_t, HAL_MakeInt, GetOversampleBitsName> oversampleBits{0};
+ SimDataValue<double, HAL_MakeDouble, GetVoltageName> voltage{0.0};
+ SimDataValue<HAL_Bool, HAL_MakeBoolean, GetAccumulatorInitializedName>
+ accumulatorInitialized{false};
+ SimDataValue<int64_t, HAL_MakeLong, GetAccumulatorValueName> accumulatorValue{
+ 0};
+ SimDataValue<int64_t, HAL_MakeLong, GetAccumulatorCountName> accumulatorCount{
+ 0};
+ SimDataValue<int32_t, HAL_MakeInt, GetAccumulatorCenterName>
+ accumulatorCenter{0};
+ SimDataValue<int32_t, HAL_MakeInt, GetAccumulatorDeadbandName>
+ accumulatorDeadband{0};
+
+ virtual void ResetData();
+};
+extern AnalogInData* SimAnalogInData;
+} // namespace hal
diff --git a/hal/src/main/native/sim/mockdata/AnalogOutData.cpp b/hal/src/main/native/sim/mockdata/AnalogOutData.cpp
new file mode 100644
index 0000000..d4b9116
--- /dev/null
+++ b/hal/src/main/native/sim/mockdata/AnalogOutData.cpp
@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 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 "../PortsInternal.h"
+#include "AnalogOutDataInternal.h"
+
+using namespace hal;
+
+namespace hal {
+namespace init {
+void InitializeAnalogOutData() {
+ static AnalogOutData siod[kNumAnalogOutputs];
+ ::hal::SimAnalogOutData = siod;
+}
+} // namespace init
+} // namespace hal
+
+AnalogOutData* hal::SimAnalogOutData;
+void AnalogOutData::ResetData() {
+ voltage.Reset(0.0);
+ initialized.Reset(0);
+}
+
+extern "C" {
+void HALSIM_ResetAnalogOutData(int32_t index) {
+ SimAnalogOutData[index].ResetData();
+}
+
+#define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME) \
+ HAL_SIMDATAVALUE_DEFINE_CAPI(TYPE, HALSIM, AnalogOut##CAPINAME, \
+ SimAnalogOutData, LOWERNAME)
+
+DEFINE_CAPI(double, Voltage, voltage)
+DEFINE_CAPI(HAL_Bool, Initialized, initialized)
+
+#define REGISTER(NAME) \
+ SimAnalogOutData[index].NAME.RegisterCallback(callback, param, initialNotify)
+
+void HALSIM_RegisterAnalogOutAllCallbacks(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param, HAL_Bool initialNotify) {
+ REGISTER(voltage);
+ REGISTER(initialized);
+}
+} // extern "C"
diff --git a/hal/src/main/native/sim/mockdata/AnalogOutDataInternal.h b/hal/src/main/native/sim/mockdata/AnalogOutDataInternal.h
new file mode 100644
index 0000000..da7d49f
--- /dev/null
+++ b/hal/src/main/native/sim/mockdata/AnalogOutDataInternal.h
@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "mockdata/AnalogOutData.h"
+#include "mockdata/SimDataValue.h"
+
+namespace hal {
+class AnalogOutData {
+ HAL_SIMDATAVALUE_DEFINE_NAME(Voltage)
+ HAL_SIMDATAVALUE_DEFINE_NAME(Initialized)
+
+ public:
+ SimDataValue<double, HAL_MakeDouble, GetVoltageName> voltage{0.0};
+ SimDataValue<HAL_Bool, HAL_MakeBoolean, GetInitializedName> initialized{0};
+
+ virtual void ResetData();
+};
+extern AnalogOutData* SimAnalogOutData;
+} // namespace hal
diff --git a/hal/src/main/native/sim/mockdata/AnalogTriggerData.cpp b/hal/src/main/native/sim/mockdata/AnalogTriggerData.cpp
new file mode 100644
index 0000000..64d1a97
--- /dev/null
+++ b/hal/src/main/native/sim/mockdata/AnalogTriggerData.cpp
@@ -0,0 +1,57 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 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 "../PortsInternal.h"
+#include "AnalogTriggerDataInternal.h"
+
+using namespace hal;
+
+namespace hal {
+namespace init {
+void InitializeAnalogTriggerData() {
+ static AnalogTriggerData satd[kNumAnalogTriggers];
+ ::hal::SimAnalogTriggerData = satd;
+}
+} // namespace init
+} // namespace hal
+
+AnalogTriggerData* hal::SimAnalogTriggerData;
+void AnalogTriggerData::ResetData() {
+ initialized.Reset(0);
+ triggerLowerBound.Reset(0);
+ triggerUpperBound.Reset(0);
+ triggerMode.Reset(static_cast<HALSIM_AnalogTriggerMode>(0));
+}
+
+extern "C" {
+void HALSIM_ResetAnalogTriggerData(int32_t index) {
+ SimAnalogTriggerData[index].ResetData();
+}
+
+#define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME) \
+ HAL_SIMDATAVALUE_DEFINE_CAPI(TYPE, HALSIM, AnalogTrigger##CAPINAME, \
+ SimAnalogTriggerData, LOWERNAME)
+
+DEFINE_CAPI(HAL_Bool, Initialized, initialized)
+DEFINE_CAPI(double, TriggerLowerBound, triggerLowerBound)
+DEFINE_CAPI(double, TriggerUpperBound, triggerUpperBound)
+DEFINE_CAPI(HALSIM_AnalogTriggerMode, TriggerMode, triggerMode)
+
+#define REGISTER(NAME) \
+ SimAnalogTriggerData[index].NAME.RegisterCallback(callback, param, \
+ initialNotify)
+
+void HALSIM_RegisterAnalogTriggerAllCallbacks(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify) {
+ REGISTER(initialized);
+ REGISTER(triggerLowerBound);
+ REGISTER(triggerUpperBound);
+ REGISTER(triggerMode);
+}
+} // extern "C"
diff --git a/hal/src/main/native/sim/mockdata/AnalogTriggerDataInternal.h b/hal/src/main/native/sim/mockdata/AnalogTriggerDataInternal.h
new file mode 100644
index 0000000..61b3c19
--- /dev/null
+++ b/hal/src/main/native/sim/mockdata/AnalogTriggerDataInternal.h
@@ -0,0 +1,38 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "mockdata/AnalogTriggerData.h"
+#include "mockdata/SimDataValue.h"
+
+namespace hal {
+class AnalogTriggerData {
+ HAL_SIMDATAVALUE_DEFINE_NAME(Initialized)
+ HAL_SIMDATAVALUE_DEFINE_NAME(TriggerLowerBound)
+ HAL_SIMDATAVALUE_DEFINE_NAME(TriggerUpperBound)
+ HAL_SIMDATAVALUE_DEFINE_NAME(TriggerMode)
+
+ static LLVM_ATTRIBUTE_ALWAYS_INLINE HAL_Value
+ MakeTriggerModeValue(HALSIM_AnalogTriggerMode value) {
+ return HAL_MakeEnum(value);
+ }
+
+ public:
+ SimDataValue<HAL_Bool, HAL_MakeBoolean, GetInitializedName> initialized{0};
+ SimDataValue<double, HAL_MakeDouble, GetTriggerLowerBoundName>
+ triggerLowerBound{0};
+ SimDataValue<double, HAL_MakeDouble, GetTriggerUpperBoundName>
+ triggerUpperBound{0};
+ SimDataValue<HALSIM_AnalogTriggerMode, MakeTriggerModeValue,
+ GetTriggerModeName>
+ triggerMode{static_cast<HALSIM_AnalogTriggerMode>(0)};
+
+ virtual void ResetData();
+};
+extern AnalogTriggerData* SimAnalogTriggerData;
+} // namespace hal
diff --git a/hal/src/main/native/sim/mockdata/CanDataInternal.cpp b/hal/src/main/native/sim/mockdata/CanDataInternal.cpp
new file mode 100644
index 0000000..e37d865
--- /dev/null
+++ b/hal/src/main/native/sim/mockdata/CanDataInternal.cpp
@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 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 "CanDataInternal.h"
+
+using namespace hal;
+
+namespace hal {
+namespace init {
+void InitializeCanData() {
+ static CanData scd;
+ ::hal::SimCanData = &scd;
+}
+} // namespace init
+} // namespace hal
+
+CanData* hal::SimCanData;
+
+void CanData::ResetData() {
+ sendMessage.Reset();
+ receiveMessage.Reset();
+ openStreamSession.Reset();
+ closeStreamSession.Reset();
+ readStreamSession.Reset();
+ getCANStatus.Reset();
+}
+
+extern "C" {
+
+void HALSIM_ResetCanData(void) { SimCanData->ResetData(); }
+
+#define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME) \
+ HAL_SIMCALLBACKREGISTRY_DEFINE_CAPI_NOINDEX(TYPE, HALSIM, Can##CAPINAME, \
+ SimCanData, LOWERNAME)
+
+DEFINE_CAPI(HAL_CAN_SendMessageCallback, SendMessage, sendMessage)
+DEFINE_CAPI(HAL_CAN_ReceiveMessageCallback, ReceiveMessage, receiveMessage)
+DEFINE_CAPI(HAL_CAN_OpenStreamSessionCallback, OpenStream, openStreamSession)
+DEFINE_CAPI(HAL_CAN_CloseStreamSessionCallback, CloseStream, closeStreamSession)
+DEFINE_CAPI(HAL_CAN_ReadStreamSessionCallback, ReadStream, readStreamSession)
+DEFINE_CAPI(HAL_CAN_GetCANStatusCallback, GetCANStatus, getCANStatus)
+
+} // extern "C"
diff --git a/hal/src/main/native/sim/mockdata/CanDataInternal.h b/hal/src/main/native/sim/mockdata/CanDataInternal.h
new file mode 100644
index 0000000..ffdd351
--- /dev/null
+++ b/hal/src/main/native/sim/mockdata/CanDataInternal.h
@@ -0,0 +1,42 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 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 "mockdata/CanData.h"
+#include "mockdata/SimCallbackRegistry.h"
+
+namespace hal {
+
+class CanData {
+ HAL_SIMCALLBACKREGISTRY_DEFINE_NAME(SendMessage)
+ HAL_SIMCALLBACKREGISTRY_DEFINE_NAME(ReceiveMessage)
+ HAL_SIMCALLBACKREGISTRY_DEFINE_NAME(OpenStream)
+ HAL_SIMCALLBACKREGISTRY_DEFINE_NAME(CloseStream)
+ HAL_SIMCALLBACKREGISTRY_DEFINE_NAME(ReadStream)
+ HAL_SIMCALLBACKREGISTRY_DEFINE_NAME(GetCanStatus)
+
+ public:
+ SimCallbackRegistry<HAL_CAN_SendMessageCallback, GetSendMessageName>
+ sendMessage;
+ SimCallbackRegistry<HAL_CAN_ReceiveMessageCallback, GetReceiveMessageName>
+ receiveMessage;
+ SimCallbackRegistry<HAL_CAN_OpenStreamSessionCallback, GetOpenStreamName>
+ openStreamSession;
+ SimCallbackRegistry<HAL_CAN_CloseStreamSessionCallback, GetCloseStreamName>
+ closeStreamSession;
+ SimCallbackRegistry<HAL_CAN_ReadStreamSessionCallback, GetReadStreamName>
+ readStreamSession;
+ SimCallbackRegistry<HAL_CAN_GetCANStatusCallback, GetGetCanStatusName>
+ getCANStatus;
+
+ void ResetData();
+};
+
+extern CanData* SimCanData;
+
+} // namespace hal
diff --git a/hal/src/main/native/sim/mockdata/DIOData.cpp b/hal/src/main/native/sim/mockdata/DIOData.cpp
new file mode 100644
index 0000000..a9c61fd
--- /dev/null
+++ b/hal/src/main/native/sim/mockdata/DIOData.cpp
@@ -0,0 +1,60 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "../PortsInternal.h"
+#include "DIODataInternal.h"
+
+using namespace hal;
+
+namespace hal {
+namespace init {
+void InitializeDIOData() {
+ static DIOData sdd[kNumDigitalChannels];
+ ::hal::SimDIOData = sdd;
+}
+} // namespace init
+} // namespace hal
+
+DIOData* hal::SimDIOData;
+void DIOData::ResetData() {
+ initialized.Reset(false);
+ simDevice = 0;
+ value.Reset(true);
+ pulseLength.Reset(0.0);
+ isInput.Reset(true);
+ filterIndex.Reset(-1);
+}
+
+extern "C" {
+void HALSIM_ResetDIOData(int32_t index) { SimDIOData[index].ResetData(); }
+
+HAL_SimDeviceHandle HALSIM_GetDIOSimDevice(int32_t index) {
+ return SimDIOData[index].simDevice;
+}
+
+#define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME) \
+ HAL_SIMDATAVALUE_DEFINE_CAPI(TYPE, HALSIM, DIO##CAPINAME, SimDIOData, \
+ LOWERNAME)
+
+DEFINE_CAPI(HAL_Bool, Initialized, initialized)
+DEFINE_CAPI(HAL_Bool, Value, value)
+DEFINE_CAPI(double, PulseLength, pulseLength)
+DEFINE_CAPI(HAL_Bool, IsInput, isInput)
+DEFINE_CAPI(int32_t, FilterIndex, filterIndex)
+
+#define REGISTER(NAME) \
+ SimDIOData[index].NAME.RegisterCallback(callback, param, initialNotify)
+
+void HALSIM_RegisterDIOAllCallbacks(int32_t index, HAL_NotifyCallback callback,
+ void* param, HAL_Bool initialNotify) {
+ REGISTER(initialized);
+ REGISTER(value);
+ REGISTER(pulseLength);
+ REGISTER(isInput);
+ REGISTER(filterIndex);
+}
+} // extern "C"
diff --git a/hal/src/main/native/sim/mockdata/DIODataInternal.h b/hal/src/main/native/sim/mockdata/DIODataInternal.h
new file mode 100644
index 0000000..49e0f75
--- /dev/null
+++ b/hal/src/main/native/sim/mockdata/DIODataInternal.h
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "mockdata/DIOData.h"
+#include "mockdata/SimDataValue.h"
+
+namespace hal {
+class DIOData {
+ HAL_SIMDATAVALUE_DEFINE_NAME(Initialized)
+ HAL_SIMDATAVALUE_DEFINE_NAME(Value)
+ HAL_SIMDATAVALUE_DEFINE_NAME(PulseLength)
+ HAL_SIMDATAVALUE_DEFINE_NAME(IsInput)
+ HAL_SIMDATAVALUE_DEFINE_NAME(FilterIndex)
+
+ public:
+ SimDataValue<HAL_Bool, HAL_MakeBoolean, GetInitializedName> initialized{
+ false};
+ std::atomic<HAL_SimDeviceHandle> simDevice;
+ SimDataValue<HAL_Bool, HAL_MakeBoolean, GetValueName> value{true};
+ SimDataValue<double, HAL_MakeDouble, GetPulseLengthName> pulseLength{0.0};
+ SimDataValue<HAL_Bool, HAL_MakeBoolean, GetIsInputName> isInput{true};
+ SimDataValue<int32_t, HAL_MakeInt, GetFilterIndexName> filterIndex{-1};
+
+ virtual void ResetData();
+};
+extern DIOData* SimDIOData;
+} // namespace hal
diff --git a/hal/src/main/native/sim/mockdata/DigitalPWMData.cpp b/hal/src/main/native/sim/mockdata/DigitalPWMData.cpp
new file mode 100644
index 0000000..78ee749
--- /dev/null
+++ b/hal/src/main/native/sim/mockdata/DigitalPWMData.cpp
@@ -0,0 +1,53 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 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 "../PortsInternal.h"
+#include "DigitalPWMDataInternal.h"
+
+using namespace hal;
+
+namespace hal {
+namespace init {
+void InitializeDigitalPWMData() {
+ static DigitalPWMData sdpd[kNumDigitalPWMOutputs];
+ ::hal::SimDigitalPWMData = sdpd;
+}
+} // namespace init
+} // namespace hal
+
+DigitalPWMData* hal::SimDigitalPWMData;
+void DigitalPWMData::ResetData() {
+ initialized.Reset(false);
+ dutyCycle.Reset(0.0);
+ pin.Reset(0);
+}
+
+extern "C" {
+void HALSIM_ResetDigitalPWMData(int32_t index) {
+ SimDigitalPWMData[index].ResetData();
+}
+
+#define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME) \
+ HAL_SIMDATAVALUE_DEFINE_CAPI(TYPE, HALSIM, DigitalPWM##CAPINAME, \
+ SimDigitalPWMData, LOWERNAME)
+
+DEFINE_CAPI(HAL_Bool, Initialized, initialized)
+DEFINE_CAPI(double, DutyCycle, dutyCycle)
+DEFINE_CAPI(int32_t, Pin, pin)
+
+#define REGISTER(NAME) \
+ SimDigitalPWMData[index].NAME.RegisterCallback(callback, param, initialNotify)
+
+void HALSIM_RegisterDigitalPWMAllCallbacks(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify) {
+ REGISTER(initialized);
+ REGISTER(dutyCycle);
+ REGISTER(pin);
+}
+} // extern "C"
diff --git a/hal/src/main/native/sim/mockdata/DigitalPWMDataInternal.h b/hal/src/main/native/sim/mockdata/DigitalPWMDataInternal.h
new file mode 100644
index 0000000..09d5903
--- /dev/null
+++ b/hal/src/main/native/sim/mockdata/DigitalPWMDataInternal.h
@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "mockdata/DigitalPWMData.h"
+#include "mockdata/SimDataValue.h"
+
+namespace hal {
+class DigitalPWMData {
+ HAL_SIMDATAVALUE_DEFINE_NAME(Initialized)
+ HAL_SIMDATAVALUE_DEFINE_NAME(DutyCycle)
+ HAL_SIMDATAVALUE_DEFINE_NAME(Pin)
+
+ public:
+ SimDataValue<HAL_Bool, HAL_MakeBoolean, GetInitializedName> initialized{
+ false};
+ SimDataValue<double, HAL_MakeDouble, GetDutyCycleName> dutyCycle{0.0};
+ SimDataValue<int32_t, HAL_MakeInt, GetPinName> pin{0};
+
+ virtual void ResetData();
+};
+extern DigitalPWMData* SimDigitalPWMData;
+} // namespace hal
diff --git a/hal/src/main/native/sim/mockdata/DriverStationData.cpp b/hal/src/main/native/sim/mockdata/DriverStationData.cpp
new file mode 100644
index 0000000..ece98c8
--- /dev/null
+++ b/hal/src/main/native/sim/mockdata/DriverStationData.cpp
@@ -0,0 +1,209 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 <cstdlib>
+#include <cstring>
+#include <string>
+
+#include "DriverStationDataInternal.h"
+#include "hal/DriverStation.h"
+
+namespace hal {
+struct JoystickOutputStore {
+ int64_t outputs = 0;
+ int32_t leftRumble = 0;
+ int32_t rightRumble = 0;
+};
+} // namespace hal
+
+using namespace hal;
+
+namespace hal {
+namespace init {
+void InitializeDriverStationData() {
+ static DriverStationData dsd;
+ ::hal::SimDriverStationData = &dsd;
+}
+} // namespace init
+} // namespace hal
+
+DriverStationData* hal::SimDriverStationData;
+
+DriverStationData::DriverStationData() { ResetData(); }
+
+void DriverStationData::ResetData() {
+ enabled.Reset(false);
+ autonomous.Reset(false);
+ test.Reset(false);
+ eStop.Reset(false);
+ fmsAttached.Reset(false);
+ dsAttached.Reset(true);
+ allianceStationId.Reset(static_cast<HAL_AllianceStationID>(0));
+ matchTime.Reset(0.0);
+
+ {
+ std::scoped_lock lock(m_joystickDataMutex);
+ m_joystickAxes = std::make_unique<HAL_JoystickAxes[]>(6);
+ m_joystickPOVs = std::make_unique<HAL_JoystickPOVs[]>(6);
+ m_joystickButtons = std::make_unique<HAL_JoystickButtons[]>(6);
+ m_joystickOutputs = std::make_unique<JoystickOutputStore[]>(6);
+ m_joystickDescriptor = std::make_unique<HAL_JoystickDescriptor[]>(6);
+
+ for (int i = 0; i < 6; i++) {
+ m_joystickAxes[i].count = 0;
+ m_joystickPOVs[i].count = 0;
+ m_joystickButtons[i].count = 0;
+ m_joystickDescriptor[i].isXbox = 0;
+ m_joystickDescriptor[i].type = -1;
+ m_joystickDescriptor[i].name[0] = '\0';
+ }
+ }
+ {
+ std::scoped_lock lock(m_matchInfoMutex);
+
+ m_matchInfo = std::make_unique<HAL_MatchInfo>();
+ }
+}
+
+void DriverStationData::GetJoystickAxes(int32_t joystickNum,
+ HAL_JoystickAxes* axes) {
+ std::scoped_lock lock(m_joystickDataMutex);
+ *axes = m_joystickAxes[joystickNum];
+}
+void DriverStationData::GetJoystickPOVs(int32_t joystickNum,
+ HAL_JoystickPOVs* povs) {
+ std::scoped_lock lock(m_joystickDataMutex);
+ *povs = m_joystickPOVs[joystickNum];
+}
+void DriverStationData::GetJoystickButtons(int32_t joystickNum,
+ HAL_JoystickButtons* buttons) {
+ std::scoped_lock lock(m_joystickDataMutex);
+ *buttons = m_joystickButtons[joystickNum];
+}
+void DriverStationData::GetJoystickDescriptor(
+ int32_t joystickNum, HAL_JoystickDescriptor* descriptor) {
+ std::scoped_lock lock(m_joystickDataMutex);
+ *descriptor = m_joystickDescriptor[joystickNum];
+ // Always ensure name is null terminated
+ descriptor->name[255] = '\0';
+}
+void DriverStationData::GetJoystickOutputs(int32_t joystickNum,
+ int64_t* outputs,
+ int32_t* leftRumble,
+ int32_t* rightRumble) {
+ std::scoped_lock lock(m_joystickDataMutex);
+ *leftRumble = m_joystickOutputs[joystickNum].leftRumble;
+ *outputs = m_joystickOutputs[joystickNum].outputs;
+ *rightRumble = m_joystickOutputs[joystickNum].rightRumble;
+}
+void DriverStationData::GetMatchInfo(HAL_MatchInfo* info) {
+ std::scoped_lock lock(m_matchInfoMutex);
+ *info = *m_matchInfo;
+}
+
+void DriverStationData::SetJoystickAxes(int32_t joystickNum,
+ const HAL_JoystickAxes* axes) {
+ std::scoped_lock lock(m_joystickDataMutex);
+ m_joystickAxes[joystickNum] = *axes;
+}
+void DriverStationData::SetJoystickPOVs(int32_t joystickNum,
+ const HAL_JoystickPOVs* povs) {
+ std::scoped_lock lock(m_joystickDataMutex);
+ m_joystickPOVs[joystickNum] = *povs;
+}
+void DriverStationData::SetJoystickButtons(int32_t joystickNum,
+ const HAL_JoystickButtons* buttons) {
+ std::scoped_lock lock(m_joystickDataMutex);
+ m_joystickButtons[joystickNum] = *buttons;
+}
+
+void DriverStationData::SetJoystickDescriptor(
+ int32_t joystickNum, const HAL_JoystickDescriptor* descriptor) {
+ std::scoped_lock lock(m_joystickDataMutex);
+ m_joystickDescriptor[joystickNum] = *descriptor;
+}
+
+void DriverStationData::SetJoystickOutputs(int32_t joystickNum, int64_t outputs,
+ int32_t leftRumble,
+ int32_t rightRumble) {
+ std::scoped_lock lock(m_joystickDataMutex);
+ m_joystickOutputs[joystickNum].leftRumble = leftRumble;
+ m_joystickOutputs[joystickNum].outputs = outputs;
+ m_joystickOutputs[joystickNum].rightRumble = rightRumble;
+}
+
+void DriverStationData::SetMatchInfo(const HAL_MatchInfo* info) {
+ std::scoped_lock lock(m_matchInfoMutex);
+ *m_matchInfo = *info;
+ *(std::end(m_matchInfo->eventName) - 1) = '\0';
+}
+
+void DriverStationData::NotifyNewData() { HAL_ReleaseDSMutex(); }
+
+extern "C" {
+void HALSIM_ResetDriverStationData(void) { SimDriverStationData->ResetData(); }
+
+#define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME) \
+ HAL_SIMDATAVALUE_DEFINE_CAPI_NOINDEX(TYPE, HALSIM, DriverStation##CAPINAME, \
+ SimDriverStationData, LOWERNAME)
+
+DEFINE_CAPI(HAL_Bool, Enabled, enabled)
+DEFINE_CAPI(HAL_Bool, Autonomous, autonomous)
+DEFINE_CAPI(HAL_Bool, Test, test)
+DEFINE_CAPI(HAL_Bool, EStop, eStop)
+DEFINE_CAPI(HAL_Bool, FmsAttached, fmsAttached)
+DEFINE_CAPI(HAL_Bool, DsAttached, dsAttached)
+DEFINE_CAPI(HAL_AllianceStationID, AllianceStationId, allianceStationId)
+DEFINE_CAPI(double, MatchTime, matchTime)
+
+void HALSIM_SetJoystickAxes(int32_t joystickNum, const HAL_JoystickAxes* axes) {
+ SimDriverStationData->SetJoystickAxes(joystickNum, axes);
+}
+
+void HALSIM_SetJoystickPOVs(int32_t joystickNum, const HAL_JoystickPOVs* povs) {
+ SimDriverStationData->SetJoystickPOVs(joystickNum, povs);
+}
+
+void HALSIM_SetJoystickButtons(int32_t joystickNum,
+ const HAL_JoystickButtons* buttons) {
+ SimDriverStationData->SetJoystickButtons(joystickNum, buttons);
+}
+void HALSIM_SetJoystickDescriptor(int32_t joystickNum,
+ const HAL_JoystickDescriptor* descriptor) {
+ SimDriverStationData->SetJoystickDescriptor(joystickNum, descriptor);
+}
+
+void HALSIM_GetJoystickOutputs(int32_t joystickNum, int64_t* outputs,
+ int32_t* leftRumble, int32_t* rightRumble) {
+ SimDriverStationData->GetJoystickOutputs(joystickNum, outputs, leftRumble,
+ rightRumble);
+}
+
+void HALSIM_SetMatchInfo(const HAL_MatchInfo* info) {
+ SimDriverStationData->SetMatchInfo(info);
+}
+
+void HALSIM_NotifyDriverStationNewData(void) {
+ SimDriverStationData->NotifyNewData();
+}
+
+#define REGISTER(NAME) \
+ SimDriverStationData->NAME.RegisterCallback(callback, param, initialNotify)
+
+void HALSIM_RegisterDriverStationAllCallbacks(HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify) {
+ REGISTER(enabled);
+ REGISTER(autonomous);
+ REGISTER(test);
+ REGISTER(eStop);
+ REGISTER(fmsAttached);
+ REGISTER(dsAttached);
+ REGISTER(allianceStationId);
+ REGISTER(matchTime);
+}
+} // extern "C"
diff --git a/hal/src/main/native/sim/mockdata/DriverStationDataInternal.h b/hal/src/main/native/sim/mockdata/DriverStationDataInternal.h
new file mode 100644
index 0000000..fdeb3c6
--- /dev/null
+++ b/hal/src/main/native/sim/mockdata/DriverStationDataInternal.h
@@ -0,0 +1,86 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 <memory>
+
+#include <wpi/spinlock.h>
+
+#include "mockdata/DriverStationData.h"
+#include "mockdata/SimDataValue.h"
+
+namespace hal {
+struct JoystickOutputStore;
+
+class DriverStationData {
+ HAL_SIMDATAVALUE_DEFINE_NAME(Enabled)
+ HAL_SIMDATAVALUE_DEFINE_NAME(Autonomous)
+ HAL_SIMDATAVALUE_DEFINE_NAME(Test)
+ HAL_SIMDATAVALUE_DEFINE_NAME(EStop)
+ HAL_SIMDATAVALUE_DEFINE_NAME(FmsAttached)
+ HAL_SIMDATAVALUE_DEFINE_NAME(DsAttached)
+ HAL_SIMDATAVALUE_DEFINE_NAME(AllianceStationId)
+ HAL_SIMDATAVALUE_DEFINE_NAME(MatchTime)
+
+ static LLVM_ATTRIBUTE_ALWAYS_INLINE HAL_Value
+ MakeAllianceStationIdValue(HAL_AllianceStationID value) {
+ return HAL_MakeEnum(value);
+ }
+
+ public:
+ DriverStationData();
+ void ResetData();
+
+ void GetJoystickAxes(int32_t joystickNum, HAL_JoystickAxes* axes);
+ void GetJoystickPOVs(int32_t joystickNum, HAL_JoystickPOVs* povs);
+ void GetJoystickButtons(int32_t joystickNum, HAL_JoystickButtons* buttons);
+ void GetJoystickDescriptor(int32_t joystickNum,
+ HAL_JoystickDescriptor* descriptor);
+ void GetJoystickOutputs(int32_t joystickNum, int64_t* outputs,
+ int32_t* leftRumble, int32_t* rightRumble);
+ void GetMatchInfo(HAL_MatchInfo* info);
+ void FreeMatchInfo(const HAL_MatchInfo* info);
+
+ void SetJoystickAxes(int32_t joystickNum, const HAL_JoystickAxes* axes);
+ void SetJoystickPOVs(int32_t joystickNum, const HAL_JoystickPOVs* povs);
+ void SetJoystickButtons(int32_t joystickNum,
+ const HAL_JoystickButtons* buttons);
+ void SetJoystickDescriptor(int32_t joystickNum,
+ const HAL_JoystickDescriptor* descriptor);
+ void SetJoystickOutputs(int32_t joystickNum, int64_t outputs,
+ int32_t leftRumble, int32_t rightRumble);
+ void SetMatchInfo(const HAL_MatchInfo* info);
+
+ void NotifyNewData();
+
+ SimDataValue<HAL_Bool, HAL_MakeBoolean, GetEnabledName> enabled{false};
+ SimDataValue<HAL_Bool, HAL_MakeBoolean, GetAutonomousName> autonomous{false};
+ SimDataValue<HAL_Bool, HAL_MakeBoolean, GetTestName> test{false};
+ SimDataValue<HAL_Bool, HAL_MakeBoolean, GetEStopName> eStop{false};
+ SimDataValue<HAL_Bool, HAL_MakeBoolean, GetFmsAttachedName> fmsAttached{
+ false};
+ SimDataValue<HAL_Bool, HAL_MakeBoolean, GetDsAttachedName> dsAttached{true};
+ SimDataValue<HAL_AllianceStationID, MakeAllianceStationIdValue,
+ GetAllianceStationIdName>
+ allianceStationId{static_cast<HAL_AllianceStationID>(0)};
+ SimDataValue<double, HAL_MakeDouble, GetMatchTimeName> matchTime{0.0};
+
+ private:
+ wpi::spinlock m_joystickDataMutex;
+ wpi::spinlock m_matchInfoMutex;
+
+ std::unique_ptr<HAL_JoystickAxes[]> m_joystickAxes;
+ std::unique_ptr<HAL_JoystickPOVs[]> m_joystickPOVs;
+ std::unique_ptr<HAL_JoystickButtons[]> m_joystickButtons;
+
+ std::unique_ptr<JoystickOutputStore[]> m_joystickOutputs;
+ std::unique_ptr<HAL_JoystickDescriptor[]> m_joystickDescriptor;
+ std::unique_ptr<HAL_MatchInfo> m_matchInfo;
+};
+extern DriverStationData* SimDriverStationData;
+} // namespace hal
diff --git a/hal/src/main/native/sim/mockdata/DutyCycleData.cpp b/hal/src/main/native/sim/mockdata/DutyCycleData.cpp
new file mode 100644
index 0000000..04806e0
--- /dev/null
+++ b/hal/src/main/native/sim/mockdata/DutyCycleData.cpp
@@ -0,0 +1,63 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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 "../PortsInternal.h"
+#include "DutyCycleDataInternal.h"
+
+using namespace hal;
+
+namespace hal {
+namespace init {
+void InitializeDutyCycleData() {
+ static DutyCycleData sed[kNumDutyCycles];
+ ::hal::SimDutyCycleData = sed;
+}
+} // namespace init
+} // namespace hal
+
+DutyCycleData* hal::SimDutyCycleData;
+
+void DutyCycleData::ResetData() {
+ digitalChannel = 0;
+ initialized.Reset(false);
+ simDevice = 0;
+ frequency.Reset(0);
+ output.Reset(0);
+}
+
+extern "C" {
+void HALSIM_ResetDutyCycleData(int32_t index) {
+ SimDutyCycleData[index].ResetData();
+}
+int32_t HALSIM_GetDutyCycleDigitalChannel(int32_t index) {
+ return SimDutyCycleData[index].digitalChannel;
+}
+
+HAL_SimDeviceHandle HALSIM_GetDutyCycleSimDevice(int32_t index) {
+ return SimDutyCycleData[index].simDevice;
+}
+
+#define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME) \
+ HAL_SIMDATAVALUE_DEFINE_CAPI(TYPE, HALSIM, DutyCycle##CAPINAME, \
+ SimDutyCycleData, LOWERNAME)
+
+DEFINE_CAPI(HAL_Bool, Initialized, initialized)
+DEFINE_CAPI(int32_t, Frequency, frequency)
+DEFINE_CAPI(double, Output, output)
+
+#define REGISTER(NAME) \
+ SimDutyCycleData[index].NAME.RegisterCallback(callback, param, initialNotify)
+
+void HALSIM_RegisterDutyCycleAllCallbacks(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param, HAL_Bool initialNotify) {
+ REGISTER(initialized);
+ REGISTER(frequency);
+ REGISTER(output);
+}
+
+} // extern "C"
diff --git a/hal/src/main/native/sim/mockdata/DutyCycleDataInternal.h b/hal/src/main/native/sim/mockdata/DutyCycleDataInternal.h
new file mode 100644
index 0000000..2f98b07
--- /dev/null
+++ b/hal/src/main/native/sim/mockdata/DutyCycleDataInternal.h
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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 <atomic>
+#include <limits>
+
+#include "mockdata/DutyCycleData.h"
+#include "mockdata/SimDataValue.h"
+
+namespace hal {
+class DutyCycleData {
+ HAL_SIMDATAVALUE_DEFINE_NAME(Initialized)
+ HAL_SIMDATAVALUE_DEFINE_NAME(Output)
+ HAL_SIMDATAVALUE_DEFINE_NAME(Frequency)
+
+ public:
+ std::atomic<int32_t> digitalChannel{0};
+ SimDataValue<HAL_Bool, HAL_MakeBoolean, GetInitializedName> initialized{
+ false};
+ std::atomic<HAL_SimDeviceHandle> simDevice;
+ SimDataValue<int32_t, HAL_MakeInt, GetFrequencyName> frequency{0};
+ SimDataValue<double, HAL_MakeDouble, GetOutputName> output{0};
+
+ virtual void ResetData();
+};
+extern DutyCycleData* SimDutyCycleData;
+} // namespace hal
diff --git a/hal/src/main/native/sim/mockdata/EncoderData.cpp b/hal/src/main/native/sim/mockdata/EncoderData.cpp
new file mode 100644
index 0000000..33bd073
--- /dev/null
+++ b/hal/src/main/native/sim/mockdata/EncoderData.cpp
@@ -0,0 +1,85 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "../PortsInternal.h"
+#include "EncoderDataInternal.h"
+
+using namespace hal;
+
+namespace hal {
+namespace init {
+void InitializeEncoderData() {
+ static EncoderData sed[kNumEncoders];
+ ::hal::SimEncoderData = sed;
+}
+} // namespace init
+} // namespace hal
+
+EncoderData* hal::SimEncoderData;
+void EncoderData::ResetData() {
+ digitalChannelA = 0;
+ digitalChannelB = 0;
+ initialized.Reset(false);
+ simDevice = 0;
+ count.Reset(0);
+ period.Reset(std::numeric_limits<double>::max());
+ reset.Reset(false);
+ maxPeriod.Reset(0);
+ direction.Reset(false);
+ reverseDirection.Reset(false);
+ samplesToAverage.Reset(0);
+ distancePerPulse.Reset(1);
+}
+
+extern "C" {
+void HALSIM_ResetEncoderData(int32_t index) {
+ SimEncoderData[index].ResetData();
+}
+
+int32_t HALSIM_GetEncoderDigitalChannelA(int32_t index) {
+ return SimEncoderData[index].digitalChannelA;
+}
+
+int32_t HALSIM_GetEncoderDigitalChannelB(int32_t index) {
+ return SimEncoderData[index].digitalChannelB;
+}
+
+HAL_SimDeviceHandle HALSIM_GetEncoderSimDevice(int32_t index) {
+ return SimEncoderData[index].simDevice;
+}
+
+#define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME) \
+ HAL_SIMDATAVALUE_DEFINE_CAPI(TYPE, HALSIM, Encoder##CAPINAME, \
+ SimEncoderData, LOWERNAME)
+
+DEFINE_CAPI(HAL_Bool, Initialized, initialized)
+DEFINE_CAPI(int32_t, Count, count)
+DEFINE_CAPI(double, Period, period)
+DEFINE_CAPI(HAL_Bool, Reset, reset)
+DEFINE_CAPI(double, MaxPeriod, maxPeriod)
+DEFINE_CAPI(HAL_Bool, Direction, direction)
+DEFINE_CAPI(HAL_Bool, ReverseDirection, reverseDirection)
+DEFINE_CAPI(int32_t, SamplesToAverage, samplesToAverage)
+DEFINE_CAPI(double, DistancePerPulse, distancePerPulse)
+
+#define REGISTER(NAME) \
+ SimEncoderData[index].NAME.RegisterCallback(callback, param, initialNotify)
+
+void HALSIM_RegisterEncoderAllCallbacks(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param, HAL_Bool initialNotify) {
+ REGISTER(initialized);
+ REGISTER(count);
+ REGISTER(period);
+ REGISTER(reset);
+ REGISTER(maxPeriod);
+ REGISTER(direction);
+ REGISTER(reverseDirection);
+ REGISTER(samplesToAverage);
+ REGISTER(distancePerPulse);
+}
+} // extern "C"
diff --git a/hal/src/main/native/sim/mockdata/EncoderDataInternal.h b/hal/src/main/native/sim/mockdata/EncoderDataInternal.h
new file mode 100644
index 0000000..389289c
--- /dev/null
+++ b/hal/src/main/native/sim/mockdata/EncoderDataInternal.h
@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 <atomic>
+#include <limits>
+
+#include "mockdata/EncoderData.h"
+#include "mockdata/SimDataValue.h"
+
+namespace hal {
+class EncoderData {
+ HAL_SIMDATAVALUE_DEFINE_NAME(Initialized)
+ HAL_SIMDATAVALUE_DEFINE_NAME(Count)
+ HAL_SIMDATAVALUE_DEFINE_NAME(Period)
+ HAL_SIMDATAVALUE_DEFINE_NAME(Reset)
+ HAL_SIMDATAVALUE_DEFINE_NAME(MaxPeriod)
+ HAL_SIMDATAVALUE_DEFINE_NAME(Direction)
+ HAL_SIMDATAVALUE_DEFINE_NAME(ReverseDirection)
+ HAL_SIMDATAVALUE_DEFINE_NAME(SamplesToAverage)
+ HAL_SIMDATAVALUE_DEFINE_NAME(DistancePerPulse)
+
+ public:
+ std::atomic<int32_t> digitalChannelA{0};
+ std::atomic<int32_t> digitalChannelB{0};
+ SimDataValue<HAL_Bool, HAL_MakeBoolean, GetInitializedName> initialized{
+ false};
+ std::atomic<HAL_SimDeviceHandle> simDevice;
+ SimDataValue<int32_t, HAL_MakeInt, GetCountName> count{0};
+ SimDataValue<double, HAL_MakeDouble, GetPeriodName> period{
+ (std::numeric_limits<double>::max)()};
+ SimDataValue<HAL_Bool, HAL_MakeBoolean, GetResetName> reset{false};
+ SimDataValue<double, HAL_MakeDouble, GetMaxPeriodName> maxPeriod{0};
+ SimDataValue<HAL_Bool, HAL_MakeBoolean, GetDirectionName> direction{false};
+ SimDataValue<HAL_Bool, HAL_MakeBoolean, GetReverseDirectionName>
+ reverseDirection{false};
+ SimDataValue<int32_t, HAL_MakeInt, GetSamplesToAverageName> samplesToAverage{
+ 0};
+ SimDataValue<double, HAL_MakeDouble, GetDistancePerPulseName>
+ distancePerPulse{1};
+
+ virtual void ResetData();
+};
+extern EncoderData* SimEncoderData;
+} // namespace hal
diff --git a/hal/src/main/native/sim/mockdata/I2CData.cpp b/hal/src/main/native/sim/mockdata/I2CData.cpp
new file mode 100644
index 0000000..b228c3b
--- /dev/null
+++ b/hal/src/main/native/sim/mockdata/I2CData.cpp
@@ -0,0 +1,58 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 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 <iostream>
+
+#include "../PortsInternal.h"
+#include "I2CDataInternal.h"
+
+using namespace hal;
+
+namespace hal {
+namespace init {
+void InitializeI2CData() {
+ static I2CData sid[2];
+ ::hal::SimI2CData = sid;
+}
+} // namespace init
+} // namespace hal
+
+I2CData* hal::SimI2CData;
+
+void I2CData::ResetData() {
+ initialized.Reset(false);
+ read.Reset();
+ write.Reset();
+}
+
+void I2CData::Write(int32_t deviceAddress, const uint8_t* dataToSend,
+ int32_t sendSize) {
+ write(dataToSend, sendSize);
+}
+
+void I2CData::Read(int32_t deviceAddress, uint8_t* buffer, int32_t count) {
+ read(buffer, count);
+}
+
+extern "C" {
+void HALSIM_ResetI2CData(int32_t index) { SimI2CData[index].ResetData(); }
+
+#define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME) \
+ HAL_SIMDATAVALUE_DEFINE_CAPI(TYPE, HALSIM, I2C##CAPINAME, SimI2CData, \
+ LOWERNAME)
+
+DEFINE_CAPI(HAL_Bool, Initialized, initialized)
+
+#undef DEFINE_CAPI
+#define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME) \
+ HAL_SIMCALLBACKREGISTRY_DEFINE_CAPI(TYPE, HALSIM, I2C##CAPINAME, SimI2CData, \
+ LOWERNAME)
+
+DEFINE_CAPI(HAL_BufferCallback, Read, read)
+DEFINE_CAPI(HAL_ConstBufferCallback, Write, write)
+
+} // extern "C"
diff --git a/hal/src/main/native/sim/mockdata/I2CDataInternal.h b/hal/src/main/native/sim/mockdata/I2CDataInternal.h
new file mode 100644
index 0000000..c799bb7
--- /dev/null
+++ b/hal/src/main/native/sim/mockdata/I2CDataInternal.h
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "mockdata/I2CData.h"
+#include "mockdata/SimCallbackRegistry.h"
+#include "mockdata/SimDataValue.h"
+
+namespace hal {
+class I2CData {
+ HAL_SIMDATAVALUE_DEFINE_NAME(Initialized)
+ HAL_SIMCALLBACKREGISTRY_DEFINE_NAME(Read)
+ HAL_SIMCALLBACKREGISTRY_DEFINE_NAME(Write)
+
+ public:
+ void Write(int32_t deviceAddress, const uint8_t* dataToSend,
+ int32_t sendSize);
+ void Read(int32_t deviceAddress, uint8_t* buffer, int32_t count);
+
+ SimDataValue<HAL_Bool, HAL_MakeBoolean, GetInitializedName> initialized{
+ false};
+ SimCallbackRegistry<HAL_BufferCallback, GetReadName> read;
+ SimCallbackRegistry<HAL_ConstBufferCallback, GetWriteName> write;
+
+ void ResetData();
+};
+extern I2CData* SimI2CData;
+} // namespace hal
diff --git a/hal/src/main/native/sim/mockdata/PCMData.cpp b/hal/src/main/native/sim/mockdata/PCMData.cpp
new file mode 100644
index 0000000..6193b05
--- /dev/null
+++ b/hal/src/main/native/sim/mockdata/PCMData.cpp
@@ -0,0 +1,90 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "../PortsInternal.h"
+#include "PCMDataInternal.h"
+
+using namespace hal;
+
+namespace hal {
+namespace init {
+void InitializePCMData() {
+ static PCMData spd[kNumPCMModules];
+ ::hal::SimPCMData = spd;
+}
+} // namespace init
+} // namespace hal
+
+PCMData* hal::SimPCMData;
+void PCMData::ResetData() {
+ for (int i = 0; i < kNumSolenoidChannels; i++) {
+ solenoidInitialized[i].Reset(false);
+ solenoidOutput[i].Reset(false);
+ }
+ compressorInitialized.Reset(false);
+ compressorOn.Reset(false);
+ closedLoopEnabled.Reset(true);
+ pressureSwitch.Reset(false);
+ compressorCurrent.Reset(0.0);
+}
+
+extern "C" {
+void HALSIM_ResetPCMData(int32_t index) { SimPCMData[index].ResetData(); }
+
+#define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME) \
+ HAL_SIMDATAVALUE_DEFINE_CAPI(TYPE, HALSIM, PCM##CAPINAME, SimPCMData, \
+ LOWERNAME)
+
+HAL_SIMDATAVALUE_DEFINE_CAPI_CHANNEL(HAL_Bool, HALSIM, PCMSolenoidInitialized,
+ SimPCMData, solenoidInitialized)
+HAL_SIMDATAVALUE_DEFINE_CAPI_CHANNEL(HAL_Bool, HALSIM, PCMSolenoidOutput,
+ SimPCMData, solenoidOutput)
+DEFINE_CAPI(HAL_Bool, CompressorInitialized, compressorInitialized)
+DEFINE_CAPI(HAL_Bool, CompressorOn, compressorOn)
+DEFINE_CAPI(HAL_Bool, ClosedLoopEnabled, closedLoopEnabled)
+DEFINE_CAPI(HAL_Bool, PressureSwitch, pressureSwitch)
+DEFINE_CAPI(double, CompressorCurrent, compressorCurrent)
+
+void HALSIM_GetPCMAllSolenoids(int32_t index, uint8_t* values) {
+ auto& data = SimPCMData[index].solenoidOutput;
+ uint8_t ret = 0;
+ for (int i = 0; i < kNumSolenoidChannels; i++) {
+ ret |= (data[i] << i);
+ }
+ *values = ret;
+}
+
+void HALSIM_SetPCMAllSolenoids(int32_t index, uint8_t values) {
+ auto& data = SimPCMData[index].solenoidOutput;
+ for (int i = 0; i < kNumSolenoidChannels; i++) {
+ data[i] = (values & 0x1) != 0;
+ values >>= 1;
+ }
+}
+
+#define REGISTER(NAME) \
+ SimPCMData[index].NAME.RegisterCallback(callback, param, initialNotify)
+
+void HALSIM_RegisterPCMAllNonSolenoidCallbacks(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify) {
+ REGISTER(compressorInitialized);
+ REGISTER(compressorOn);
+ REGISTER(closedLoopEnabled);
+ REGISTER(pressureSwitch);
+ REGISTER(compressorCurrent);
+}
+
+void HALSIM_RegisterPCMAllSolenoidCallbacks(int32_t index, int32_t channel,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify) {
+ REGISTER(solenoidInitialized[channel]);
+ REGISTER(solenoidOutput[channel]);
+}
+} // extern "C"
diff --git a/hal/src/main/native/sim/mockdata/PCMDataInternal.h b/hal/src/main/native/sim/mockdata/PCMDataInternal.h
new file mode 100644
index 0000000..0c7b99b
--- /dev/null
+++ b/hal/src/main/native/sim/mockdata/PCMDataInternal.h
@@ -0,0 +1,54 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "../PortsInternal.h"
+#include "mockdata/PCMData.h"
+#include "mockdata/SimDataValue.h"
+
+namespace hal {
+class PCMData {
+ HAL_SIMDATAVALUE_DEFINE_NAME(SolenoidInitialized)
+ HAL_SIMDATAVALUE_DEFINE_NAME(SolenoidOutput)
+ HAL_SIMDATAVALUE_DEFINE_NAME(CompressorInitialized)
+ HAL_SIMDATAVALUE_DEFINE_NAME(CompressorOn)
+ HAL_SIMDATAVALUE_DEFINE_NAME(ClosedLoopEnabled)
+ HAL_SIMDATAVALUE_DEFINE_NAME(PressureSwitch)
+ HAL_SIMDATAVALUE_DEFINE_NAME(CompressorCurrent)
+
+ static LLVM_ATTRIBUTE_ALWAYS_INLINE constexpr HAL_Bool
+ GetSolenoidInitializedDefault() {
+ return false;
+ }
+ static LLVM_ATTRIBUTE_ALWAYS_INLINE constexpr HAL_Bool
+ GetSolenoidOutputDefault() {
+ return false;
+ }
+
+ public:
+ SimDataValue<HAL_Bool, HAL_MakeBoolean, GetSolenoidInitializedName,
+ GetSolenoidInitializedDefault>
+ solenoidInitialized[kNumSolenoidChannels];
+ SimDataValue<HAL_Bool, HAL_MakeBoolean, GetSolenoidOutputName,
+ GetSolenoidOutputDefault>
+ solenoidOutput[kNumSolenoidChannels];
+ SimDataValue<HAL_Bool, HAL_MakeBoolean, GetCompressorInitializedName>
+ compressorInitialized{false};
+ SimDataValue<HAL_Bool, HAL_MakeBoolean, GetCompressorOnName> compressorOn{
+ false};
+ SimDataValue<HAL_Bool, HAL_MakeBoolean, GetClosedLoopEnabledName>
+ closedLoopEnabled{true};
+ SimDataValue<HAL_Bool, HAL_MakeBoolean, GetPressureSwitchName> pressureSwitch{
+ false};
+ SimDataValue<double, HAL_MakeDouble, GetCompressorCurrentName>
+ compressorCurrent{0.0};
+
+ virtual void ResetData();
+};
+extern PCMData* SimPCMData;
+} // namespace hal
diff --git a/hal/src/main/native/sim/mockdata/PDPData.cpp b/hal/src/main/native/sim/mockdata/PDPData.cpp
new file mode 100644
index 0000000..1c150bb
--- /dev/null
+++ b/hal/src/main/native/sim/mockdata/PDPData.cpp
@@ -0,0 +1,70 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "../PortsInternal.h"
+#include "PDPDataInternal.h"
+
+using namespace hal;
+
+namespace hal {
+namespace init {
+void InitializePDPData() {
+ static PDPData spd[kNumPDPModules];
+ ::hal::SimPDPData = spd;
+}
+} // namespace init
+} // namespace hal
+
+PDPData* hal::SimPDPData;
+void PDPData::ResetData() {
+ initialized.Reset(false);
+ temperature.Reset(0.0);
+ voltage.Reset(12.0);
+ for (int i = 0; i < kNumPDPChannels; i++) {
+ current[i].Reset(0.0);
+ }
+}
+
+extern "C" {
+void HALSIM_ResetPDPData(int32_t index) { SimPDPData[index].ResetData(); }
+
+#define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME) \
+ HAL_SIMDATAVALUE_DEFINE_CAPI(TYPE, HALSIM, PDP##CAPINAME, SimPDPData, \
+ LOWERNAME)
+
+DEFINE_CAPI(HAL_Bool, Initialized, initialized)
+DEFINE_CAPI(double, Temperature, temperature)
+DEFINE_CAPI(double, Voltage, voltage)
+HAL_SIMDATAVALUE_DEFINE_CAPI_CHANNEL(double, HALSIM, PDPCurrent, SimPDPData,
+ current)
+
+void HALSIM_GetPDPAllCurrents(int32_t index, double* currents) {
+ auto& data = SimPDPData[index].current;
+ for (int i = 0; i < kNumPDPChannels; i++) {
+ currents[i] = data[i];
+ }
+}
+
+void HALSIM_SetPDPAllCurrents(int32_t index, const double* currents) {
+ auto& data = SimPDPData[index].current;
+ for (int i = 0; i < kNumPDPChannels; i++) {
+ data[i] = currents[i];
+ }
+}
+
+#define REGISTER(NAME) \
+ SimPDPData[index].NAME.RegisterCallback(callback, param, initialNotify)
+
+void HALSIM_RegisterPDPAllNonCurrentCallbacks(int32_t index, int32_t channel,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify) {
+ REGISTER(initialized);
+ REGISTER(temperature);
+ REGISTER(voltage);
+}
+} // extern "C"
diff --git a/hal/src/main/native/sim/mockdata/PDPDataInternal.h b/hal/src/main/native/sim/mockdata/PDPDataInternal.h
new file mode 100644
index 0000000..8d45416
--- /dev/null
+++ b/hal/src/main/native/sim/mockdata/PDPDataInternal.h
@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "../PortsInternal.h"
+#include "mockdata/PDPData.h"
+#include "mockdata/SimDataValue.h"
+
+namespace hal {
+class PDPData {
+ HAL_SIMDATAVALUE_DEFINE_NAME(Initialized)
+ HAL_SIMDATAVALUE_DEFINE_NAME(Temperature)
+ HAL_SIMDATAVALUE_DEFINE_NAME(Voltage)
+ HAL_SIMDATAVALUE_DEFINE_NAME(Current)
+
+ static LLVM_ATTRIBUTE_ALWAYS_INLINE constexpr double GetCurrentDefault() {
+ return 0.0;
+ }
+
+ public:
+ SimDataValue<HAL_Bool, HAL_MakeBoolean, GetInitializedName> initialized{
+ false};
+ SimDataValue<double, HAL_MakeDouble, GetTemperatureName> temperature{0.0};
+ SimDataValue<double, HAL_MakeDouble, GetVoltageName> voltage{12.0};
+ SimDataValue<double, HAL_MakeDouble, GetCurrentName, GetCurrentDefault>
+ current[kNumPDPChannels];
+
+ virtual void ResetData();
+};
+extern PDPData* SimPDPData;
+} // namespace hal
diff --git a/hal/src/main/native/sim/mockdata/PWMData.cpp b/hal/src/main/native/sim/mockdata/PWMData.cpp
new file mode 100644
index 0000000..4d2121d
--- /dev/null
+++ b/hal/src/main/native/sim/mockdata/PWMData.cpp
@@ -0,0 +1,58 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 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 "../PortsInternal.h"
+#include "PWMDataInternal.h"
+
+using namespace hal;
+
+namespace hal {
+namespace init {
+void InitializePWMData() {
+ static PWMData spd[kNumPWMChannels];
+ ::hal::SimPWMData = spd;
+}
+} // namespace init
+} // namespace hal
+
+PWMData* hal::SimPWMData;
+void PWMData::ResetData() {
+ initialized.Reset(false);
+ rawValue.Reset(0);
+ speed.Reset(0);
+ position.Reset(0);
+ periodScale.Reset(0);
+ zeroLatch.Reset(false);
+}
+
+extern "C" {
+void HALSIM_ResetPWMData(int32_t index) { SimPWMData[index].ResetData(); }
+
+#define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME) \
+ HAL_SIMDATAVALUE_DEFINE_CAPI(TYPE, HALSIM, PWM##CAPINAME, SimPWMData, \
+ LOWERNAME)
+
+DEFINE_CAPI(HAL_Bool, Initialized, initialized)
+DEFINE_CAPI(int32_t, RawValue, rawValue)
+DEFINE_CAPI(double, Speed, speed)
+DEFINE_CAPI(double, Position, position)
+DEFINE_CAPI(int32_t, PeriodScale, periodScale)
+DEFINE_CAPI(HAL_Bool, ZeroLatch, zeroLatch)
+
+#define REGISTER(NAME) \
+ SimPWMData[index].NAME.RegisterCallback(callback, param, initialNotify)
+
+void HALSIM_RegisterPWMAllCallbacks(int32_t index, HAL_NotifyCallback callback,
+ void* param, HAL_Bool initialNotify) {
+ REGISTER(initialized);
+ REGISTER(rawValue);
+ REGISTER(speed);
+ REGISTER(position);
+ REGISTER(periodScale);
+ REGISTER(zeroLatch);
+}
+} // extern "C"
diff --git a/hal/src/main/native/sim/mockdata/PWMDataInternal.h b/hal/src/main/native/sim/mockdata/PWMDataInternal.h
new file mode 100644
index 0000000..248b7b3
--- /dev/null
+++ b/hal/src/main/native/sim/mockdata/PWMDataInternal.h
@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "mockdata/PWMData.h"
+#include "mockdata/SimDataValue.h"
+
+namespace hal {
+class PWMData {
+ HAL_SIMDATAVALUE_DEFINE_NAME(Initialized)
+ HAL_SIMDATAVALUE_DEFINE_NAME(RawValue)
+ HAL_SIMDATAVALUE_DEFINE_NAME(Speed)
+ HAL_SIMDATAVALUE_DEFINE_NAME(Position)
+ HAL_SIMDATAVALUE_DEFINE_NAME(PeriodScale)
+ HAL_SIMDATAVALUE_DEFINE_NAME(ZeroLatch)
+
+ public:
+ SimDataValue<HAL_Bool, HAL_MakeBoolean, GetInitializedName> initialized{
+ false};
+ SimDataValue<int32_t, HAL_MakeInt, GetRawValueName> rawValue{0};
+ SimDataValue<double, HAL_MakeDouble, GetSpeedName> speed{0};
+ SimDataValue<double, HAL_MakeDouble, GetPositionName> position{0};
+ SimDataValue<int32_t, HAL_MakeInt, GetPeriodScaleName> periodScale{0};
+ SimDataValue<HAL_Bool, HAL_MakeBoolean, GetZeroLatchName> zeroLatch{false};
+
+ virtual void ResetData();
+};
+extern PWMData* SimPWMData;
+} // namespace hal
diff --git a/hal/src/main/native/sim/mockdata/RelayData.cpp b/hal/src/main/native/sim/mockdata/RelayData.cpp
new file mode 100644
index 0000000..4623203
--- /dev/null
+++ b/hal/src/main/native/sim/mockdata/RelayData.cpp
@@ -0,0 +1,53 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 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 "../PortsInternal.h"
+#include "RelayDataInternal.h"
+
+using namespace hal;
+
+namespace hal {
+namespace init {
+void InitializeRelayData() {
+ static RelayData srd[kNumRelayHeaders];
+ ::hal::SimRelayData = srd;
+}
+} // namespace init
+} // namespace hal
+
+RelayData* hal::SimRelayData;
+void RelayData::ResetData() {
+ initializedForward.Reset(false);
+ initializedReverse.Reset(false);
+ forward.Reset(false);
+ reverse.Reset(false);
+}
+
+extern "C" {
+void HALSIM_ResetRelayData(int32_t index) { SimRelayData[index].ResetData(); }
+
+#define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME) \
+ HAL_SIMDATAVALUE_DEFINE_CAPI(TYPE, HALSIM, Relay##CAPINAME, SimRelayData, \
+ LOWERNAME)
+
+DEFINE_CAPI(HAL_Bool, InitializedForward, initializedForward)
+DEFINE_CAPI(HAL_Bool, InitializedReverse, initializedReverse)
+DEFINE_CAPI(HAL_Bool, Forward, forward)
+DEFINE_CAPI(HAL_Bool, Reverse, reverse)
+
+#define REGISTER(NAME) \
+ SimRelayData[index].NAME.RegisterCallback(callback, param, initialNotify)
+
+void HALSIM_RegisterRelayAllCallbacks(int32_t index,
+ HAL_NotifyCallback callback, void* param,
+ HAL_Bool initialNotify) {
+ REGISTER(initializedForward);
+ REGISTER(initializedReverse);
+ REGISTER(forward);
+ REGISTER(reverse);
+}
+} // extern "C"
diff --git a/hal/src/main/native/sim/mockdata/RelayDataInternal.h b/hal/src/main/native/sim/mockdata/RelayDataInternal.h
new file mode 100644
index 0000000..cb38388
--- /dev/null
+++ b/hal/src/main/native/sim/mockdata/RelayDataInternal.h
@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "mockdata/RelayData.h"
+#include "mockdata/SimDataValue.h"
+
+namespace hal {
+class RelayData {
+ HAL_SIMDATAVALUE_DEFINE_NAME(InitializedForward)
+ HAL_SIMDATAVALUE_DEFINE_NAME(InitializedReverse)
+ HAL_SIMDATAVALUE_DEFINE_NAME(Forward)
+ HAL_SIMDATAVALUE_DEFINE_NAME(Reverse)
+
+ public:
+ SimDataValue<HAL_Bool, HAL_MakeBoolean, GetInitializedForwardName>
+ initializedForward{false};
+ SimDataValue<HAL_Bool, HAL_MakeBoolean, GetInitializedReverseName>
+ initializedReverse{false};
+ SimDataValue<HAL_Bool, HAL_MakeBoolean, GetForwardName> forward{false};
+ SimDataValue<HAL_Bool, HAL_MakeBoolean, GetReverseName> reverse{false};
+
+ virtual void ResetData();
+};
+extern RelayData* SimRelayData;
+} // namespace hal
diff --git a/hal/src/main/native/sim/mockdata/RoboRioData.cpp b/hal/src/main/native/sim/mockdata/RoboRioData.cpp
new file mode 100644
index 0000000..5cff1d9
--- /dev/null
+++ b/hal/src/main/native/sim/mockdata/RoboRioData.cpp
@@ -0,0 +1,88 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 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 "../PortsInternal.h"
+#include "RoboRioDataInternal.h"
+
+using namespace hal;
+
+namespace hal {
+namespace init {
+void InitializeRoboRioData() {
+ static RoboRioData srrd[1];
+ ::hal::SimRoboRioData = srrd;
+}
+} // namespace init
+} // namespace hal
+
+RoboRioData* hal::SimRoboRioData;
+void RoboRioData::ResetData() {
+ fpgaButton.Reset(false);
+ vInVoltage.Reset(0.0);
+ vInCurrent.Reset(0.0);
+ userVoltage6V.Reset(6.0);
+ userCurrent6V.Reset(0.0);
+ userActive6V.Reset(false);
+ userVoltage5V.Reset(5.0);
+ userCurrent5V.Reset(0.0);
+ userActive5V.Reset(false);
+ userVoltage3V3.Reset(3.3);
+ userCurrent3V3.Reset(0.0);
+ userActive3V3.Reset(false);
+ userFaults6V.Reset(0);
+ userFaults5V.Reset(0);
+ userFaults3V3.Reset(0);
+}
+
+extern "C" {
+void HALSIM_ResetRoboRioData(int32_t index) {
+ SimRoboRioData[index].ResetData();
+}
+
+#define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME) \
+ HAL_SIMDATAVALUE_DEFINE_CAPI(TYPE, HALSIM, RoboRio##CAPINAME, \
+ SimRoboRioData, LOWERNAME)
+
+DEFINE_CAPI(HAL_Bool, FPGAButton, fpgaButton)
+DEFINE_CAPI(double, VInVoltage, vInVoltage)
+DEFINE_CAPI(double, VInCurrent, vInCurrent)
+DEFINE_CAPI(double, UserVoltage6V, userVoltage6V)
+DEFINE_CAPI(double, UserCurrent6V, userCurrent6V)
+DEFINE_CAPI(HAL_Bool, UserActive6V, userActive6V)
+DEFINE_CAPI(double, UserVoltage5V, userVoltage5V)
+DEFINE_CAPI(double, UserCurrent5V, userCurrent5V)
+DEFINE_CAPI(HAL_Bool, UserActive5V, userActive5V)
+DEFINE_CAPI(double, UserVoltage3V3, userVoltage3V3)
+DEFINE_CAPI(double, UserCurrent3V3, userCurrent3V3)
+DEFINE_CAPI(HAL_Bool, UserActive3V3, userActive3V3)
+DEFINE_CAPI(int32_t, UserFaults6V, userFaults6V)
+DEFINE_CAPI(int32_t, UserFaults5V, userFaults5V)
+DEFINE_CAPI(int32_t, UserFaults3V3, userFaults3V3)
+
+#define REGISTER(NAME) \
+ SimRoboRioData[index].NAME.RegisterCallback(callback, param, initialNotify)
+
+void HALSIM_RegisterRoboRioAllCallbacks(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param, HAL_Bool initialNotify) {
+ REGISTER(fpgaButton);
+ REGISTER(vInVoltage);
+ REGISTER(vInCurrent);
+ REGISTER(userVoltage6V);
+ REGISTER(userCurrent6V);
+ REGISTER(userActive6V);
+ REGISTER(userVoltage5V);
+ REGISTER(userCurrent5V);
+ REGISTER(userActive5V);
+ REGISTER(userVoltage3V3);
+ REGISTER(userCurrent3V3);
+ REGISTER(userActive3V3);
+ REGISTER(userFaults6V);
+ REGISTER(userFaults5V);
+ REGISTER(userFaults3V3);
+}
+} // extern "C"
diff --git a/hal/src/main/native/sim/mockdata/RoboRioDataInternal.h b/hal/src/main/native/sim/mockdata/RoboRioDataInternal.h
new file mode 100644
index 0000000..cc74fa2
--- /dev/null
+++ b/hal/src/main/native/sim/mockdata/RoboRioDataInternal.h
@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "mockdata/RoboRioData.h"
+#include "mockdata/SimDataValue.h"
+
+namespace hal {
+class RoboRioData {
+ HAL_SIMDATAVALUE_DEFINE_NAME(FPGAButton)
+ HAL_SIMDATAVALUE_DEFINE_NAME(VInVoltage)
+ HAL_SIMDATAVALUE_DEFINE_NAME(VInCurrent)
+ HAL_SIMDATAVALUE_DEFINE_NAME(UserVoltage6V)
+ HAL_SIMDATAVALUE_DEFINE_NAME(UserCurrent6V)
+ HAL_SIMDATAVALUE_DEFINE_NAME(UserActive6V)
+ HAL_SIMDATAVALUE_DEFINE_NAME(UserVoltage5V)
+ HAL_SIMDATAVALUE_DEFINE_NAME(UserCurrent5V)
+ HAL_SIMDATAVALUE_DEFINE_NAME(UserActive5V)
+ HAL_SIMDATAVALUE_DEFINE_NAME(UserVoltage3V3)
+ HAL_SIMDATAVALUE_DEFINE_NAME(UserCurrent3V3)
+ HAL_SIMDATAVALUE_DEFINE_NAME(UserActive3V3)
+ HAL_SIMDATAVALUE_DEFINE_NAME(UserFaults6V)
+ HAL_SIMDATAVALUE_DEFINE_NAME(UserFaults5V)
+ HAL_SIMDATAVALUE_DEFINE_NAME(UserFaults3V3)
+
+ public:
+ SimDataValue<HAL_Bool, HAL_MakeBoolean, GetFPGAButtonName> fpgaButton{false};
+ SimDataValue<double, HAL_MakeDouble, GetVInVoltageName> vInVoltage{0.0};
+ SimDataValue<double, HAL_MakeDouble, GetVInCurrentName> vInCurrent{0.0};
+ SimDataValue<double, HAL_MakeDouble, GetUserVoltage6VName> userVoltage6V{6.0};
+ SimDataValue<double, HAL_MakeDouble, GetUserCurrent6VName> userCurrent6V{0.0};
+ SimDataValue<HAL_Bool, HAL_MakeBoolean, GetUserActive6VName> userActive6V{
+ false};
+ SimDataValue<double, HAL_MakeDouble, GetUserVoltage5VName> userVoltage5V{5.0};
+ SimDataValue<double, HAL_MakeDouble, GetUserCurrent5VName> userCurrent5V{0.0};
+ SimDataValue<HAL_Bool, HAL_MakeBoolean, GetUserActive5VName> userActive5V{
+ false};
+ SimDataValue<double, HAL_MakeDouble, GetUserVoltage3V3Name> userVoltage3V3{
+ 3.3};
+ SimDataValue<double, HAL_MakeDouble, GetUserCurrent3V3Name> userCurrent3V3{
+ 0.0};
+ SimDataValue<HAL_Bool, HAL_MakeBoolean, GetUserActive3V3Name> userActive3V3{
+ false};
+ SimDataValue<int32_t, HAL_MakeInt, GetUserFaults6VName> userFaults6V{0};
+ SimDataValue<int32_t, HAL_MakeInt, GetUserFaults5VName> userFaults5V{0};
+ SimDataValue<int32_t, HAL_MakeInt, GetUserFaults3V3Name> userFaults3V3{0};
+
+ virtual void ResetData();
+};
+extern RoboRioData* SimRoboRioData;
+} // namespace hal
diff --git a/hal/src/main/native/sim/mockdata/SPIAccelerometerData.cpp b/hal/src/main/native/sim/mockdata/SPIAccelerometerData.cpp
new file mode 100644
index 0000000..db7dc1d
--- /dev/null
+++ b/hal/src/main/native/sim/mockdata/SPIAccelerometerData.cpp
@@ -0,0 +1,60 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 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 "../PortsInternal.h"
+#include "SPIAccelerometerDataInternal.h"
+
+using namespace hal;
+
+namespace hal {
+namespace init {
+void InitializeSPIAccelerometerData() {
+ static SPIAccelerometerData ssad[5];
+ ::hal::SimSPIAccelerometerData = ssad;
+}
+} // namespace init
+} // namespace hal
+
+SPIAccelerometerData* hal::SimSPIAccelerometerData;
+void SPIAccelerometerData::ResetData() {
+ active.Reset(false);
+ range.Reset(0);
+ x.Reset(0.0);
+ y.Reset(0.0);
+ z.Reset(0.0);
+}
+
+extern "C" {
+void HALSIM_ResetSPIAccelerometerData(int32_t index) {
+ SimSPIAccelerometerData[index].ResetData();
+}
+
+#define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME) \
+ HAL_SIMDATAVALUE_DEFINE_CAPI(TYPE, HALSIM, SPIAccelerometer##CAPINAME, \
+ SimSPIAccelerometerData, LOWERNAME)
+
+DEFINE_CAPI(HAL_Bool, Active, active)
+DEFINE_CAPI(int32_t, Range, range)
+DEFINE_CAPI(double, X, x)
+DEFINE_CAPI(double, Y, y)
+DEFINE_CAPI(double, Z, z)
+
+#define REGISTER(NAME) \
+ SimSPIAccelerometerData[index].NAME.RegisterCallback(callback, param, \
+ initialNotify)
+
+void HALSIM_RegisterSPIAccelerometerAllCallbcaks(int32_t index,
+ HAL_NotifyCallback callback,
+ void* param,
+ HAL_Bool initialNotify) {
+ REGISTER(active);
+ REGISTER(range);
+ REGISTER(x);
+ REGISTER(y);
+ REGISTER(z);
+}
+} // extern "C"
diff --git a/hal/src/main/native/sim/mockdata/SPIAccelerometerDataInternal.h b/hal/src/main/native/sim/mockdata/SPIAccelerometerDataInternal.h
new file mode 100644
index 0000000..661e01b
--- /dev/null
+++ b/hal/src/main/native/sim/mockdata/SPIAccelerometerDataInternal.h
@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "mockdata/SPIAccelerometerData.h"
+#include "mockdata/SimDataValue.h"
+
+namespace hal {
+class SPIAccelerometerData {
+ HAL_SIMDATAVALUE_DEFINE_NAME(Active)
+ HAL_SIMDATAVALUE_DEFINE_NAME(Range)
+ HAL_SIMDATAVALUE_DEFINE_NAME(X)
+ HAL_SIMDATAVALUE_DEFINE_NAME(Y)
+ HAL_SIMDATAVALUE_DEFINE_NAME(Z)
+
+ public:
+ SimDataValue<HAL_Bool, HAL_MakeBoolean, GetActiveName> active{false};
+ SimDataValue<int32_t, HAL_MakeInt, GetRangeName> range{0};
+ SimDataValue<double, HAL_MakeDouble, GetXName> x{0.0};
+ SimDataValue<double, HAL_MakeDouble, GetYName> y{0.0};
+ SimDataValue<double, HAL_MakeDouble, GetZName> z{0.0};
+
+ virtual void ResetData();
+};
+extern SPIAccelerometerData* SimSPIAccelerometerData;
+} // namespace hal
diff --git a/hal/src/main/native/sim/mockdata/SPIData.cpp b/hal/src/main/native/sim/mockdata/SPIData.cpp
new file mode 100644
index 0000000..3afc606
--- /dev/null
+++ b/hal/src/main/native/sim/mockdata/SPIData.cpp
@@ -0,0 +1,75 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 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 <iostream>
+
+#include "../PortsInternal.h"
+#include "SPIDataInternal.h"
+
+using namespace hal;
+
+namespace hal {
+namespace init {
+void InitializeSPIData() {
+ static SPIData ssd[5];
+ ::hal::SimSPIData = ssd;
+}
+} // namespace init
+} // namespace hal
+
+SPIData* hal::SimSPIData;
+void SPIData::ResetData() {
+ initialized.Reset(false);
+ read.Reset();
+ write.Reset();
+ autoReceivedData.Reset();
+}
+
+int32_t SPIData::Read(uint8_t* buffer, int32_t count) {
+ read(buffer, count);
+ return count;
+}
+
+int32_t SPIData::Write(const uint8_t* dataToSend, int32_t sendSize) {
+ write(dataToSend, sendSize);
+ return sendSize;
+}
+
+int32_t SPIData::Transaction(const uint8_t* dataToSend, uint8_t* dataReceived,
+ int32_t size) {
+ write(dataToSend, size);
+ read(dataReceived, size);
+ return size;
+}
+
+int32_t SPIData::ReadAutoReceivedData(uint32_t* buffer, int32_t numToRead,
+ double timeout, int32_t* status) {
+ int32_t outputCount = 0;
+ autoReceivedData(buffer, numToRead, &outputCount);
+ return outputCount;
+}
+
+extern "C" {
+void HALSIM_ResetSPIData(int32_t index) { SimSPIData[index].ResetData(); }
+
+#define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME) \
+ HAL_SIMDATAVALUE_DEFINE_CAPI(TYPE, HALSIM, SPI##CAPINAME, SimSPIData, \
+ LOWERNAME)
+
+DEFINE_CAPI(HAL_Bool, Initialized, initialized)
+
+#undef DEFINE_CAPI
+#define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME) \
+ HAL_SIMCALLBACKREGISTRY_DEFINE_CAPI(TYPE, HALSIM, SPI##CAPINAME, SimSPIData, \
+ LOWERNAME)
+
+DEFINE_CAPI(HAL_BufferCallback, Read, read)
+DEFINE_CAPI(HAL_ConstBufferCallback, Write, write)
+DEFINE_CAPI(HAL_SpiReadAutoReceiveBufferCallback, ReadAutoReceivedData,
+ autoReceivedData)
+
+} // extern "C"
diff --git a/hal/src/main/native/sim/mockdata/SPIDataInternal.h b/hal/src/main/native/sim/mockdata/SPIDataInternal.h
new file mode 100644
index 0000000..44868d2
--- /dev/null
+++ b/hal/src/main/native/sim/mockdata/SPIDataInternal.h
@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 "mockdata/SPIData.h"
+#include "mockdata/SimCallbackRegistry.h"
+#include "mockdata/SimDataValue.h"
+
+namespace hal {
+
+class SPIData {
+ HAL_SIMDATAVALUE_DEFINE_NAME(Initialized)
+ HAL_SIMCALLBACKREGISTRY_DEFINE_NAME(Read)
+ HAL_SIMCALLBACKREGISTRY_DEFINE_NAME(Write)
+ HAL_SIMCALLBACKREGISTRY_DEFINE_NAME(AutoReceive)
+
+ public:
+ 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);
+ int32_t ReadAutoReceivedData(uint32_t* buffer, int32_t numToRead,
+ double timeout, int32_t* status);
+
+ SimDataValue<HAL_Bool, HAL_MakeBoolean, GetInitializedName> initialized{
+ false};
+ SimCallbackRegistry<HAL_BufferCallback, GetReadName> read;
+ SimCallbackRegistry<HAL_ConstBufferCallback, GetWriteName> write;
+ SimCallbackRegistry<HAL_SpiReadAutoReceiveBufferCallback, GetAutoReceiveName>
+ autoReceivedData;
+
+ void ResetData();
+};
+extern SPIData* SimSPIData;
+} // namespace hal
diff --git a/hal/src/main/native/sim/mockdata/SimDeviceData.cpp b/hal/src/main/native/sim/mockdata/SimDeviceData.cpp
new file mode 100644
index 0000000..a703257
--- /dev/null
+++ b/hal/src/main/native/sim/mockdata/SimDeviceData.cpp
@@ -0,0 +1,414 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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 "mockdata/SimDeviceData.h" // NOLINT(build/include_order)
+
+#include <algorithm>
+
+#include "SimDeviceDataInternal.h"
+
+using namespace hal;
+
+namespace hal {
+namespace init {
+void InitializeSimDeviceData() {
+ static SimDeviceData sdd;
+ ::hal::SimSimDeviceData = &sdd;
+}
+} // namespace init
+} // namespace hal
+
+SimDeviceData* hal::SimSimDeviceData;
+
+SimDeviceData::Device* SimDeviceData::LookupDevice(HAL_SimDeviceHandle handle) {
+ if (handle <= 0) return nullptr;
+ --handle;
+ if (static_cast<uint32_t>(handle) >= m_devices.size() || !m_devices[handle])
+ return nullptr;
+ return m_devices[handle].get();
+}
+
+SimDeviceData::Value* SimDeviceData::LookupValue(HAL_SimValueHandle handle) {
+ if (handle <= 0) return nullptr;
+
+ // look up device
+ Device* deviceImpl = LookupDevice(handle >> 16);
+ if (!deviceImpl) return nullptr;
+
+ // look up value
+ handle &= 0xffff;
+ --handle;
+ if (static_cast<uint32_t>(handle) >= deviceImpl->values.size() ||
+ !deviceImpl->values[handle])
+ return nullptr;
+
+ return deviceImpl->values[handle].get();
+}
+
+HAL_SimDeviceHandle SimDeviceData::CreateDevice(const char* name) {
+ std::scoped_lock lock(m_mutex);
+
+ // check for duplicates and don't overwrite them
+ auto it = m_deviceMap.find(name);
+ if (it != m_deviceMap.end()) return 0;
+
+ // don't allow more than 4096 devices (limit driven by 12-bit allocation in
+ // value changed callback uid)
+ if (m_devices.size() >= 4095) return 0;
+
+ // create and save
+ auto deviceImpl = std::make_shared<Device>(name);
+ HAL_SimDeviceHandle deviceHandle = m_devices.emplace_back(deviceImpl) + 1;
+ deviceImpl->handle = deviceHandle;
+ m_deviceMap[name] = deviceImpl;
+
+ // notify callbacks
+ m_deviceCreated(name, deviceHandle);
+
+ return deviceHandle;
+}
+
+void SimDeviceData::FreeDevice(HAL_SimDeviceHandle handle) {
+ std::scoped_lock lock(m_mutex);
+ --handle;
+
+ // see if it exists
+ if (handle < 0 || static_cast<uint32_t>(handle) >= m_devices.size()) return;
+ auto deviceImpl = std::move(m_devices[handle]);
+ if (!deviceImpl) return;
+
+ // remove from map
+ m_deviceMap.erase(deviceImpl->name);
+
+ // remove from vector
+ m_devices.erase(handle);
+
+ // notify callbacks
+ m_deviceFreed(deviceImpl->name.c_str(), handle + 1);
+}
+
+HAL_SimValueHandle SimDeviceData::CreateValue(HAL_SimDeviceHandle device,
+ const char* name, bool readonly,
+ int32_t numOptions,
+ const char** options,
+ const HAL_Value& initialValue) {
+ std::scoped_lock lock(m_mutex);
+
+ // look up device
+ Device* deviceImpl = LookupDevice(device);
+ if (!deviceImpl) return 0;
+
+ // check for duplicates and don't overwrite them
+ auto it = deviceImpl->valueMap.find(name);
+ if (it != deviceImpl->valueMap.end()) return 0;
+
+ // don't allow more than 4096 values per device (limit driven by 12-bit
+ // allocation in value changed callback uid)
+ if (deviceImpl->values.size() >= 4095) return 0;
+
+ // create and save; encode device into handle
+ auto valueImplPtr = std::make_unique<Value>(name, readonly, initialValue);
+ Value* valueImpl = valueImplPtr.get();
+ HAL_SimValueHandle valueHandle =
+ (device << 16) |
+ (deviceImpl->values.emplace_back(std::move(valueImplPtr)) + 1);
+ valueImpl->handle = valueHandle;
+ // copy options (if any provided)
+ if (numOptions > 0 && options) {
+ valueImpl->enumOptions.reserve(numOptions);
+ valueImpl->cstrEnumOptions.reserve(numOptions);
+ for (int32_t i = 0; i < numOptions; ++i) {
+ valueImpl->enumOptions.emplace_back(options[i]);
+ // point to our copy of the string, not the passed-in one
+ valueImpl->cstrEnumOptions.emplace_back(
+ valueImpl->enumOptions.back().c_str());
+ }
+ }
+ deviceImpl->valueMap[name] = valueImpl;
+
+ // notify callbacks
+ deviceImpl->valueCreated(name, valueHandle, readonly, &initialValue);
+
+ return valueHandle;
+}
+
+HAL_Value SimDeviceData::GetValue(HAL_SimValueHandle handle) {
+ std::scoped_lock lock(m_mutex);
+ Value* valueImpl = LookupValue(handle);
+
+ if (!valueImpl) {
+ HAL_Value value;
+ value.data.v_int = 0;
+ value.type = HAL_UNASSIGNED;
+ return value;
+ }
+
+ return valueImpl->value;
+}
+
+void SimDeviceData::SetValue(HAL_SimValueHandle handle,
+ const HAL_Value& value) {
+ std::scoped_lock lock(m_mutex);
+ Value* valueImpl = LookupValue(handle);
+ if (!valueImpl) return;
+
+ valueImpl->value = value;
+
+ // notify callbacks
+ valueImpl->changed(valueImpl->name.c_str(), valueImpl->handle,
+ valueImpl->readonly, &value);
+}
+
+int32_t SimDeviceData::RegisterDeviceCreatedCallback(
+ const char* prefix, void* param, HALSIM_SimDeviceCallback callback,
+ bool initialNotify) {
+ std::scoped_lock lock(m_mutex);
+
+ // register callback
+ int32_t index = m_deviceCreated.Register(prefix, param, callback);
+
+ // initial notifications
+ if (initialNotify) {
+ for (auto&& device : m_devices)
+ callback(device->name.c_str(), param, device->handle);
+ }
+
+ return index;
+}
+
+void SimDeviceData::CancelDeviceCreatedCallback(int32_t uid) {
+ if (uid <= 0) return;
+ std::scoped_lock lock(m_mutex);
+ m_deviceCreated.Cancel(uid);
+}
+
+int32_t SimDeviceData::RegisterDeviceFreedCallback(
+ const char* prefix, void* param, HALSIM_SimDeviceCallback callback) {
+ std::scoped_lock lock(m_mutex);
+ return m_deviceFreed.Register(prefix, param, callback);
+}
+
+void SimDeviceData::CancelDeviceFreedCallback(int32_t uid) {
+ if (uid <= 0) return;
+ std::scoped_lock lock(m_mutex);
+ m_deviceFreed.Cancel(uid);
+}
+
+HAL_SimDeviceHandle SimDeviceData::GetDeviceHandle(const char* name) {
+ std::scoped_lock lock(m_mutex);
+ auto it = m_deviceMap.find(name);
+ if (it == m_deviceMap.end()) return 0;
+ if (auto deviceImpl = it->getValue().lock())
+ return deviceImpl->handle;
+ else
+ return 0;
+}
+
+const char* SimDeviceData::GetDeviceName(HAL_SimDeviceHandle handle) {
+ std::scoped_lock lock(m_mutex);
+
+ // look up device
+ Device* deviceImpl = LookupDevice(handle);
+ if (!deviceImpl) return nullptr;
+
+ return deviceImpl->name.c_str();
+}
+
+void SimDeviceData::EnumerateDevices(const char* prefix, void* param,
+ HALSIM_SimDeviceCallback callback) {
+ std::scoped_lock lock(m_mutex);
+ for (auto&& device : m_devices) {
+ if (wpi::StringRef{device->name}.startswith(prefix))
+ callback(device->name.c_str(), param, device->handle);
+ }
+}
+
+int32_t SimDeviceData::RegisterValueCreatedCallback(
+ HAL_SimDeviceHandle device, void* param, HALSIM_SimValueCallback callback,
+ bool initialNotify) {
+ std::scoped_lock lock(m_mutex);
+ Device* deviceImpl = LookupDevice(device);
+ if (!deviceImpl) return -1;
+
+ // register callback
+ int32_t index = deviceImpl->valueCreated.Register(callback, param);
+
+ // initial notifications
+ if (initialNotify) {
+ for (auto&& value : deviceImpl->values)
+ callback(value->name.c_str(), param, value->handle, value->readonly,
+ &value->value);
+ }
+
+ // encode device into uid
+ return (device << 16) | (index & 0xffff);
+}
+
+void SimDeviceData::CancelValueCreatedCallback(int32_t uid) {
+ if (uid <= 0) return;
+ std::scoped_lock lock(m_mutex);
+ Device* deviceImpl = LookupDevice(uid >> 16);
+ if (!deviceImpl) return;
+ deviceImpl->valueCreated.Cancel(uid & 0xffff);
+}
+
+int32_t SimDeviceData::RegisterValueChangedCallback(
+ HAL_SimValueHandle handle, void* param, HALSIM_SimValueCallback callback,
+ bool initialNotify) {
+ std::scoped_lock lock(m_mutex);
+ Value* valueImpl = LookupValue(handle);
+ if (!valueImpl) return -1;
+
+ // register callback
+ int32_t index = valueImpl->changed.Register(callback, param);
+
+ // initial notification
+ if (initialNotify)
+ callback(valueImpl->name.c_str(), param, valueImpl->handle,
+ valueImpl->readonly, &valueImpl->value);
+
+ // encode device and value into uid
+ return (((handle >> 16) & 0xfff) << 19) | ((handle & 0xfff) << 7) |
+ (index & 0x7f);
+}
+
+void SimDeviceData::CancelValueChangedCallback(int32_t uid) {
+ if (uid <= 0) return;
+ std::scoped_lock lock(m_mutex);
+ Value* valueImpl = LookupValue(((uid >> 19) << 16) | ((uid >> 7) & 0xfff));
+ if (!valueImpl) return;
+ valueImpl->changed.Cancel(uid & 0x7f);
+}
+
+HAL_SimValueHandle SimDeviceData::GetValueHandle(HAL_SimDeviceHandle device,
+ const char* name) {
+ std::scoped_lock lock(m_mutex);
+ Device* deviceImpl = LookupDevice(device);
+ if (!deviceImpl) return 0;
+
+ // lookup value
+ auto it = deviceImpl->valueMap.find(name);
+ if (it == deviceImpl->valueMap.end()) return 0;
+ if (!it->getValue()) return 0;
+ return it->getValue()->handle;
+}
+
+void SimDeviceData::EnumerateValues(HAL_SimDeviceHandle device, void* param,
+ HALSIM_SimValueCallback callback) {
+ std::scoped_lock lock(m_mutex);
+ Device* deviceImpl = LookupDevice(device);
+ if (!deviceImpl) return;
+
+ for (auto&& value : deviceImpl->values)
+ callback(value->name.c_str(), param, value->handle, value->readonly,
+ &value->value);
+}
+
+const char** SimDeviceData::GetValueEnumOptions(HAL_SimValueHandle handle,
+ int32_t* numOptions) {
+ *numOptions = 0;
+
+ std::scoped_lock lock(m_mutex);
+ Value* valueImpl = LookupValue(handle);
+ if (!valueImpl) return nullptr;
+
+ // get list of options (safe to return as they never change)
+ auto& options = valueImpl->cstrEnumOptions;
+ *numOptions = options.size();
+ return options.data();
+}
+
+void SimDeviceData::ResetData() {
+ std::scoped_lock lock(m_mutex);
+ m_devices.clear();
+ m_deviceMap.clear();
+ m_deviceCreated.Reset();
+ m_deviceFreed.Reset();
+}
+
+extern "C" {
+
+int32_t HALSIM_RegisterSimDeviceCreatedCallback(
+ const char* prefix, void* param, HALSIM_SimDeviceCallback callback,
+ HAL_Bool initialNotify) {
+ return SimSimDeviceData->RegisterDeviceCreatedCallback(
+ prefix, param, callback, initialNotify);
+}
+
+void HALSIM_CancelSimDeviceCreatedCallback(int32_t uid) {
+ SimSimDeviceData->CancelDeviceCreatedCallback(uid);
+}
+
+int32_t HALSIM_RegisterSimDeviceFreedCallback(
+ const char* prefix, void* param, HALSIM_SimDeviceCallback callback) {
+ return SimSimDeviceData->RegisterDeviceFreedCallback(prefix, param, callback);
+}
+
+void HALSIM_CancelSimDeviceFreedCallback(int32_t uid) {
+ SimSimDeviceData->CancelDeviceFreedCallback(uid);
+}
+
+HAL_SimDeviceHandle HALSIM_GetSimDeviceHandle(const char* name) {
+ return SimSimDeviceData->GetDeviceHandle(name);
+}
+
+const char* HALSIM_GetSimDeviceName(HAL_SimDeviceHandle handle) {
+ return SimSimDeviceData->GetDeviceName(handle);
+}
+
+HAL_SimDeviceHandle HALSIM_GetSimValueDeviceHandle(HAL_SimValueHandle handle) {
+ if (handle <= 0) return 0;
+ return handle >> 16;
+}
+
+void HALSIM_EnumerateSimDevices(const char* prefix, void* param,
+ HALSIM_SimDeviceCallback callback) {
+ SimSimDeviceData->EnumerateDevices(prefix, param, callback);
+}
+
+int32_t HALSIM_RegisterSimValueCreatedCallback(HAL_SimDeviceHandle device,
+ void* param,
+ HALSIM_SimValueCallback callback,
+ HAL_Bool initialNotify) {
+ return SimSimDeviceData->RegisterValueCreatedCallback(device, param, callback,
+ initialNotify);
+}
+
+void HALSIM_CancelSimValueCreatedCallback(int32_t uid) {
+ SimSimDeviceData->CancelValueCreatedCallback(uid);
+}
+
+int32_t HALSIM_RegisterSimValueChangedCallback(HAL_SimValueHandle handle,
+ void* param,
+ HALSIM_SimValueCallback callback,
+ HAL_Bool initialNotify) {
+ return SimSimDeviceData->RegisterValueChangedCallback(handle, param, callback,
+ initialNotify);
+}
+
+void HALSIM_CancelSimValueChangedCallback(int32_t uid) {
+ SimSimDeviceData->CancelValueChangedCallback(uid);
+}
+
+HAL_SimValueHandle HALSIM_GetSimValueHandle(HAL_SimDeviceHandle device,
+ const char* name) {
+ return SimSimDeviceData->GetValueHandle(device, name);
+}
+
+void HALSIM_EnumerateSimValues(HAL_SimDeviceHandle device, void* param,
+ HALSIM_SimValueCallback callback) {
+ SimSimDeviceData->EnumerateValues(device, param, callback);
+}
+
+const char** HALSIM_GetSimValueEnumOptions(HAL_SimValueHandle handle,
+ int32_t* numOptions) {
+ return SimSimDeviceData->GetValueEnumOptions(handle, numOptions);
+}
+
+void HALSIM_ResetSimDeviceData(void) { SimSimDeviceData->ResetData(); }
+
+} // extern "C"
diff --git a/hal/src/main/native/sim/mockdata/SimDeviceDataInternal.h b/hal/src/main/native/sim/mockdata/SimDeviceDataInternal.h
new file mode 100644
index 0000000..2582eca
--- /dev/null
+++ b/hal/src/main/native/sim/mockdata/SimDeviceDataInternal.h
@@ -0,0 +1,214 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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 <stdint.h>
+
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include <wpi/StringMap.h>
+#include <wpi/UidVector.h>
+#include <wpi/spinlock.h>
+
+#include "hal/Value.h"
+#include "mockdata/SimCallbackRegistry.h"
+#include "mockdata/SimDeviceData.h"
+
+namespace hal {
+
+namespace impl {
+
+template <typename CallbackFunction>
+class SimUnnamedCallbackRegistry {
+ public:
+ using RawFunctor = void (*)();
+
+ protected:
+ using CallbackVector = wpi::UidVector<HalCallbackListener<RawFunctor>, 4>;
+
+ public:
+ void Cancel(int32_t uid) {
+ if (m_callbacks) m_callbacks->erase(uid - 1);
+ }
+
+ void Reset() {
+ if (m_callbacks) m_callbacks->clear();
+ }
+
+ int32_t Register(CallbackFunction callback, void* param) {
+ // Must return -1 on a null callback for error handling
+ if (callback == nullptr) return -1;
+ if (!m_callbacks) m_callbacks = std::make_unique<CallbackVector>();
+ return m_callbacks->emplace_back(param,
+ reinterpret_cast<RawFunctor>(callback)) +
+ 1;
+ }
+
+ template <typename... U>
+ void Invoke(const char* name, U&&... u) const {
+ if (m_callbacks) {
+ for (auto&& cb : *m_callbacks) {
+ reinterpret_cast<CallbackFunction>(cb.callback)(name, cb.param,
+ std::forward<U>(u)...);
+ }
+ }
+ }
+
+ template <typename... U>
+ LLVM_ATTRIBUTE_ALWAYS_INLINE void operator()(U&&... u) const {
+ return Invoke(std::forward<U>(u)...);
+ }
+
+ private:
+ std::unique_ptr<CallbackVector> m_callbacks;
+};
+
+template <typename CallbackFunction>
+class SimPrefixCallbackRegistry {
+ struct CallbackData {
+ CallbackData() = default;
+ CallbackData(const char* prefix_, void* param_, CallbackFunction callback_)
+ : prefix{prefix_}, callback{callback_}, param{param_} {}
+ std::string prefix;
+ CallbackFunction callback;
+ void* param;
+
+ explicit operator bool() const { return callback != nullptr; }
+ };
+ using CallbackVector = wpi::UidVector<CallbackData, 4>;
+
+ public:
+ void Cancel(int32_t uid) {
+ if (m_callbacks) m_callbacks->erase(uid - 1);
+ }
+
+ void Reset() {
+ if (m_callbacks) m_callbacks->clear();
+ }
+
+ int32_t Register(const char* prefix, void* param, CallbackFunction callback) {
+ // Must return -1 on a null callback for error handling
+ if (callback == nullptr) return -1;
+ if (!m_callbacks) m_callbacks = std::make_unique<CallbackVector>();
+ return m_callbacks->emplace_back(prefix, param, callback) + 1;
+ }
+
+ template <typename... U>
+ void Invoke(const char* name, U&&... u) const {
+ if (m_callbacks) {
+ for (auto&& cb : *m_callbacks) {
+ if (wpi::StringRef{name}.startswith(cb.prefix)) {
+ cb.callback(name, cb.param, std::forward<U>(u)...);
+ }
+ }
+ }
+ }
+
+ template <typename... U>
+ LLVM_ATTRIBUTE_ALWAYS_INLINE void operator()(U&&... u) const {
+ return Invoke(std::forward<U>(u)...);
+ }
+
+ private:
+ std::unique_ptr<CallbackVector> m_callbacks;
+};
+
+} // namespace impl
+
+class SimDeviceData {
+ private:
+ struct Value {
+ Value(const char* name_, bool readonly_, const HAL_Value& value_)
+ : name{name_}, readonly{readonly_}, value{value_} {}
+
+ HAL_SimValueHandle handle{0};
+ std::string name;
+ bool readonly;
+ HAL_Value value;
+ std::vector<std::string> enumOptions;
+ std::vector<const char*> cstrEnumOptions;
+ impl::SimUnnamedCallbackRegistry<HALSIM_SimValueCallback> changed;
+ };
+
+ struct Device {
+ explicit Device(const char* name_) : name{name_} {}
+
+ HAL_SimDeviceHandle handle{0};
+ std::string name;
+ wpi::UidVector<std::unique_ptr<Value>, 16> values;
+ wpi::StringMap<Value*> valueMap;
+ impl::SimUnnamedCallbackRegistry<HALSIM_SimValueCallback> valueCreated;
+ };
+
+ wpi::UidVector<std::shared_ptr<Device>, 4> m_devices;
+ wpi::StringMap<std::weak_ptr<Device>> m_deviceMap;
+
+ wpi::recursive_spinlock m_mutex;
+
+ impl::SimPrefixCallbackRegistry<HALSIM_SimDeviceCallback> m_deviceCreated;
+ impl::SimPrefixCallbackRegistry<HALSIM_SimDeviceCallback> m_deviceFreed;
+
+ // call with lock held, returns null if does not exist
+ Device* LookupDevice(HAL_SimDeviceHandle handle);
+ Value* LookupValue(HAL_SimValueHandle handle);
+
+ public:
+ HAL_SimDeviceHandle CreateDevice(const char* name);
+ void FreeDevice(HAL_SimDeviceHandle handle);
+ HAL_SimValueHandle CreateValue(HAL_SimDeviceHandle device, const char* name,
+ bool readonly, int32_t numOptions,
+ const char** options,
+ const HAL_Value& initialValue);
+ HAL_Value GetValue(HAL_SimValueHandle handle);
+ void SetValue(HAL_SimValueHandle handle, const HAL_Value& value);
+
+ int32_t RegisterDeviceCreatedCallback(const char* prefix, void* param,
+ HALSIM_SimDeviceCallback callback,
+ bool initialNotify);
+
+ void CancelDeviceCreatedCallback(int32_t uid);
+
+ int32_t RegisterDeviceFreedCallback(const char* prefix, void* param,
+ HALSIM_SimDeviceCallback callback);
+
+ void CancelDeviceFreedCallback(int32_t uid);
+
+ HAL_SimDeviceHandle GetDeviceHandle(const char* name);
+ const char* GetDeviceName(HAL_SimDeviceHandle handle);
+
+ void EnumerateDevices(const char* prefix, void* param,
+ HALSIM_SimDeviceCallback callback);
+
+ int32_t RegisterValueCreatedCallback(HAL_SimDeviceHandle device, void* param,
+ HALSIM_SimValueCallback callback,
+ bool initialNotify);
+
+ void CancelValueCreatedCallback(int32_t uid);
+
+ int32_t RegisterValueChangedCallback(HAL_SimValueHandle handle, void* param,
+ HALSIM_SimValueCallback callback,
+ bool initialNotify);
+
+ void CancelValueChangedCallback(int32_t uid);
+
+ HAL_SimValueHandle GetValueHandle(HAL_SimDeviceHandle device,
+ const char* name);
+
+ void EnumerateValues(HAL_SimDeviceHandle device, void* param,
+ HALSIM_SimValueCallback callback);
+
+ const char** GetValueEnumOptions(HAL_SimValueHandle handle,
+ int32_t* numOptions);
+
+ void ResetData();
+};
+extern SimDeviceData* SimSimDeviceData;
+} // namespace hal