Squashed 'third_party/allwpilib_2017/' content from commit 35ac87d
Change-Id: I7bb6f5556c30d3f5a092e68de0be9c710c60c9f4
git-subtree-dir: third_party/allwpilib_2017
git-subtree-split: 35ac87d6ff8b7f061c4f18c9ea316e5dccd4888a
diff --git a/hal/lib/athena/Accelerometer.cpp b/hal/lib/athena/Accelerometer.cpp
new file mode 100644
index 0000000..6c912f3
--- /dev/null
+++ b/hal/lib/athena/Accelerometer.cpp
@@ -0,0 +1,252 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be 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 "HAL/ChipObject.h"
+#include "HAL/HAL.h"
+
+using namespace hal;
+
+// The 7-bit I2C address with a 0 "send" bit
+static const uint8_t kSendAddress = (0x1c << 1) | 0;
+
+// The 7-bit I2C address with a 1 "receive" bit
+static const uint8_t kReceiveAddress = (0x1c << 1) | 1;
+
+static const uint8_t kControlTxRx = 1;
+static const uint8_t kControlStart = 2;
+static const 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
+};
+
+static void writeRegister(Register reg, uint8_t data);
+static uint8_t readRegister(Register reg);
+
+/**
+ * Initialize the accelerometer.
+ */
+static void initializeAccelerometer() {
+ int32_t status;
+
+ if (!accel) {
+ accel.reset(tAccel::create(&status));
+
+ // 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;
+ }
+}
+
+extern "C" {
+
+/**
+ * Set the accelerometer to active or standby mode. It must be in standby
+ * mode to change any configuration.
+ */
+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));
+}
+
+/**
+ * Set 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.
+ */
+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);
+}
+
+/**
+ * Get the x-axis acceleration
+ *
+ * This is a floating point value in units of 1 g-force
+ */
+double HAL_GetAccelerometerX() {
+ initializeAccelerometer();
+
+ int32_t raw =
+ (readRegister(kReg_OutXMSB) << 4) | (readRegister(kReg_OutXLSB) >> 4);
+ return unpackAxis(raw);
+}
+
+/**
+ * Get the y-axis acceleration
+ *
+ * This is a floating point value in units of 1 g-force
+ */
+double HAL_GetAccelerometerY() {
+ initializeAccelerometer();
+
+ int32_t raw =
+ (readRegister(kReg_OutYMSB) << 4) | (readRegister(kReg_OutYLSB) >> 4);
+ return unpackAxis(raw);
+}
+
+/**
+ * Get the z-axis acceleration
+ *
+ * This is a floating point value in units of 1 g-force
+ */
+double HAL_GetAccelerometerZ() {
+ initializeAccelerometer();
+
+ int32_t raw =
+ (readRegister(kReg_OutZMSB) << 4) | (readRegister(kReg_OutZLSB) >> 4);
+ return unpackAxis(raw);
+}
+
+} // extern "C"
diff --git a/hal/lib/athena/AnalogAccumulator.cpp b/hal/lib/athena/AnalogAccumulator.cpp
new file mode 100644
index 0000000..b3af537
--- /dev/null
+++ b/hal/lib/athena/AnalogAccumulator.cpp
@@ -0,0 +1,196 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be 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;
+
+extern "C" {
+/**
+ * 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) {
+ 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;
+}
+
+/**
+ * Initialize the accumulator.
+ *
+ * @param analogPortHandle Handle to the analog port.
+ */
+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);
+}
+
+/**
+ * Resets the accumulator to the initial value.
+ *
+ * @param analogPortHandle Handle to the analog port.
+ */
+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);
+}
+
+/**
+ * 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) {
+ 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);
+}
+
+/**
+ * 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) {
+ 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);
+}
+
+/**
+ * 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) {
+ 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;
+}
+
+/**
+ * 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) {
+ 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);
+}
+
+/**
+ * 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) {
+ 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;
+}
+}
diff --git a/hal/lib/athena/AnalogGyro.cpp b/hal/lib/athena/AnalogGyro.cpp
new file mode 100644
index 0000000..8d3d82d
--- /dev/null
+++ b/hal/lib/athena/AnalogGyro.cpp
@@ -0,0 +1,242 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be 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 "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;
+};
+}
+
+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;
+
+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) {
+ 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;
+ 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) + .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;
+}
+}
diff --git a/hal/lib/athena/AnalogInput.cpp b/hal/lib/athena/AnalogInput.cpp
new file mode 100644
index 0000000..7c966ef
--- /dev/null
+++ b/hal/lib/athena/AnalogInput.cpp
@@ -0,0 +1,385 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be 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 <mutex>
+
+#include "AnalogInternal.h"
+#include "FRC_NetworkCommunication/AICalibration.h"
+#include "HAL/AnalogAccumulator.h"
+#include "HAL/HAL.h"
+#include "HAL/cpp/priority_mutex.h"
+#include "HAL/handles/HandlesInternal.h"
+#include "PortsInternal.h"
+
+using namespace hal;
+
+extern "C" {
+/**
+ * Initialize the analog input port using the given port object.
+ *
+ * @param portHandle Handle to the port to initialize.
+ */
+HAL_AnalogInputHandle HAL_InitializeAnalogInputPort(HAL_PortHandle portHandle,
+ int32_t* status) {
+ 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;
+}
+
+/**
+ * @param analogPortHandle Handle to the analog port.
+ */
+void HAL_FreeAnalogInputPort(HAL_AnalogInputHandle analogPortHandle) {
+ // no status, so no need to check for a proper free.
+ analogInputHandles.Free(analogPortHandle);
+}
+
+/**
+ * Check 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) { return module == 1; }
+
+/**
+ * Check that the analog output channel number is value.
+ * Verify 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) {
+ return channel < kNumAnalogInputs && channel >= 0;
+}
+
+/**
+ * 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 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);
+}
+
+/**
+ * Get 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) {
+ 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);
+}
+
+/**
+ * Set 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) {
+ auto port = analogInputHandles.Get(analogPortHandle);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ analogInputSystem->writeAverageBits(port->channel, static_cast<uint8_t>(bits),
+ status);
+}
+
+/**
+ * Get 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) {
+ auto port = analogInputHandles.Get(analogPortHandle);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return kDefaultAverageBits;
+ }
+ uint8_t result = analogInputSystem->readAverageBits(port->channel, status);
+ return result;
+}
+
+/**
+ * Set 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) {
+ auto port = analogInputHandles.Get(analogPortHandle);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ analogInputSystem->writeOversampleBits(port->channel,
+ static_cast<uint8_t>(bits), status);
+}
+
+/**
+ * Get 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) {
+ auto port = analogInputHandles.Get(analogPortHandle);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return kDefaultOversampleBits;
+ }
+ uint8_t result = analogInputSystem->readOversampleBits(port->channel, status);
+ return result;
+}
+
+/**
+ * Get 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) {
+ 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::lock_guard<priority_recursive_mutex> sync(analogRegisterWindowMutex);
+ analogInputSystem->writeReadSelect(readSelect, status);
+ analogInputSystem->strobeLatchOutput(status);
+ return static_cast<int16_t>(analogInputSystem->readOutput(status));
+}
+
+/**
+ * Get 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) {
+ 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::lock_guard<priority_recursive_mutex> sync(analogRegisterWindowMutex);
+ analogInputSystem->writeReadSelect(readSelect, status);
+ analogInputSystem->strobeLatchOutput(status);
+ return static_cast<int32_t>(analogInputSystem->readOutput(status));
+}
+
+/**
+ * Get 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) {
+ 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;
+}
+
+/**
+ * Get 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) {
+ 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;
+}
+
+/**
+ * Convert 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) {
+ 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;
+}
+
+/**
+ * Get 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) {
+ 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;
+}
+
+/**
+ * Get 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) {
+ 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;
+}
+}
diff --git a/hal/lib/athena/AnalogInternal.cpp b/hal/lib/athena/AnalogInternal.cpp
new file mode 100644
index 0000000..98e5524
--- /dev/null
+++ b/hal/lib/athena/AnalogInternal.cpp
@@ -0,0 +1,121 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be 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 "HAL/AnalogInput.h"
+#include "HAL/ChipObject.h"
+#include "HAL/cpp/priority_mutex.h"
+#include "PortsInternal.h"
+
+namespace hal {
+priority_recursive_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;
+
+/**
+ * Initialize the analog System.
+ */
+void initializeAnalog(int32_t* status) {
+ if (analogSystemInitialized) return;
+ std::lock_guard<priority_recursive_mutex> sync(analogRegisterWindowMutex);
+ if (analogSystemInitialized) return;
+ analogInputSystem.reset(tAI::create(status));
+ analogOutputSystem.reset(tAO::create(status));
+ setAnalogNumChannelsToActivate(kNumAnalogInputs);
+ setAnalogSampleRate(kDefaultSampleRate, status);
+ analogSystemInitialized = true;
+}
+
+/**
+ * Return the number of channels on the module in use.
+ *
+ * @return Active channels.
+ */
+int32_t getAnalogNumActiveChannels(int32_t* status) {
+ int32_t scanSize = analogInputSystem->readConfig_ScanSize(status);
+ if (scanSize == 0) return 8;
+ return scanSize;
+}
+
+/**
+ * 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) {
+ if (analogNumChannelsToActivate == 0)
+ return getAnalogNumActiveChannels(status);
+ return analogNumChannelsToActivate;
+}
+
+/**
+ * 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) {
+ // 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);
+}
+
+/**
+ * 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) {
+ analogNumChannelsToActivate = channels;
+}
+} // namespace hal
diff --git a/hal/lib/athena/AnalogInternal.h b/hal/lib/athena/AnalogInternal.h
new file mode 100644
index 0000000..f91c8e5
--- /dev/null
+++ b/hal/lib/athena/AnalogInternal.h
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be 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 "HAL/ChipObject.h"
+#include "HAL/Ports.h"
+#include "HAL/cpp/priority_mutex.h"
+#include "HAL/handles/IndexedHandleResource.h"
+#include "PortsInternal.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 const uint32_t kAccumulatorChannels[] = {0, 1};
+
+extern std::unique_ptr<tAI> analogInputSystem;
+extern std::unique_ptr<tAO> analogOutputSystem;
+extern priority_recursive_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;
+
+int32_t getAnalogNumActiveChannels(int32_t* status);
+int32_t getAnalogNumChannelsToActivate(int32_t* status);
+void setAnalogNumChannelsToActivate(int32_t channels);
+void setAnalogSampleRate(double samplesPerSecond, int32_t* status);
+void initializeAnalog(int32_t* status);
+} // namespace hal
diff --git a/hal/lib/athena/AnalogOutput.cpp b/hal/lib/athena/AnalogOutput.cpp
new file mode 100644
index 0000000..33642a9
--- /dev/null
+++ b/hal/lib/athena/AnalogOutput.cpp
@@ -0,0 +1,106 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be 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 "HAL/Errors.h"
+#include "HAL/handles/HandlesInternal.h"
+#include "HAL/handles/IndexedHandleResource.h"
+#include "PortsInternal.h"
+
+using namespace hal;
+
+namespace {
+struct AnalogOutput {
+ uint8_t channel;
+};
+}
+
+static IndexedHandleResource<HAL_AnalogOutputHandle, AnalogOutput,
+ kNumAnalogOutputs, HAL_HandleEnum::AnalogOutput>
+ analogOutputHandles;
+
+extern "C" {
+
+/**
+ * Initialize the analog output port using the given port object.
+ */
+HAL_AnalogOutputHandle HAL_InitializeAnalogOutputPort(HAL_PortHandle portHandle,
+ int32_t* status) {
+ 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);
+}
+
+/**
+ * Check that the analog output channel number is value.
+ * Verify 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) {
+ 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;
+ }
+
+ 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;
+}
+}
diff --git a/hal/lib/athena/AnalogTrigger.cpp b/hal/lib/athena/AnalogTrigger.cpp
new file mode 100644
index 0000000..a1d85bc
--- /dev/null
+++ b/hal/lib/athena/AnalogTrigger.cpp
@@ -0,0 +1,206 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be 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 "HAL/AnalogInput.h"
+#include "HAL/Errors.h"
+#include "HAL/handles/HandlesInternal.h"
+#include "HAL/handles/LimitedHandleResource.h"
+#include "PortsInternal.h"
+
+using namespace hal;
+
+namespace {
+struct AnalogTrigger {
+ std::unique_ptr<tAnalogTrigger> trigger;
+ HAL_AnalogInputHandle analogHandle;
+ uint8_t index;
+};
+}
+
+static LimitedHandleResource<HAL_AnalogTriggerHandle, AnalogTrigger,
+ kNumAnalogTriggers, HAL_HandleEnum::AnalogTrigger>
+ analogTriggerHandles;
+
+extern "C" {
+
+HAL_AnalogTriggerHandle HAL_InitializeAnalogTrigger(
+ HAL_AnalogInputHandle portHandle, int32_t* index, int32_t* status) {
+ // 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));
+ *index = trigger->index;
+
+ trigger->trigger.reset(tAnalogTrigger::create(trigger->index, status));
+ trigger->trigger->writeSourceSelect_Channel(analog_port->channel, status);
+ return handle;
+}
+
+void HAL_CleanAnalogTrigger(HAL_AnalogTriggerHandle analogTriggerHandle,
+ int32_t* status) {
+ analogTriggerHandles.Free(analogTriggerHandle);
+ // caller owns the analog 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;
+ }
+ trigger->trigger->writeLowerLimit(lower, status);
+ trigger->trigger->writeUpperLimit(upper, status);
+}
+
+/**
+ * Set the upper and lower limits of the analog trigger.
+ * The limits are given as floating point voltage values.
+ */
+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;
+ }
+
+ // TODO: This depends on the averaged setting. Only raw values will work as
+ // is.
+ trigger->trigger->writeLowerLimit(
+ HAL_GetAnalogVoltsToValue(trigger->analogHandle, lower, status), status);
+ trigger->trigger->writeUpperLimit(
+ HAL_GetAnalogVoltsToValue(trigger->analogHandle, upper, status), status);
+}
+
+/**
+ * Configure 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.
+ */
+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) {
+ *status = INCOMPATIBLE_STATE;
+ // TODO: wpi_setWPIErrorWithContext(IncompatibleMode, "Hardware does not
+ // support average and filtering at the same time.");
+ }
+ trigger->trigger->writeSourceSelect_Averaged(useAveragedValue, status);
+}
+
+/**
+ * Configure 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.
+ */
+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) {
+ *status = INCOMPATIBLE_STATE;
+ // TODO: wpi_setWPIErrorWithContext(IncompatibleMode, "Hardware does not "
+ // "support average and filtering at the same time.");
+ }
+ trigger->trigger->writeSourceSelect_Filter(useFilteredValue, status);
+}
+
+/**
+ * Return the InWindow output of the analog trigger.
+ * True if the analog input is between the upper and lower limits.
+ * @return The InWindow output of the analog trigger.
+ */
+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;
+}
+
+/**
+ * Return the TriggerState output of the analog trigger.
+ * True if above upper limit.
+ * False if below lower limit.
+ * If in Hysteresis, maintain previous state.
+ * @return The TriggerState output of the analog trigger.
+ */
+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;
+}
+
+/**
+ * Get the state of the analog trigger output.
+ * @return The state of the analog trigger output.
+ */
+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; // XXX: Backport
+ case HAL_Trigger_kState:
+ result = trigger->trigger->readOutput_OverLimit(trigger->index, status);
+ break; // XXX: Backport
+ case HAL_Trigger_kRisingPulse:
+ case HAL_Trigger_kFallingPulse:
+ *status = ANALOG_TRIGGER_PULSE_OUTPUT_ERROR;
+ return false;
+ }
+ return result;
+}
+}
diff --git a/hal/lib/athena/Compressor.cpp b/hal/lib/athena/Compressor.cpp
new file mode 100644
index 0000000..210beca
--- /dev/null
+++ b/hal/lib/athena/Compressor.cpp
@@ -0,0 +1,193 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be 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 "HAL/Errors.h"
+#include "HAL/handles/HandlesInternal.h"
+#include "PCMInternal.h"
+#include "PortsInternal.h"
+#include "ctre/PCM.h"
+
+using namespace hal;
+
+extern "C" {
+
+HAL_CompressorHandle HAL_InitializeCompressor(int32_t module, int32_t* status) {
+ // 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);
+}
+
+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);
+ 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);
+ 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);
+ 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);
+ 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);
+ 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);
+ 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);
+ 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);
+ 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);
+ 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);
+ 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);
+ 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/lib/athena/Constants.cpp b/hal/lib/athena/Constants.cpp
new file mode 100644
index 0000000..1f4517b
--- /dev/null
+++ b/hal/lib/athena/Constants.cpp
@@ -0,0 +1,18 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be 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;
+
+extern "C" {
+int32_t HAL_GetSystemClockTicksPerMicrosecond(void) {
+ return kSystemClockTicksPerMicrosecond;
+}
+}
diff --git a/hal/lib/athena/ConstantsInternal.h b/hal/lib/athena/ConstantsInternal.h
new file mode 100644
index 0000000..a1466fb
--- /dev/null
+++ b/hal/lib/athena/ConstantsInternal.h
@@ -0,0 +1,14 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be 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;
+}
diff --git a/hal/lib/athena/Counter.cpp b/hal/lib/athena/Counter.cpp
new file mode 100644
index 0000000..9273ea9
--- /dev/null
+++ b/hal/lib/athena/Counter.cpp
@@ -0,0 +1,466 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be 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 "HAL/HAL.h"
+#include "HAL/handles/LimitedHandleResource.h"
+#include "PortsInternal.h"
+
+using namespace hal;
+
+namespace {
+struct Counter {
+ std::unique_ptr<tCounter> counter;
+ uint8_t index;
+};
+}
+
+static LimitedHandleResource<HAL_CounterHandle, Counter, kNumCounters,
+ HAL_HandleEnum::Counter>
+ counterHandles;
+
+extern "C" {
+HAL_CounterHandle HAL_InitializeCounter(HAL_Counter_Mode mode, int32_t* index,
+ int32_t* status) {
+ 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);
+}
+
+/**
+ * Set the source object that causes the counter to count up.
+ * Set the up counting DigitalSource.
+ */
+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);
+}
+
+/**
+ * Set the edge sensitivity on an up counting source.
+ * Set the up source to either detect rising edges or falling edges.
+ */
+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);
+}
+
+/**
+ * Disable the up counting source to the counter.
+ */
+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);
+}
+
+/**
+ * Set the source object that causes the counter to count down.
+ * Set the down counting DigitalSource.
+ */
+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);
+}
+
+/**
+ * Set the edge sensitivity on a down counting source.
+ * Set the down source to either detect rising edges or falling edges.
+ */
+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);
+}
+
+/**
+ * Disable the down counting source to the counter.
+ */
+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);
+}
+
+/**
+ * Set standard up / down counting mode on this counter.
+ * Up and down counts are sourced independently from two inputs.
+ */
+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);
+}
+
+/**
+ * Set external direction mode on this counter.
+ * Counts are sourced on the Up counter input.
+ * The Down counter input represents the direction to count.
+ */
+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);
+}
+
+/**
+ * Set Semi-period mode on this counter.
+ * Counts up on both rising and falling edges.
+ */
+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);
+}
+
+/**
+ * Configure 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 threshold The pulse length beyond which the counter counts the
+ * opposite direction. Units are seconds.
+ */
+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);
+}
+
+/**
+ * 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_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);
+}
+
+/**
+ * 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_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);
+}
+
+/**
+ * Reset the Counter to zero.
+ * Set the counter value to zero. This doesn't effect the running state of the
+ * counter, just sets the current value to zero.
+ */
+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);
+}
+
+/**
+ * Read the current counter 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.
+ */
+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;
+}
+
+/*
+ * Get 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.
+ * @returns The period of the last two pulses in units of seconds.
+ */
+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)
+}
+
+/**
+ * Set 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 GetStopped
+ * method.
+ * @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) {
+ 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);
+}
+
+/**
+ * Select 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).
+ */
+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);
+}
+
+/**
+ * Determine 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.
+ * @return Returns true if the most recent counter period exceeds the MaxPeriod
+ * value set by SetMaxPeriod.
+ */
+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);
+}
+
+/**
+ * The last direction the counter value changed.
+ * @return The last direction the counter value changed.
+ */
+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;
+}
+
+/**
+ * Set 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 reverseDirection true if the value counted should be negated.
+ */
+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);
+ }
+}
+}
diff --git a/hal/lib/athena/DIO.cpp b/hal/lib/athena/DIO.cpp
new file mode 100644
index 0000000..cf01397
--- /dev/null
+++ b/hal/lib/athena/DIO.cpp
@@ -0,0 +1,532 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be 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 "HAL/handles/HandlesInternal.h"
+#include "HAL/handles/LimitedHandleResource.h"
+#include "PortsInternal.h"
+
+using namespace hal;
+
+// Create a mutex to protect changes to the digital output values
+static priority_recursive_mutex digitalDIOMutex;
+
+static LimitedHandleResource<HAL_DigitalPWMHandle, uint8_t,
+ kNumDigitalPWMOutputs, HAL_HandleEnum::DigitalPWM>
+ digitalPWMHandles;
+
+extern "C" {
+
+/**
+ * Create a new instance of a digital port.
+ */
+HAL_DigitalHandle HAL_InitializeDIOPort(HAL_PortHandle portHandle,
+ HAL_Bool input, int32_t* status) {
+ 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::lock_guard<priority_recursive_mutex> sync(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.
+ digitalChannelHandles.Free(dioPortHandle, HAL_HandleEnum::DIO);
+ if (port == nullptr) return;
+ int32_t status = 0;
+ std::lock_guard<priority_recursive_mutex> sync(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);
+ }
+}
+
+/**
+ * Allocate a DO PWM Generator.
+ * Allocate PWM generators so that they are not accidentally reused.
+ *
+ * @return PWM Generator handle
+ */
+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;
+}
+
+/**
+ * Free the resource associated with a DO PWM generator.
+ *
+ * @param pwmGenerator The pwmGen to free that was allocated with
+ * allocateDigitalPWM()
+ */
+void HAL_FreeDigitalPWM(HAL_DigitalPWMHandle pwmGenerator, int32_t* status) {
+ digitalPWMHandles.Free(pwmGenerator);
+}
+
+/**
+ * Change 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) {
+ // 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;
+ uint8_t pwmPeriodPower = static_cast<uint8_t>(
+ std::log(1.0 / (pwmSystem->readLoopTiming(status) * 0.25E-6 * rate)) /
+ std::log(2.0) +
+ 0.5);
+ digitalSystem->writePWMPeriodPower(pwmPeriodPower, status);
+}
+
+/**
+ * Configure the duty-cycle of the PWM generator
+ *
+ * @param pwmGenerator The generator index reserved by allocateDigitalPWM()
+ * @param dutyCycle The percent duty cycle to output [0..1].
+ */
+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::lock_guard<priority_recursive_mutex> sync(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);
+ }
+}
+
+/**
+ * Configure which DO channel the PWM signal is output on
+ *
+ * @param pwmGenerator The generator index reserved by allocateDigitalPWM()
+ * @param channel The Digital Output channel to output on
+ */
+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);
+}
+
+/**
+ * Write a digital I/O bit to the FPGA.
+ * Set a single value on a digital I/O channel.
+ *
+ * @param channel The Digital I/O channel
+ * @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) {
+ 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::lock_guard<priority_recursive_mutex> sync(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);
+ }
+}
+
+/**
+ * Read a digital I/O bit from the FPGA.
+ * Get a single value from a digital I/O channel.
+ *
+ * @param channel The digital I/O channel
+ * @return The state of the specified channel
+ */
+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;
+ }
+}
+
+/**
+ * Read the direction of a the Digital I/O lines
+ * A 1 bit means output and a 0 bit means input.
+ *
+ * @param channel The digital I/O channel
+ * @return The direction of the specified channel
+ */
+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;
+ }
+}
+
+/**
+ * Generate a single pulse.
+ * Write a pulse to the specified digital output channel. There can only be a
+ * single pulse going at any time.
+ *
+ * @param channel The Digital Output channel that the pulse should be output on
+ * @param pulseLength The active length of the pulse (in seconds)
+ */
+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<uint8_t>(1.0e9 * pulseLength /
+ (pwmSystem->readLoopTiming(status) * 25)),
+ status);
+ digitalSystem->writePulse(pulse, status);
+}
+
+/**
+ * Check a DIO line to see if it is currently generating a pulse.
+ *
+ * @return A pulse is in progress
+ */
+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;
+ }
+}
+
+/**
+ * Check if any DIO line is currently generating a pulse.
+ *
+ * @return A pulse on some line is in progress
+ */
+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;
+}
+
+/**
+ * Write the filter index from the FPGA.
+ * Set the filter index used to filter out short pulses.
+ *
+ * @param dioPortHandle Handle to the digital I/O channel
+ * @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) {
+ auto port = digitalChannelHandles.Get(dioPortHandle, HAL_HandleEnum::DIO);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ std::lock_guard<priority_recursive_mutex> sync(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);
+ }
+}
+
+/**
+ * Read the filter index from the FPGA.
+ * Get the filter index used to filter out short pulses.
+ *
+ * @param dioPortHandle Handle to the digital I/O channel
+ * @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) {
+ auto port = digitalChannelHandles.Get(dioPortHandle, HAL_HandleEnum::DIO);
+ if (port == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+
+ std::lock_guard<priority_recursive_mutex> sync(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);
+ }
+}
+
+/**
+ * Set the filter period for the specified filter index.
+ *
+ * Set 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) {
+ initializeDigital(status);
+ if (*status != 0) return;
+ std::lock_guard<priority_recursive_mutex> sync(digitalDIOMutex);
+ digitalSystem->writeFilterPeriodHdr(filterIndex, value, status);
+ if (*status == 0) {
+ digitalSystem->writeFilterPeriodMXP(filterIndex, value, status);
+ }
+}
+
+/**
+ * Get the filter period for the specified filter index.
+ *
+ * Get 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) {
+ initializeDigital(status);
+ if (*status != 0) return 0;
+ uint32_t hdrPeriod = 0;
+ uint32_t mxpPeriod = 0;
+ {
+ std::lock_guard<priority_recursive_mutex> sync(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;
+}
+}
diff --git a/hal/lib/athena/DigitalInternal.cpp b/hal/lib/athena/DigitalInternal.cpp
new file mode 100644
index 0000000..01e6c16
--- /dev/null
+++ b/hal/lib/athena/DigitalInternal.cpp
@@ -0,0 +1,160 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be 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 <mutex>
+#include <thread>
+
+#include "ConstantsInternal.h"
+#include "FRC_NetworkCommunication/LoadOut.h"
+#include "HAL/AnalogTrigger.h"
+#include "HAL/ChipObject.h"
+#include "HAL/HAL.h"
+#include "HAL/Ports.h"
+#include "HAL/cpp/priority_mutex.h"
+#include "PortsInternal.h"
+
+namespace hal {
+// Create a mutex to protect changes to the DO PWM config
+priority_recursive_mutex digitalPwmMutex;
+
+std::unique_ptr<tDIO> digitalSystem;
+std::unique_ptr<tRelay> relaySystem;
+std::unique_ptr<tPWM> pwmSystem;
+std::unique_ptr<tSPI> spiSystem;
+
+static std::atomic<bool> digitalSystemsInitialized{false};
+static priority_mutex initializeMutex;
+
+DigitalHandleResource<HAL_DigitalHandle, DigitalPort,
+ kNumDigitalChannels + kNumPWMHeaders>
+ digitalChannelHandles;
+
+/**
+ * Initialize the digital system.
+ */
+void initializeDigital(int32_t* status) {
+ // Initial check, as if it's true initialization has finished
+ if (digitalSystemsInitialized) return;
+
+ std::lock_guard<priority_mutex> lock(initializeMutex);
+ // Second check in case another thread was waiting
+ if (digitalSystemsInitialized) 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 + .5), status);
+ uint16_t minHigh = static_cast<uint16_t>(
+ (kDefaultPwmCenter - kDefaultPwmStepsDown * loopTime) / loopTime + .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));
+
+ digitalSystemsInitialized = true;
+}
+
+/**
+ * Map SPI channel numbers from their physical number (27 to 31) to their
+ * position in the bit field.
+ */
+int32_t remapSPIChannel(int32_t channel) { return channel - 26; }
+
+/**
+ * Map DIO channel numbers from their physical number (10 to 26) to their
+ * position in the bit field.
+ */
+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
+ }
+}
+
+/**
+ * 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) {
+ 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;
+ }
+}
+} // namespace hal
diff --git a/hal/lib/athena/DigitalInternal.h b/hal/lib/athena/DigitalInternal.h
new file mode 100644
index 0000000..65dcc85
--- /dev/null
+++ b/hal/lib/athena/DigitalInternal.h
@@ -0,0 +1,89 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be 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 "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"
+#include "PortsInternal.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;
+
+// Create a mutex to protect changes to the DO PWM config
+extern priority_recursive_mutex digitalPwmMutex;
+
+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;
+
+void initializeDigital(int32_t* status);
+bool remapDigitalSource(HAL_Handle digitalSourceHandle,
+ HAL_AnalogTriggerType analogTriggerType,
+ uint8_t& channel, uint8_t& module, bool& analogTrigger);
+int32_t remapSPIChannel(int32_t channel);
+int32_t remapMXPPWMChannel(int32_t channel);
+int32_t remapMXPChannel(int32_t channel);
+} // namespace hal
diff --git a/hal/lib/athena/Encoder.cpp b/hal/lib/athena/Encoder.cpp
new file mode 100644
index 0000000..d7998d1
--- /dev/null
+++ b/hal/lib/athena/Encoder.cpp
@@ -0,0 +1,448 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be 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 "HAL/ChipObject.h"
+#include "HAL/Counter.h"
+#include "HAL/Errors.h"
+#include "HAL/handles/LimitedClassedHandleResource.h"
+#include "PortsInternal.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(.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;
+
+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) {
+ 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);
+}
+
+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();
+}
+}
diff --git a/hal/lib/athena/EncoderInternal.h b/hal/lib/athena/EncoderInternal.h
new file mode 100644
index 0000000..76ec840
--- /dev/null
+++ b/hal/lib/athena/EncoderInternal.h
@@ -0,0 +1,76 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be 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 {
+class Encoder {
+ public:
+ 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/lib/athena/FPGAEncoder.cpp b/hal/lib/athena/FPGAEncoder.cpp
new file mode 100644
index 0000000..9b0e5ad
--- /dev/null
+++ b/hal/lib/athena/FPGAEncoder.cpp
@@ -0,0 +1,296 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be 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 "HAL/handles/LimitedHandleResource.h"
+#include "PortsInternal.h"
+
+using namespace hal;
+
+namespace {
+struct Encoder {
+ std::unique_ptr<tEncoder> encoder;
+ uint8_t index;
+};
+}
+
+static const double DECODING_SCALING_FACTOR = 0.25;
+
+static LimitedHandleResource<HAL_FPGAEncoderHandle, Encoder, kNumEncoders,
+ HAL_HandleEnum::FPGAEncoder>
+ fpgaEncoderHandles;
+
+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) {
+ 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);
+}
+
+/**
+ * Reset the Encoder distance to zero.
+ * Resets the current count to zero on the encoder.
+ */
+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);
+}
+
+/**
+ * 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) {
+ auto encoder = fpgaEncoderHandles.Get(fpgaEncoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+ return encoder->encoder->readOutput_Value(status);
+}
+
+/**
+ * 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) {
+ 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;
+}
+
+/**
+ * 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) {
+ 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);
+}
+
+/**
+ * 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) {
+ auto encoder = fpgaEncoderHandles.Get(fpgaEncoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return false;
+ }
+ return encoder->encoder->readTimerOutput_Stalled(status) != 0;
+}
+
+/**
+ * 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) {
+ auto encoder = fpgaEncoderHandles.Get(fpgaEncoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return false;
+ }
+ return encoder->encoder->readOutput_Direction(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) {
+ auto encoder = fpgaEncoderHandles.Get(fpgaEncoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ encoder->encoder->writeConfig_Reverse(reverseDirection, 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) {
+ 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);
+}
+
+/**
+ * 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) {
+ auto encoder = fpgaEncoderHandles.Get(fpgaEncoderHandle);
+ if (encoder == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+ return encoder->encoder->readTimerConfig_AverageSize(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) {
+ 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);
+}
+}
diff --git a/hal/lib/athena/FPGAEncoder.h b/hal/lib/athena/FPGAEncoder.h
new file mode 100644
index 0000000..279fa81
--- /dev/null
+++ b/hal/lib/athena/FPGAEncoder.h
@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be 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);
+void HAL_ResetFPGAEncoder(HAL_FPGAEncoderHandle fpgaEncoderHandle,
+ int32_t* status);
+int32_t HAL_GetFPGAEncoder(HAL_FPGAEncoderHandle fpgaEncoderHandle,
+ int32_t* status); // Raw value
+double HAL_GetFPGAEncoderPeriod(HAL_FPGAEncoderHandle fpgaEncoderHandle,
+ int32_t* status);
+void HAL_SetFPGAEncoderMaxPeriod(HAL_FPGAEncoderHandle fpgaEncoderHandle,
+ double maxPeriod, int32_t* status);
+HAL_Bool HAL_GetFPGAEncoderStopped(HAL_FPGAEncoderHandle fpgaEncoderHandle,
+ int32_t* status);
+HAL_Bool HAL_GetFPGAEncoderDirection(HAL_FPGAEncoderHandle fpgaEncoderHandle,
+ int32_t* status);
+void HAL_SetFPGAEncoderReverseDirection(HAL_FPGAEncoderHandle fpgaEncoderHandle,
+ HAL_Bool reverseDirection,
+ int32_t* status);
+void HAL_SetFPGAEncoderSamplesToAverage(HAL_FPGAEncoderHandle fpgaEncoderHandle,
+ int32_t samplesToAverage,
+ int32_t* status);
+int32_t HAL_GetFPGAEncoderSamplesToAverage(
+ HAL_FPGAEncoderHandle fpgaEncoderHandle, int32_t* status);
+void HAL_SetFPGAEncoderIndexSource(HAL_FPGAEncoderHandle fpgaEncoderHandle,
+ HAL_Handle digitalSourceHandle,
+ HAL_AnalogTriggerType analogTriggerType,
+ HAL_Bool activeHigh, HAL_Bool edgeSensitive,
+ int32_t* status);
+}
diff --git a/hal/lib/athena/FRCDriverStation.cpp b/hal/lib/athena/FRCDriverStation.cpp
new file mode 100644
index 0000000..28855db
--- /dev/null
+++ b/hal/lib/athena/FRCDriverStation.cpp
@@ -0,0 +1,260 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be 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 <chrono>
+#include <cstdio>
+#include <cstdlib>
+#include <cstring>
+#include <limits>
+
+#include "FRC_NetworkCommunication/FRCComm.h"
+#include "HAL/DriverStation.h"
+#include "HAL/cpp/priority_condition_variable.h"
+#include "HAL/cpp/priority_mutex.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 priority_mutex msgMutex;
+static priority_condition_variable newDSDataAvailableCond;
+static priority_mutex newDSDataAvailableMutex;
+
+extern "C" {
+int32_t HAL_SetErrorData(const char* errors, int32_t errorsLength,
+ int32_t waitMs) {
+ return setErrorData(errors, errorsLength, waitMs);
+}
+
+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::lock_guard<priority_mutex> 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)) {
+ retval = FRC_NetworkCommunication_sendError(isError, errorCode, isLVCode,
+ details, location, callStack);
+ 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) {
+ std::memset(controlWord, 0, sizeof(HAL_ControlWord));
+ return FRC_NetworkCommunication_getControlWord(
+ reinterpret_cast<ControlWord_t*>(controlWord));
+}
+
+HAL_AllianceStationID HAL_GetAllianceStation(int32_t* status) {
+ HAL_AllianceStationID allianceStation;
+ *status = FRC_NetworkCommunication_getAllianceStation(
+ reinterpret_cast<AllianceStationID_t*>(&allianceStation));
+ return allianceStation;
+}
+
+int32_t HAL_GetJoystickAxes(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;
+}
+
+int32_t HAL_GetJoystickPOVs(int32_t joystickNum, HAL_JoystickPOVs* povs) {
+ return FRC_NetworkCommunication_getJoystickPOVs(
+ joystickNum, reinterpret_cast<JoystickPOV_t*>(povs),
+ HAL_kMaxJoystickPOVs);
+}
+
+int32_t HAL_GetJoystickButtons(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.
+ */
+int32_t HAL_GetJoystickDescriptor(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;
+}
+
+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;
+ }
+}
+
+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();
+}
+
+/**
+ * Waits for the newest DS packet to arrive. Note that this is a blocking call.
+ */
+void HAL_WaitForDSData(void) {
+ std::unique_lock<priority_mutex> lock(newDSDataAvailableMutex);
+ newDSDataAvailableCond.wait(lock);
+}
+
+void HAL_InitializeDriverStation(void) {
+ // Set our DS new data condition variable.
+ setNewDataSem(newDSDataAvailableCond.native_handle());
+}
+
+} // extern "C"
diff --git a/hal/lib/athena/HAL.cpp b/hal/lib/athena/HAL.cpp
new file mode 100644
index 0000000..708ca1b
--- /dev/null
+++ b/hal/lib/athena/HAL.cpp
@@ -0,0 +1,377 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be 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 <iostream>
+#include <mutex>
+#include <thread>
+
+#include "FRC_NetworkCommunication/CANSessionMux.h"
+#include "FRC_NetworkCommunication/FRCComm.h"
+#include "FRC_NetworkCommunication/LoadOut.h"
+#include "HAL/ChipObject.h"
+#include "HAL/DriverStation.h"
+#include "HAL/Errors.h"
+#include "HAL/Notifier.h"
+#include "HAL/cpp/priority_mutex.h"
+#include "HAL/handles/HandlesInternal.h"
+#include "ctre/ctre.h"
+#include "visa/visa.h"
+
+using namespace hal;
+
+static std::unique_ptr<tGlobal> global;
+static std::unique_ptr<tSysWatchdog> watchdog;
+
+static priority_mutex timeMutex;
+static uint32_t timeEpoch = 0;
+static uint32_t prevFPGATime = 0;
+static HAL_NotifierHandle rolloverNotifier = 0;
+
+using 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);
+}
+
+/**
+ * @deprecated Uses module numbers
+ */
+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 ERR_CANSessionMux_InvalidBuffer:
+ return ERR_CANSessionMux_InvalidBuffer_MESSAGE;
+ case ERR_CANSessionMux_MessageNotFound:
+ return ERR_CANSessionMux_MessageNotFound_MESSAGE;
+ case WARN_CANSessionMux_NoToken:
+ return WARN_CANSessionMux_NoToken_MESSAGE;
+ case ERR_CANSessionMux_NotAllowed:
+ return ERR_CANSessionMux_NotAllowed_MESSAGE;
+ case 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_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;
+ default:
+ return "Unknown error status";
+ }
+}
+
+/**
+ * Returns the runtime type of this HAL
+ */
+HAL_RuntimeType HAL_GetRuntimeType() { return HAL_Athena; }
+
+/**
+ * Return the FPGA Version number.
+ * For now, expect this to be competition year.
+ * @return FPGA Version number.
+ */
+int32_t HAL_GetFPGAVersion(int32_t* status) {
+ if (!global) {
+ *status = NiFpga_Status_ResourceNotInitialized;
+ return 0;
+ }
+ return global->readVersion(status);
+}
+
+/**
+ * Return 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) {
+ if (!global) {
+ *status = NiFpga_Status_ResourceNotInitialized;
+ return 0;
+ }
+ return global->readRevision(status);
+}
+
+/**
+ * Read 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) {
+ if (!global) {
+ *status = NiFpga_Status_ResourceNotInitialized;
+ return 0;
+ }
+ std::lock_guard<priority_mutex> lock(timeMutex);
+ uint32_t fpgaTime = global->readLocalTime(status);
+ if (*status != 0) return 0;
+ // check for rollover
+ if (fpgaTime < prevFPGATime) ++timeEpoch;
+ prevFPGATime = fpgaTime;
+ return static_cast<uint64_t>(timeEpoch) << 32 |
+ static_cast<uint64_t>(fpgaTime);
+}
+
+/**
+ * Get 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) {
+ 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 void timerRollover(uint64_t currentTime, HAL_NotifierHandle handle) {
+ // reschedule timer for next rollover
+ int32_t status = 0;
+ HAL_UpdateNotifierAlarm(handle, currentTime + 0x80000000ULL, &status);
+}
+
+void HAL_BaseInitialize(int32_t* status) {
+ static std::atomic_bool initialized{false};
+ static priority_mutex initializeMutex;
+ // Initial check, as if it's true initialization has finished
+ if (initialized) return;
+
+ std::lock_guard<priority_mutex> lock(initializeMutex);
+ // Second check in case another thread was waiting
+ if (initialized) return;
+ // image 4; Fixes errors caused by multiple processes. Talk to NI about this
+ nFPGA::nRoboRIO_FPGANamespace::g_currentTargetClass =
+ nLoadOut::kTargetClass_RoboRIO;
+
+ global.reset(tGlobal::create(status));
+ watchdog.reset(tSysWatchdog::create(status));
+ initialized = true;
+}
+
+/**
+ * Call this to start up HAL. This is required for robot programs.
+ */
+int32_t HAL_Initialize(int32_t mode) {
+ setlinebuf(stdin);
+ setlinebuf(stdout);
+
+ prctl(PR_SET_PDEATHSIG, SIGTERM);
+
+ FRC_NetworkCommunication_Reserve(nullptr);
+
+ std::atexit([]() {
+ // Unregister our new data condition variable.
+ setNewDataSem(nullptr);
+ });
+
+ int32_t status = 0;
+ HAL_BaseInitialize(&status);
+
+ if (!rolloverNotifier)
+ rolloverNotifier = HAL_InitializeNotifier(timerRollover, nullptr, &status);
+ if (status == 0) {
+ uint64_t curTime = HAL_GetFPGATime(&status);
+ if (status == 0)
+ HAL_UpdateNotifierAlarm(rolloverNotifier, curTime + 0x80000000ULL,
+ &status);
+ }
+
+ // 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 0;
+
+ 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()) {
+ std::cout << "Killing previously running FRC program..." << std::endl;
+ kill(pid, SIGTERM); // try to kill it
+ std::this_thread::sleep_for(std::chrono::milliseconds(100));
+ if (kill(pid, 0) == 0) {
+ // still not successfull
+ if (mode == 0) {
+ std::cout << "FRC pid " << pid
+ << " did not die within 110ms. Aborting" << std::endl;
+ return 0; // just fail
+ } else if (mode == 1) { // kill -9 it
+ kill(pid, SIGKILL);
+ } else {
+ std::cout << "WARNING: FRC pid " << pid
+ << " did not die within 110ms." << std::endl;
+ }
+ }
+ }
+ }
+ 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();
+
+ HAL_InitializeDriverStation();
+
+ return 1;
+}
+
+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 RTSetCleanupProc() {}
+void EDVR_CreateReference() {}
+void Occur() {}
+
+} // extern "C"
diff --git a/hal/lib/athena/I2C.cpp b/hal/lib/athena/I2C.cpp
new file mode 100644
index 0000000..8065d37
--- /dev/null
+++ b/hal/lib/athena/I2C.cpp
@@ -0,0 +1,183 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be 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 "DigitalInternal.h"
+#include "HAL/DIO.h"
+#include "HAL/HAL.h"
+#include "i2clib/i2c-lib.h"
+
+using namespace hal;
+
+static priority_recursive_mutex digitalI2COnBoardMutex;
+static priority_recursive_mutex digitalI2CMXPMutex;
+
+static uint8_t i2COnboardObjCount = 0;
+static uint8_t i2CMXPObjCount = 0;
+static uint8_t i2COnBoardHandle = 0;
+static uint8_t i2CMXPHandle = 0;
+
+static HAL_DigitalHandle i2CMXPDigitalHandle1 = HAL_kInvalidHandle;
+static HAL_DigitalHandle i2CMXPDigitalHandle2 = HAL_kInvalidHandle;
+
+extern "C" {
+/*
+ * Initialize 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(int32_t port, int32_t* status) {
+ initializeDigital(status);
+ if (*status != 0) return;
+
+ if (port > 1) {
+ // Set port out of range error here
+ return;
+ }
+
+ priority_recursive_mutex& lock =
+ port == 0 ? digitalI2COnBoardMutex : digitalI2CMXPMutex;
+ {
+ std::lock_guard<priority_recursive_mutex> sync(lock);
+ if (port == 0) {
+ i2COnboardObjCount++;
+ if (i2COnBoardHandle > 0) return;
+ i2COnBoardHandle = i2clib_open("/dev/i2c-2");
+ } else if (port == 1) {
+ i2CMXPObjCount++;
+ if (i2CMXPHandle > 0) 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);
+ i2CMXPHandle = i2clib_open("/dev/i2c-1");
+ }
+ return;
+ }
+}
+
+/**
+ * Generic transaction.
+ *
+ * This is a lower-level interface to the I2C hardware giving you more control
+ * over each transaction.
+ *
+ * @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 The number of bytes read (>= 0) or -1 on transfer abort.
+ */
+int32_t HAL_TransactionI2C(int32_t port, int32_t deviceAddress,
+ uint8_t* dataToSend, int32_t sendSize,
+ uint8_t* dataReceived, int32_t receiveSize) {
+ if (port > 1) {
+ // Set port out of range error here
+ return -1;
+ }
+
+ int32_t handle = port == 0 ? i2COnBoardHandle : i2CMXPHandle;
+ priority_recursive_mutex& lock =
+ port == 0 ? digitalI2COnBoardMutex : digitalI2CMXPMutex;
+
+ {
+ std::lock_guard<priority_recursive_mutex> sync(lock);
+ return i2clib_writeread(
+ handle, deviceAddress, reinterpret_cast<const char*>(dataToSend),
+ static_cast<int32_t>(sendSize), reinterpret_cast<char*>(dataReceived),
+ static_cast<int32_t>(receiveSize));
+ }
+}
+
+/**
+ * Execute a write transaction with the device.
+ *
+ * Write a single byte to a register on a device and wait until the
+ * transaction is complete.
+ *
+ * @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 The number of bytes written (>= 0) or -1 on transfer abort.
+ */
+int32_t HAL_WriteI2C(int32_t port, int32_t deviceAddress, uint8_t* dataToSend,
+ int32_t sendSize) {
+ if (port > 1) {
+ // Set port out of range error here
+ return -1;
+ }
+
+ int32_t handle = port == 0 ? i2COnBoardHandle : i2CMXPHandle;
+ priority_recursive_mutex& lock =
+ port == 0 ? digitalI2COnBoardMutex : digitalI2CMXPMutex;
+ {
+ std::lock_guard<priority_recursive_mutex> sync(lock);
+ return i2clib_write(handle, deviceAddress,
+ reinterpret_cast<const char*>(dataToSend), sendSize);
+ }
+}
+
+/**
+ * Execute a read transaction with the device.
+ *
+ * Read 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 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 The number of bytes read (>= 0) or -1 on transfer abort.
+ */
+int32_t HAL_ReadI2C(int32_t port, int32_t deviceAddress, uint8_t* buffer,
+ int32_t count) {
+ if (port > 1) {
+ // Set port out of range error here
+ return -1;
+ }
+
+ int32_t handle = port == 0 ? i2COnBoardHandle : i2CMXPHandle;
+ priority_recursive_mutex& lock =
+ port == 0 ? digitalI2COnBoardMutex : digitalI2CMXPMutex;
+ {
+ std::lock_guard<priority_recursive_mutex> sync(lock);
+ return i2clib_read(handle, deviceAddress, reinterpret_cast<char*>(buffer),
+ static_cast<int32_t>(count));
+ }
+}
+
+void HAL_CloseI2C(int32_t port) {
+ if (port > 1) {
+ // Set port out of range error here
+ return;
+ }
+ priority_recursive_mutex& lock =
+ port == 0 ? digitalI2COnBoardMutex : digitalI2CMXPMutex;
+ {
+ std::lock_guard<priority_recursive_mutex> sync(lock);
+ if ((port == 0 ? i2COnboardObjCount-- : i2CMXPObjCount--) == 0) {
+ int32_t handle = port == 0 ? i2COnBoardHandle : i2CMXPHandle;
+ i2clib_close(handle);
+ }
+ }
+
+ if (port == 1) {
+ HAL_FreeDIOPort(i2CMXPDigitalHandle1);
+ HAL_FreeDIOPort(i2CMXPDigitalHandle2);
+ }
+ return;
+}
+}
diff --git a/hal/lib/athena/Interrupts.cpp b/hal/lib/athena/Interrupts.cpp
new file mode 100644
index 0000000..571253e
--- /dev/null
+++ b/hal/lib/athena/Interrupts.cpp
@@ -0,0 +1,258 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be 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 "DigitalInternal.h"
+#include "HAL/ChipObject.h"
+#include "HAL/Errors.h"
+#include "HAL/cpp/make_unique.h"
+#include "HAL/handles/HandlesInternal.h"
+#include "HAL/handles/LimitedHandleResource.h"
+#include "PortsInternal.h"
+#include "support/SafeThread.h"
+
+using namespace hal;
+
+namespace {
+struct Interrupt {
+ std::unique_ptr<tInterrupt> anInterrupt;
+ std::unique_ptr<tInterruptManager> manager;
+};
+
+// Safe thread to allow callbacks to run on their own thread
+class InterruptThread : public wpi::SafeThread {
+ public:
+ void Main() {
+ std::unique_lock<std::mutex> 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();
+ }
+};
+
+} // 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;
+
+extern "C" {
+
+HAL_InterruptHandle HAL_InitializeInterrupts(HAL_Bool watcher,
+ int32_t* status) {
+ 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) {
+ interruptHandles.Free(interruptHandle);
+}
+
+/**
+ * In synchronous mode, wait for the defined interrupt to occur.
+ * @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) {
+ 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;
+}
+
+/**
+ * Enable 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.
+ */
+void HAL_EnableInterrupts(HAL_InterruptHandle interruptHandle,
+ int32_t* status) {
+ auto anInterrupt = interruptHandles.Get(interruptHandle);
+ if (anInterrupt == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ anInterrupt->manager->enable(status);
+}
+
+/**
+ * Disable Interrupts without without deallocating structures.
+ */
+void HAL_DisableInterrupts(HAL_InterruptHandle interruptHandle,
+ int32_t* status) {
+ auto anInterrupt = interruptHandles.Get(interruptHandle);
+ if (anInterrupt == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ anInterrupt->manager->disable(status);
+}
+
+/**
+ * Return the timestamp for the rising interrupt that occurred most recently.
+ * This is in the same time domain as GetClock().
+ * @return Timestamp in seconds since boot.
+ */
+double 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 * 1e-6;
+}
+
+/**
+* Return the timestamp for the falling interrupt that occurred most recently.
+* This is in the same time domain as GetClock().
+* @return Timestamp in seconds since boot.
+*/
+double 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 * 1e-6;
+}
+
+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);
+}
+
+void HAL_AttachInterruptHandlerThreaded(HAL_InterruptHandle interrupt_handle,
+ HAL_InterruptHandlerFunction handler,
+ void* param, int32_t* status) {
+ InterruptThreadOwner* intr = new InterruptThreadOwner;
+ intr->Start();
+ intr->SetFunc(handler, param);
+
+ HAL_AttachInterruptHandler(interrupt_handle, threadedInterruptHandler, intr,
+ status);
+
+ if (*status != 0) {
+ delete intr;
+ }
+}
+
+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/lib/athena/Notifier.cpp b/hal/lib/athena/Notifier.cpp
new file mode 100644
index 0000000..3bedb68
--- /dev/null
+++ b/hal/lib/athena/Notifier.cpp
@@ -0,0 +1,299 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be 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"
+
+// For std::atexit()
+#include <cstdlib>
+
+#include <atomic>
+#include <memory>
+#include <mutex>
+
+#include "HAL/ChipObject.h"
+#include "HAL/Errors.h"
+#include "HAL/HAL.h"
+#include "HAL/cpp/make_unique.h"
+#include "HAL/cpp/priority_mutex.h"
+#include "HAL/handles/UnlimitedHandleResource.h"
+#include "support/SafeThread.h"
+
+using namespace hal;
+
+static const int32_t kTimerInterruptNumber = 28;
+
+static priority_mutex notifierInterruptMutex;
+static priority_recursive_mutex notifierMutex;
+static std::unique_ptr<tAlarm> notifierAlarm;
+static std::unique_ptr<tInterruptManager> notifierManager;
+static uint64_t closestTrigger = UINT64_MAX;
+
+namespace {
+struct Notifier {
+ std::shared_ptr<Notifier> prev, next;
+ void* param;
+ HAL_NotifierProcessFunction process;
+ uint64_t triggerTime = UINT64_MAX;
+ HAL_NotifierHandle handle;
+ bool threaded;
+};
+
+// Safe thread to allow callbacks to run on their own thread
+class NotifierThread : public wpi::SafeThread {
+ public:
+ void Main() {
+ std::unique_lock<std::mutex> lock(m_mutex);
+ while (m_active) {
+ m_cond.wait(lock, [&] { return !m_active || m_notify; });
+ if (!m_active) break;
+ m_notify = false;
+ uint64_t currentTime = m_currentTime;
+ HAL_NotifierHandle handle = m_handle;
+ HAL_NotifierProcessFunction process = m_process;
+ lock.unlock(); // don't hold mutex during callback execution
+ process(currentTime, handle);
+ lock.lock();
+ }
+ }
+
+ bool m_notify = false;
+ HAL_NotifierHandle m_handle = HAL_kInvalidHandle;
+ HAL_NotifierProcessFunction m_process;
+ uint64_t m_currentTime;
+};
+
+class NotifierThreadOwner : public wpi::SafeThreadOwner<NotifierThread> {
+ public:
+ void SetFunc(HAL_NotifierProcessFunction process, void* param) {
+ auto thr = GetThread();
+ if (!thr) return;
+ thr->m_process = process;
+ m_param = param;
+ }
+
+ void Notify(uint64_t currentTime, HAL_NotifierHandle handle) {
+ auto thr = GetThread();
+ if (!thr) return;
+ thr->m_currentTime = currentTime;
+ thr->m_handle = handle;
+ thr->m_notify = true;
+ thr->m_cond.notify_one();
+ }
+
+ void* m_param;
+};
+} // namespace
+
+static std::shared_ptr<Notifier> notifiers;
+static std::atomic_flag notifierAtexitRegistered = ATOMIC_FLAG_INIT;
+static std::atomic_int notifierRefCount{0};
+
+using namespace hal;
+
+static UnlimitedHandleResource<HAL_NotifierHandle, Notifier,
+ HAL_HandleEnum::Notifier>
+ notifierHandles;
+
+// internal version of updateAlarm used during the alarmCallback when we know
+// that the pointer is a valid pointer.
+void updateNotifierAlarmInternal(std::shared_ptr<Notifier> notifierPointer,
+ uint64_t triggerTime, int32_t* status) {
+ std::lock_guard<priority_recursive_mutex> sync(notifierMutex);
+
+ auto notifier = notifierPointer;
+ // no need for a null check, as this must always be a valid pointer.
+ notifier->triggerTime = triggerTime;
+ bool wasActive = (closestTrigger != UINT64_MAX);
+
+ if (!notifierInterruptMutex.try_lock() || notifierRefCount == 0 ||
+ !notifierAlarm)
+ return;
+
+ // Update alarm time if closer than current.
+ if (triggerTime < closestTrigger) {
+ closestTrigger = triggerTime;
+ // Simply truncate the hardware trigger time to 32-bit.
+ notifierAlarm->writeTriggerTime(static_cast<uint32_t>(triggerTime), status);
+ }
+ // Enable the alarm. The hardware disables itself after each alarm.
+ if (!wasActive) notifierAlarm->writeEnable(true, status);
+
+ notifierInterruptMutex.unlock();
+}
+
+static void alarmCallback(uint32_t, void*) {
+ std::unique_lock<priority_recursive_mutex> sync(notifierMutex);
+
+ int32_t status = 0;
+ uint64_t currentTime = 0;
+
+ // the hardware disables itself after each alarm
+ closestTrigger = UINT64_MAX;
+
+ // process all notifiers
+ std::shared_ptr<Notifier> notifier = notifiers;
+ while (notifier) {
+ if (notifier->triggerTime != UINT64_MAX) {
+ if (currentTime == 0) currentTime = HAL_GetFPGATime(&status);
+ if (notifier->triggerTime < currentTime) {
+ notifier->triggerTime = UINT64_MAX;
+ auto process = notifier->process;
+ auto handle = notifier->handle;
+ sync.unlock();
+ process(currentTime, handle);
+ sync.lock();
+ } else if (notifier->triggerTime < closestTrigger) {
+ updateNotifierAlarmInternal(notifier, notifier->triggerTime, &status);
+ }
+ }
+ notifier = notifier->next;
+ }
+}
+
+static void cleanupNotifierAtExit() {
+ notifierAlarm = nullptr;
+ notifierManager = nullptr;
+}
+
+static void threadedNotifierHandler(uint64_t currentTimeInt,
+ HAL_NotifierHandle handle) {
+ // Grab notifier and get handler param
+ auto notifier = notifierHandles.Get(handle);
+ if (!notifier) return;
+ auto notifierPointer = notifier->param;
+ if (notifierPointer == nullptr) return;
+ NotifierThreadOwner* owner =
+ static_cast<NotifierThreadOwner*>(notifierPointer);
+ owner->Notify(currentTimeInt, handle);
+}
+
+extern "C" {
+
+HAL_NotifierHandle HAL_InitializeNotifier(HAL_NotifierProcessFunction process,
+ void* param, int32_t* status) {
+ if (!process) {
+ *status = NULL_PARAMETER;
+ return 0;
+ }
+ if (!notifierAtexitRegistered.test_and_set())
+ std::atexit(cleanupNotifierAtExit);
+ if (notifierRefCount.fetch_add(1) == 0) {
+ std::lock_guard<priority_mutex> sync(notifierInterruptMutex);
+ // 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::lock_guard<priority_recursive_mutex> sync(notifierMutex);
+ 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;
+ }
+ // create notifier structure and add to list
+ notifier->next = notifiers;
+ if (notifier->next) notifier->next->prev = notifier;
+ notifier->param = param;
+ notifier->process = process;
+ notifier->handle = handle;
+ notifier->threaded = false;
+ notifiers = notifier;
+ return handle;
+}
+
+HAL_NotifierHandle HAL_InitializeNotifierThreaded(
+ HAL_NotifierProcessFunction process, void* param, int32_t* status) {
+ NotifierThreadOwner* notify = new NotifierThreadOwner;
+ notify->Start();
+ notify->SetFunc(process, param);
+
+ auto notifierHandle =
+ HAL_InitializeNotifier(threadedNotifierHandler, notify, status);
+
+ if (notifierHandle == HAL_kInvalidHandle || *status != 0) {
+ delete notify;
+ return HAL_kInvalidHandle;
+ }
+
+ auto notifier = notifierHandles.Get(notifierHandle);
+ if (!notifier) {
+ return HAL_kInvalidHandle;
+ }
+ notifier->threaded = true;
+
+ return notifierHandle;
+}
+
+void HAL_CleanNotifier(HAL_NotifierHandle notifierHandle, int32_t* status) {
+ {
+ std::lock_guard<priority_recursive_mutex> sync(notifierMutex);
+ auto notifier = notifierHandles.Get(notifierHandle);
+ if (!notifier) return;
+
+ // remove from list
+ if (notifier->prev) notifier->prev->next = notifier->next;
+ if (notifier->next) notifier->next->prev = notifier->prev;
+ if (notifiers == notifier) notifiers = notifier->next;
+ notifierHandles.Free(notifierHandle);
+
+ if (notifier->threaded) {
+ NotifierThreadOwner* owner =
+ static_cast<NotifierThreadOwner*>(notifier->param);
+ delete owner;
+ }
+ }
+
+ if (notifierRefCount.fetch_sub(1) == 1) {
+ std::lock_guard<priority_mutex> sync(notifierInterruptMutex);
+ // if this was the last notifier, clean up alarm and manager
+ if (notifierAlarm) {
+ notifierAlarm->writeEnable(false, status);
+ notifierAlarm = nullptr;
+ }
+ if (notifierManager) {
+ notifierManager->disable(status);
+ notifierManager = nullptr;
+ }
+ closestTrigger = UINT64_MAX;
+ }
+}
+
+void* HAL_GetNotifierParam(HAL_NotifierHandle notifierHandle, int32_t* status) {
+ auto notifier = notifierHandles.Get(notifierHandle);
+ if (!notifier) return nullptr;
+ if (notifier->threaded) {
+ // If threaded, return thread param rather then notifier param
+ NotifierThreadOwner* owner =
+ static_cast<NotifierThreadOwner*>(notifier->param);
+ return owner->m_param;
+ }
+ return notifier->param;
+}
+
+void HAL_UpdateNotifierAlarm(HAL_NotifierHandle notifierHandle,
+ uint64_t triggerTime, int32_t* status) {
+ std::lock_guard<priority_recursive_mutex> sync(notifierMutex);
+
+ auto notifier = notifierHandles.Get(notifierHandle);
+ if (!notifier) return;
+ updateNotifierAlarmInternal(notifier, triggerTime, status);
+}
+
+void HAL_StopNotifierAlarm(HAL_NotifierHandle notifierHandle, int32_t* status) {
+ std::lock_guard<priority_recursive_mutex> sync(notifierMutex);
+ auto notifier = notifierHandles.Get(notifierHandle);
+ if (!notifier) return;
+ notifier->triggerTime = UINT64_MAX;
+}
+
+} // extern "C"
diff --git a/hal/lib/athena/OSSerialPort.cpp b/hal/lib/athena/OSSerialPort.cpp
new file mode 100644
index 0000000..95c467d
--- /dev/null
+++ b/hal/lib/athena/OSSerialPort.cpp
@@ -0,0 +1,232 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2017. All Rights Reserved. */
+/* Open Source Software - may be 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/OSSerialPort.h"
+
+#include <fcntl.h>
+#include <sys/ioctl.h>
+#include <termios.h>
+#include <unistd.h>
+
+#include <chrono>
+#include <cstring>
+#include <string>
+
+#include "HAL/Errors.h"
+#include "HAL/cpp/SerialHelper.h"
+
+static int portHandles[4]{-1, -1, -1, -1};
+static std::chrono::milliseconds portTimeouts[4]{
+ std::chrono::milliseconds(0), std::chrono::milliseconds(0),
+ std::chrono::milliseconds(0), std::chrono::milliseconds(0)};
+
+extern "C" {
+
+void HAL_InitializeOSSerialPort(HAL_SerialPort port, int32_t* status) {
+ std::string portName;
+
+ hal::SerialHelper serialHelper;
+
+ portName = serialHelper.GetOSSerialPortName(port, status);
+
+ if (*status < 0) {
+ return;
+ }
+
+ int fs = open(portName.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
+ if (fs == -1) {
+ *status = HAL_SERIAL_PORT_OPEN_ERROR;
+ return;
+ }
+ portHandles[port] = fs;
+
+ struct termios options;
+ tcgetattr(fs, &options);
+ options.c_cflag = B9600 | CS8 | CLOCAL | CREAD;
+ options.c_iflag = 0;
+ options.c_oflag = 0;
+ options.c_lflag = 0;
+ tcflush(fs, TCIFLUSH);
+ tcsetattr(fs, TCSANOW, &options);
+}
+
+void HAL_SetOSSerialBaudRate(HAL_SerialPort port, int32_t baud,
+ int32_t* status) {
+ int baudRate = -1;
+ switch (baud) {
+ case 9600:
+ baudRate = B9600;
+ break;
+ case 19200:
+ baudRate = B19200;
+ break;
+ case 38400:
+ baudRate = B38400;
+ break;
+ case 57600:
+ baudRate = B57600;
+ break;
+ case 115200:
+ baudRate = B115200;
+ break;
+ default:
+ *status = PARAMETER_OUT_OF_RANGE;
+ return;
+ }
+
+ struct termios options;
+ tcgetattr(portHandles[port], &options);
+ auto set = cfsetospeed(&options, baudRate);
+ if (set != 0) {
+ *status = HAL_SERIAL_PORT_ERROR;
+ return;
+ }
+ set = tcsetattr(portHandles[port], TCSANOW, &options);
+ if (set != 0) {
+ *status = HAL_SERIAL_PORT_ERROR;
+ return;
+ }
+}
+
+void HAL_SetOSSerialDataBits(HAL_SerialPort port, int32_t bits,
+ int32_t* status) {
+ int numBits = -1;
+ switch (bits) {
+ case 5:
+ numBits = CS5;
+ break;
+ case 6:
+ numBits = CS6;
+ break;
+ case 7:
+ numBits = CS7;
+ break;
+ case 8:
+ numBits = CS8;
+ break;
+ default:
+ *status = PARAMETER_OUT_OF_RANGE;
+ return;
+ }
+
+ struct termios options;
+ tcgetattr(portHandles[port], &options);
+ options.c_cflag &= ~CSIZE;
+ options.c_cflag |= numBits;
+ auto set = tcsetattr(portHandles[port], TCSANOW, &options);
+ if (set != 0) {
+ *status = HAL_SERIAL_PORT_ERROR;
+ return;
+ }
+}
+
+void HAL_SetOSSerialParity(HAL_SerialPort port, int32_t parity,
+ int32_t* status) {
+ // Just set none parity
+ struct termios options;
+ tcgetattr(portHandles[port], &options);
+ options.c_cflag &= ~PARENB;
+ auto set = tcsetattr(portHandles[port], TCSANOW, &options);
+ if (set != 0) {
+ *status = HAL_SERIAL_PORT_ERROR;
+ return;
+ }
+}
+
+void HAL_SetOSSerialStopBits(HAL_SerialPort port, int32_t stopBits,
+ int32_t* status) {
+ // Force 1 stop bit
+ struct termios options;
+ tcgetattr(portHandles[port], &options);
+ options.c_cflag &= ~CSTOPB;
+ auto set = tcsetattr(portHandles[port], TCSANOW, &options);
+ if (set != 0) {
+ *status = HAL_SERIAL_PORT_ERROR;
+ return;
+ }
+}
+
+void HAL_SetOSSerialWriteMode(HAL_SerialPort port, int32_t mode,
+ int32_t* status) {
+ // No op
+}
+
+void HAL_SetOSSerialFlowControl(HAL_SerialPort port, int32_t flow,
+ int32_t* status) {
+ // No op
+}
+
+void HAL_SetOSSerialTimeout(HAL_SerialPort port, double timeout,
+ int32_t* status) {
+ // Convert to millis
+ int t = timeout / 1000;
+ portTimeouts[port] = std::chrono::milliseconds(t);
+}
+
+void HAL_EnableOSSerialTermination(HAL_SerialPort port, char terminator,
+ int32_t* status) {
+ // \n is hardcoded for now. Will fix later
+ // Seems like a VISA only setting, need to check
+}
+
+void HAL_DisableOSSerialTermination(HAL_SerialPort port, int32_t* status) {
+ // Seems like a VISA only setting, need to check
+}
+
+void HAL_SetOSSerialReadBufferSize(HAL_SerialPort port, int32_t size,
+ int32_t* status) {
+ // No op
+}
+
+void HAL_SetOSSerialWriteBufferSize(HAL_SerialPort port, int32_t size,
+ int32_t* status) {
+ // No op
+}
+
+int32_t HAL_GetOSSerialBytesReceived(HAL_SerialPort port, int32_t* status) {
+ int bytes = 0;
+ ioctl(portHandles[port], FIONREAD, &bytes);
+ return bytes;
+}
+
+int32_t HAL_ReadOSSerial(HAL_SerialPort port, char* buffer, int32_t count,
+ int32_t* status) {
+ auto endTime = std::chrono::steady_clock::now() + portTimeouts[port];
+
+ int bytesRead = 0;
+
+ unsigned char buf[256];
+
+ do {
+ int rx = read(portHandles[port], buf, count - bytesRead);
+ std::memcpy(&buffer[bytesRead], buf, rx);
+ bytesRead += rx;
+ if (bytesRead >= count) break;
+ llvm::StringRef tmp(buffer, bytesRead);
+ auto loc = tmp.find('\n');
+ if (loc != llvm::StringRef::npos) {
+ bytesRead = loc;
+ break;
+ }
+ } while (std::chrono::steady_clock::now() < endTime);
+ return bytesRead;
+}
+
+int32_t HAL_WriteOSSerial(HAL_SerialPort port, const char* buffer,
+ int32_t count, int32_t* status) {
+ return write(portHandles[port], buffer, count);
+}
+void HAL_FlushOSSerial(HAL_SerialPort port, int32_t* status) {
+ tcdrain(portHandles[port]);
+}
+void HAL_ClearOSSerial(HAL_SerialPort port, int32_t* status) {
+ tcflush(portHandles[port], TCIOFLUSH);
+}
+void HAL_CloseOSSerial(HAL_SerialPort port, int32_t* status) {
+ close(portHandles[port]);
+}
+}
diff --git a/hal/lib/athena/PCMInternal.cpp b/hal/lib/athena/PCMInternal.cpp
new file mode 100644
index 0000000..c767a8f
--- /dev/null
+++ b/hal/lib/athena/PCMInternal.cpp
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be 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 "HAL/Errors.h"
+#include "HAL/Solenoid.h"
+#include "HAL/cpp/make_unique.h"
+#include "PortsInternal.h"
+
+namespace hal {
+std::unique_ptr<PCM> PCM_modules[kNumPCMModules];
+
+void initializePCM(int32_t module, int32_t* status) {
+ 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/lib/athena/PCMInternal.h b/hal/lib/athena/PCMInternal.h
new file mode 100644
index 0000000..4d47792
--- /dev/null
+++ b/hal/lib/athena/PCMInternal.h
@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be 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 "HAL/Errors.h"
+#include "HAL/Ports.h"
+#include "HAL/Solenoid.h"
+#include "PortsInternal.h"
+#include "ctre/PCM.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/lib/athena/PDP.cpp b/hal/lib/athena/PDP.cpp
new file mode 100644
index 0000000..61b8c5b
--- /dev/null
+++ b/hal/lib/athena/PDP.cpp
@@ -0,0 +1,127 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be 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 "HAL/Errors.h"
+#include "HAL/Ports.h"
+#include "HAL/cpp/make_unique.h"
+#include "PortsInternal.h"
+#include "ctre/PDP.h"
+
+using namespace hal;
+
+static std::unique_ptr<PDP> pdp[kNumPDPModules];
+
+static inline bool checkPDPInit(int32_t module, int32_t* status) {
+ if (!HAL_CheckPDPModule(module)) {
+ *status = RESOURCE_OUT_OF_RANGE;
+ return false;
+ }
+ if (!pdp[module]) {
+ *status = INCOMPATIBLE_STATE;
+ return false;
+ }
+ return true;
+}
+
+extern "C" {
+
+void HAL_InitializePDP(int32_t module, int32_t* status) {
+ if (!HAL_CheckPDPModule(module)) {
+ *status = RESOURCE_OUT_OF_RANGE;
+ return;
+ }
+ if (!pdp[module]) {
+ pdp[module] = std::make_unique<PDP>(module);
+ }
+}
+
+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(int32_t module, int32_t* status) {
+ if (!checkPDPInit(module, status)) return 0;
+
+ double temperature;
+
+ *status = pdp[module]->GetTemperature(temperature);
+
+ return temperature;
+}
+
+double HAL_GetPDPVoltage(int32_t module, int32_t* status) {
+ if (!checkPDPInit(module, status)) return 0;
+
+ double voltage;
+
+ *status = pdp[module]->GetVoltage(voltage);
+
+ return voltage;
+}
+
+double HAL_GetPDPChannelCurrent(int32_t module, int32_t channel,
+ int32_t* status) {
+ if (!checkPDPInit(module, status)) return 0;
+
+ double current;
+
+ *status = pdp[module]->GetChannelCurrent(channel, current);
+
+ return current;
+}
+
+double HAL_GetPDPTotalCurrent(int32_t module, int32_t* status) {
+ if (!checkPDPInit(module, status)) return 0;
+
+ double current;
+
+ *status = pdp[module]->GetTotalCurrent(current);
+
+ return current;
+}
+
+double HAL_GetPDPTotalPower(int32_t module, int32_t* status) {
+ if (!checkPDPInit(module, status)) return 0;
+
+ double power;
+
+ *status = pdp[module]->GetTotalPower(power);
+
+ return power;
+}
+
+double HAL_GetPDPTotalEnergy(int32_t module, int32_t* status) {
+ if (!checkPDPInit(module, status)) return 0;
+
+ double energy;
+
+ *status = pdp[module]->GetTotalEnergy(energy);
+
+ return energy;
+}
+
+void HAL_ResetPDPTotalEnergy(int32_t module, int32_t* status) {
+ if (!checkPDPInit(module, status)) return;
+
+ *status = pdp[module]->ResetEnergy();
+}
+
+void HAL_ClearPDPStickyFaults(int32_t module, int32_t* status) {
+ if (!checkPDPInit(module, status)) return;
+
+ *status = pdp[module]->ClearStickyFaults();
+}
+
+} // extern "C"
diff --git a/hal/lib/athena/PWM.cpp b/hal/lib/athena/PWM.cpp
new file mode 100644
index 0000000..a193842
--- /dev/null
+++ b/hal/lib/athena/PWM.cpp
@@ -0,0 +1,452 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be 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 "ConstantsInternal.h"
+#include "DigitalInternal.h"
+#include "HAL/handles/HandlesInternal.h"
+#include "PortsInternal.h"
+
+using namespace hal;
+
+static inline int32_t GetMaxPositivePwm(DigitalPort* port) {
+ return port->maxPwm;
+}
+static inline int32_t GetMinPositivePwm(DigitalPort* port) {
+ return port->eliminateDeadband ? port->deadbandMaxPwm : port->centerPwm + 1;
+}
+static inline int32_t GetCenterPwm(DigitalPort* port) {
+ return port->centerPwm;
+}
+static inline int32_t GetMaxNegativePwm(DigitalPort* port) {
+ return port->eliminateDeadband ? port->deadbandMinPwm : 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.
+
+extern "C" {
+
+HAL_DigitalHandle HAL_InitializePWMPort(HAL_PortHandle portHandle,
+ int32_t* status) {
+ 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;
+
+ int32_t bitToSet = 1 << remapMXPPWMChannel(port->channel);
+ uint16_t specialFunctions =
+ digitalSystem->readEnableMXPSpecialFunction(status);
+ digitalSystem->writeEnableMXPSpecialFunction(specialFunctions | bitToSet,
+ 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;
+ }
+
+ if (port->channel > tPWM::kNumHdrRegisters - 1) {
+ int32_t bitToUnset = 1 << remapMXPPWMChannel(port->channel);
+ uint16_t specialFunctions =
+ digitalSystem->readEnableMXPSpecialFunction(status);
+ digitalSystem->writeEnableMXPSpecialFunction(specialFunctions & ~bitToUnset,
+ status);
+ }
+
+ 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_GetLoopTiming(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;
+}
+
+/**
+ * Set a PWM channel to the desired value. The values range from 0 to 255 and
+ * the period is controlled
+ * by the PWM Period and MinHigh registers.
+ *
+ * @param channel The PWM channel to set.
+ * @param value The PWM value to set.
+ */
+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);
+ }
+}
+
+/**
+ * Set 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 channel The PWM channel to set.
+ * @param value The scaled PWM value to set.
+ */
+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 (speed < -1.0) {
+ speed = -1.0;
+ } else if (speed > 1.0) {
+ speed = 1.0;
+ } else if (!std::isfinite(speed)) {
+ 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);
+}
+
+/**
+ * Set 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 channel The PWM channel to set.
+ * @param value The scaled PWM value to set.
+ */
+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);
+}
+
+/**
+ * Get a value from a PWM channel. The values range from 0 to 255.
+ *
+ * @param channel The PWM channel to read from.
+ * @return The raw PWM value.
+ */
+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);
+ }
+}
+
+/**
+ * Get a scaled value from a PWM channel. The values range from -1 to 1.
+ *
+ * @param channel The PWM channel to read from.
+ * @return The scaled PWM value.
+ */
+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;
+ }
+}
+
+/**
+ * Get a position value from a PWM channel. The values range from 0 to 1.
+ *
+ * @param channel The PWM channel to read from.
+ * @return The scaled PWM value.
+ */
+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);
+}
+
+/**
+ * Set how how often the PWM signal is squelched, thus scaling the period.
+ *
+ * @param channel The PWM channel to configure.
+ * @param squelchMask The 2-bit mask of outputs to squelch.
+ */
+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);
+ }
+}
+
+/**
+ * Get the loop timing of the PWM system
+ *
+ * @return The loop time
+ */
+int32_t HAL_GetLoopTiming(int32_t* status) {
+ initializeDigital(status);
+ if (*status != 0) return 0;
+ return pwmSystem->readLoopTiming(status);
+}
+}
diff --git a/hal/lib/athena/Ports.cpp b/hal/lib/athena/Ports.cpp
new file mode 100644
index 0000000..98246bd
--- /dev/null
+++ b/hal/lib/athena/Ports.cpp
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be 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;
+
+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; }
+}
diff --git a/hal/lib/athena/PortsInternal.h b/hal/lib/athena/PortsInternal.h
new file mode 100644
index 0000000..391c38b
--- /dev/null
+++ b/hal/lib/athena/PortsInternal.h
@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be 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;
+} // namespace hal
diff --git a/hal/lib/athena/Power.cpp b/hal/lib/athena/Power.cpp
new file mode 100644
index 0000000..4d66fe2
--- /dev/null
+++ b/hal/lib/athena/Power.cpp
@@ -0,0 +1,141 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be 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 "HAL/ChipObject.h"
+
+using namespace hal;
+
+static std::unique_ptr<tPower> power;
+
+static void initializePower(int32_t* status) {
+ if (power == nullptr) {
+ power.reset(tPower::create(status));
+ }
+}
+
+extern "C" {
+
+/**
+ * Get the roboRIO input voltage
+ */
+double HAL_GetVinVoltage(int32_t* status) {
+ initializePower(status);
+ return power->readVinVoltage(status) / 4.096 * 0.025733 - 0.029;
+}
+
+/**
+ * Get the roboRIO input current
+ */
+double HAL_GetVinCurrent(int32_t* status) {
+ initializePower(status);
+ return power->readVinCurrent(status) / 4.096 * 0.017042 - 0.071;
+}
+
+/**
+ * Get the 6V rail voltage
+ */
+double HAL_GetUserVoltage6V(int32_t* status) {
+ initializePower(status);
+ return power->readUserVoltage6V(status) / 4.096 * 0.007019 - 0.014;
+}
+
+/**
+ * Get the 6V rail current
+ */
+double HAL_GetUserCurrent6V(int32_t* status) {
+ initializePower(status);
+ return power->readUserCurrent6V(status) / 4.096 * 0.005566 - 0.009;
+}
+
+/**
+ * Get the active state of the 6V rail
+ */
+HAL_Bool HAL_GetUserActive6V(int32_t* status) {
+ initializePower(status);
+ return power->readStatus_User6V(status) == 4;
+}
+
+/**
+ * Get the fault count for the 6V rail
+ */
+int32_t HAL_GetUserCurrentFaults6V(int32_t* status) {
+ initializePower(status);
+ return static_cast<int32_t>(
+ power->readFaultCounts_OverCurrentFaultCount6V(status));
+}
+
+/**
+ * Get the 5V rail voltage
+ */
+double HAL_GetUserVoltage5V(int32_t* status) {
+ initializePower(status);
+ return power->readUserVoltage5V(status) / 4.096 * 0.005962 - 0.013;
+}
+
+/**
+ * Get the 5V rail current
+ */
+double HAL_GetUserCurrent5V(int32_t* status) {
+ initializePower(status);
+ return power->readUserCurrent5V(status) / 4.096 * 0.001996 - 0.002;
+}
+
+/**
+ * Get the active state of the 5V rail
+ */
+HAL_Bool HAL_GetUserActive5V(int32_t* status) {
+ initializePower(status);
+ return power->readStatus_User5V(status) == 4;
+}
+
+/**
+ * Get the fault count for the 5V rail
+ */
+int32_t HAL_GetUserCurrentFaults5V(int32_t* status) {
+ initializePower(status);
+ return static_cast<int32_t>(
+ power->readFaultCounts_OverCurrentFaultCount5V(status));
+}
+
+/**
+ * Get the 3.3V rail voltage
+ */
+double HAL_GetUserVoltage3V3(int32_t* status) {
+ initializePower(status);
+ return power->readUserVoltage3V3(status) / 4.096 * 0.004902 - 0.01;
+}
+
+/**
+ * Get the 3.3V rail current
+ */
+double HAL_GetUserCurrent3V3(int32_t* status) {
+ initializePower(status);
+ return power->readUserCurrent3V3(status) / 4.096 * 0.002486 - 0.003;
+}
+
+/**
+ * Get the active state of the 3.3V rail
+ */
+HAL_Bool HAL_GetUserActive3V3(int32_t* status) {
+ initializePower(status);
+ return power->readStatus_User3V3(status) == 4;
+}
+
+/**
+ * Get the fault count for the 3.3V rail
+ */
+int32_t HAL_GetUserCurrentFaults3V3(int32_t* status) {
+ initializePower(status);
+ return static_cast<int32_t>(
+ power->readFaultCounts_OverCurrentFaultCount3V3(status));
+}
+
+} // extern "C"
diff --git a/hal/lib/athena/Relay.cpp b/hal/lib/athena/Relay.cpp
new file mode 100644
index 0000000..3c39869
--- /dev/null
+++ b/hal/lib/athena/Relay.cpp
@@ -0,0 +1,134 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be 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 "HAL/handles/IndexedHandleResource.h"
+#include "PortsInternal.h"
+
+using namespace hal;
+
+namespace {
+struct Relay {
+ uint8_t channel;
+ bool fwd;
+};
+}
+
+static IndexedHandleResource<HAL_RelayHandle, Relay, kNumRelayChannels,
+ HAL_HandleEnum::Relay>
+ relayHandles;
+
+// Create a mutex to protect changes to the relay values
+static priority_recursive_mutex digitalRelayMutex;
+
+extern "C" {
+HAL_RelayHandle HAL_InitializeRelayPort(HAL_PortHandle portHandle, HAL_Bool fwd,
+ int32_t* status) {
+ 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;
+}
+
+/**
+ * Set the state of a relay.
+ * Set the state of a relay output.
+ */
+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::lock_guard<priority_recursive_mutex> sync(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);
+ }
+}
+
+/**
+ * Get the current state of the relay channel
+ */
+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;
+}
+}
diff --git a/hal/lib/athena/SPI.cpp b/hal/lib/athena/SPI.cpp
new file mode 100644
index 0000000..d64f1c2
--- /dev/null
+++ b/hal/lib/athena/SPI.cpp
@@ -0,0 +1,665 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be 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 <atomic>
+#include <cstdio>
+
+#include "DigitalInternal.h"
+#include "HAL/DIO.h"
+#include "HAL/HAL.h"
+#include "HAL/Notifier.h"
+#include "HAL/cpp/make_unique.h"
+#include "HAL/cpp/priority_mutex.h"
+#include "HAL/handles/HandlesInternal.h"
+#include "spilib/spi-lib.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 priority_recursive_mutex spiOnboardMutex;
+static priority_recursive_mutex spiMXPMutex;
+
+// MXP SPI does not count towards this
+std::atomic<int32_t> spiPortCount{0};
+
+static HAL_DigitalHandle digitalHandles[9]{HAL_kInvalidHandle};
+
+/**
+ * Get the semaphore for a SPI port
+ *
+ * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP
+ * @return The semaphore for the SPI port.
+ */
+static priority_recursive_mutex& spiGetMutex(int32_t port) {
+ if (port < 4)
+ return spiOnboardMutex;
+ else
+ return spiMXPMutex;
+}
+
+extern "C" {
+
+struct SPIAccumulator {
+ std::atomic<HAL_NotifierHandle> notifier{0};
+ uint64_t triggerTime;
+ int32_t period;
+
+ int64_t value = 0;
+ uint32_t count = 0;
+ int32_t lastValue = 0;
+
+ int32_t center = 0;
+ int32_t deadband = 0;
+
+ uint8_t cmd[4]; // command to send (up to 4 bytes)
+ int32_t validMask;
+ int32_t validValue;
+ int32_t dataMax; // one more than max data value
+ int32_t dataMsbMask; // data field MSB mask (for signed)
+ uint8_t dataShift; // data field shift right amount, in bits
+ uint8_t xferSize; // SPI transfer size, in bytes (up to 4)
+ uint8_t port;
+ bool isSigned; // is data field signed?
+ bool bigEndian; // is response big endian?
+};
+std::unique_ptr<SPIAccumulator> spiAccumulators[5];
+
+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() {
+ if (spiPortCount.fetch_sub(1) == 1) {
+ // Clean up SPI Handles
+ HAL_FreeDIOPort(digitalHandles[3]);
+ HAL_FreeDIOPort(digitalHandles[4]);
+ }
+}
+
+/*
+ * Initialize 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(int32_t port, int32_t* status) {
+ if (HAL_GetSPIHandle(port) != 0) return;
+ switch (port) {
+ case 0:
+ CommonSPIPortInit(status);
+ if (*status != 0) return;
+ // CS0 is not a DIO port, so nothing to allocate
+ HAL_SetSPIHandle(0, spilib_open("/dev/spidev0.0"));
+ break;
+ case 1:
+ CommonSPIPortInit(status);
+ if (*status != 0) return;
+ // CS1, Allocate
+ if ((digitalHandles[0] = HAL_InitializeDIOPort(
+ HAL_GetPort(26), false, status)) == HAL_kInvalidHandle) {
+ std::printf("Failed to allocate DIO 26 (CS1)\n");
+ CommonSPIPortFree();
+ return;
+ }
+ HAL_SetSPIHandle(1, spilib_open("/dev/spidev0.1"));
+ break;
+ case 2:
+ CommonSPIPortInit(status);
+ if (*status != 0) return;
+ // CS2, Allocate
+ if ((digitalHandles[1] = HAL_InitializeDIOPort(
+ HAL_GetPort(27), false, status)) == HAL_kInvalidHandle) {
+ std::printf("Failed to allocate DIO 27 (CS2)\n");
+ CommonSPIPortFree();
+ return;
+ }
+ HAL_SetSPIHandle(2, spilib_open("/dev/spidev0.2"));
+ break;
+ case 3:
+ CommonSPIPortInit(status);
+ if (*status != 0) return;
+ // CS3, Allocate
+ if ((digitalHandles[2] = HAL_InitializeDIOPort(
+ HAL_GetPort(28), false, status)) == HAL_kInvalidHandle) {
+ std::printf("Failed to allocate DIO 28 (CS3)\n");
+ CommonSPIPortFree();
+ return;
+ }
+ HAL_SetSPIHandle(3, spilib_open("/dev/spidev0.3"));
+ break;
+ case 4:
+ initializeDigital(status);
+ if (*status != 0) return;
+ if ((digitalHandles[5] = HAL_InitializeDIOPort(
+ HAL_GetPort(14), false, status)) == HAL_kInvalidHandle) {
+ std::printf("Failed to allocate DIO 14\n");
+ return;
+ }
+ if ((digitalHandles[6] = HAL_InitializeDIOPort(
+ HAL_GetPort(15), false, status)) == HAL_kInvalidHandle) {
+ std::printf("Failed to allocate DIO 15\n");
+ HAL_FreeDIOPort(digitalHandles[5]); // free the first port allocated
+ return;
+ }
+ if ((digitalHandles[7] = HAL_InitializeDIOPort(
+ HAL_GetPort(16), false, status)) == HAL_kInvalidHandle) {
+ std::printf("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(
+ HAL_GetPort(17), false, status)) == HAL_kInvalidHandle) {
+ std::printf("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);
+ HAL_SetSPIHandle(4, spilib_open("/dev/spidev1.0"));
+ break;
+ default:
+ *status = PARAMETER_OUT_OF_RANGE;
+ break;
+ }
+ return;
+}
+
+/**
+ * Generic 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(int32_t port, uint8_t* dataToSend,
+ uint8_t* dataReceived, int32_t size) {
+ std::lock_guard<priority_recursive_mutex> sync(spiGetMutex(port));
+ return spilib_writeread(
+ HAL_GetSPIHandle(port), reinterpret_cast<const char*>(dataToSend),
+ reinterpret_cast<char*>(dataReceived), static_cast<int32_t>(size));
+}
+
+/**
+ * Execute a write transaction with the device.
+ *
+ * Write 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(int32_t port, uint8_t* dataToSend, int32_t sendSize) {
+ std::lock_guard<priority_recursive_mutex> sync(spiGetMutex(port));
+ return spilib_write(HAL_GetSPIHandle(port),
+ reinterpret_cast<const char*>(dataToSend),
+ static_cast<int32_t>(sendSize));
+}
+
+/**
+ * Execute 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(int32_t port, uint8_t* buffer, int32_t count) {
+ std::lock_guard<priority_recursive_mutex> sync(spiGetMutex(port));
+ return spilib_read(HAL_GetSPIHandle(port), reinterpret_cast<char*>(buffer),
+ static_cast<int32_t>(count));
+}
+
+/**
+ * Close the SPI port
+ *
+ * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP
+ */
+void HAL_CloseSPI(int32_t port) {
+ std::lock_guard<priority_recursive_mutex> sync(spiGetMutex(port));
+ if (spiAccumulators[port]) {
+ int32_t status = 0;
+ HAL_FreeSPIAccumulator(port, &status);
+ }
+ spilib_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;
+ }
+ return;
+}
+
+/**
+ * Set 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(int32_t port, int32_t speed) {
+ std::lock_guard<priority_recursive_mutex> sync(spiGetMutex(port));
+ spilib_setspeed(HAL_GetSPIHandle(port), speed);
+}
+
+/**
+ * Set 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(int32_t port, HAL_Bool msbFirst, HAL_Bool sampleOnTrailing,
+ HAL_Bool clkIdleHigh) {
+ std::lock_guard<priority_recursive_mutex> sync(spiGetMutex(port));
+ spilib_setopts(HAL_GetSPIHandle(port), msbFirst, sampleOnTrailing,
+ clkIdleHigh);
+}
+
+/**
+ * Set 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(int32_t port, int32_t* status) {
+ std::lock_guard<priority_recursive_mutex> sync(spiGetMutex(port));
+ if (port < 4) {
+ spiSystem->writeChipSelectActiveHigh_Hdr(
+ spiSystem->readChipSelectActiveHigh_Hdr(status) | (1 << port), status);
+ } else {
+ spiSystem->writeChipSelectActiveHigh_MXP(1, status);
+ }
+}
+
+/**
+ * Set 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(int32_t port, int32_t* status) {
+ std::lock_guard<priority_recursive_mutex> sync(spiGetMutex(port));
+ if (port < 4) {
+ spiSystem->writeChipSelectActiveHigh_Hdr(
+ spiSystem->readChipSelectActiveHigh_Hdr(status) & ~(1 << port), status);
+ } else {
+ spiSystem->writeChipSelectActiveHigh_MXP(0, status);
+ }
+}
+
+/**
+ * Get 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(int32_t port) {
+ std::lock_guard<priority_recursive_mutex> sync(spiGetMutex(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;
+ }
+}
+
+/**
+ * Set 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(int32_t port, int32_t handle) {
+ std::lock_guard<priority_recursive_mutex> sync(spiGetMutex(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;
+ }
+}
+
+static void spiAccumulatorProcess(uint64_t currentTime,
+ HAL_NotifierHandle handle) {
+ int32_t status = 0;
+ auto param = HAL_GetNotifierParam(handle, &status);
+ if (param == nullptr) return;
+ SPIAccumulator* accum = static_cast<SPIAccumulator*>(param);
+
+ // perform SPI transaction
+ uint8_t resp_b[4];
+ std::lock_guard<priority_recursive_mutex> sync(spiGetMutex(accum->port));
+ spilib_writeread(
+ HAL_GetSPIHandle(accum->port), reinterpret_cast<const char*>(accum->cmd),
+ reinterpret_cast<char*>(resp_b), static_cast<int32_t>(accum->xferSize));
+
+ // convert from bytes
+ uint32_t resp = 0;
+ if (accum->bigEndian) {
+ for (int32_t i = 0; i < accum->xferSize; ++i) {
+ resp <<= 8;
+ resp |= resp_b[i] & 0xff;
+ }
+ } else {
+ for (int32_t i = accum->xferSize - 1; i >= 0; --i) {
+ resp <<= 8;
+ resp |= resp_b[i] & 0xff;
+ }
+ }
+
+ // process response
+ if ((resp & accum->validMask) == static_cast<uint32_t>(accum->validValue)) {
+ // valid sensor data; extract data field
+ int32_t data = static_cast<int32_t>(resp >> accum->dataShift);
+ data &= accum->dataMax - 1;
+ // 2s complement conversion if signed MSB is set
+ if (accum->isSigned && (data & accum->dataMsbMask) != 0)
+ data -= accum->dataMax;
+ // center offset
+ data -= accum->center;
+ // only accumulate if outside deadband
+ if (data < -accum->deadband || data > accum->deadband) accum->value += data;
+ ++accum->count;
+ accum->lastValue = data;
+ } else {
+ // no data from the sensor; just clear the last value
+ accum->lastValue = 0;
+ }
+
+ // reschedule timer
+ accum->triggerTime += accum->period;
+ // handle timer slip
+ if (accum->triggerTime < currentTime)
+ accum->triggerTime = currentTime + accum->period;
+ status = 0;
+ HAL_UpdateNotifierAlarm(accum->notifier, accum->triggerTime, &status);
+}
+
+/**
+ * Initialize a SPI accumulator.
+ *
+ * @param port SPI port
+ * @param period Time between reads, in us
+ * @param cmd SPI command to send to request data
+ * @param xferSize SPI transfer size, in bytes
+ * @param validMask Mask to apply to received data for validity checking
+ * @param valid_data After validMask is applied, required matching value for
+ * validity checking
+ * @param dataShift Bit shift to apply to received data to get actual data
+ * value
+ * @param dataSize Size (in bits) of data field
+ * @param isSigned Is data field signed?
+ * @param bigEndian Is device big endian?
+ */
+void HAL_InitSPIAccumulator(int32_t port, int32_t period, int32_t cmd,
+ int32_t xferSize, int32_t validMask,
+ int32_t validValue, int32_t dataShift,
+ int32_t dataSize, HAL_Bool isSigned,
+ HAL_Bool bigEndian, int32_t* status) {
+ std::lock_guard<priority_recursive_mutex> sync(spiGetMutex(port));
+ if (port > 4) return;
+ if (!spiAccumulators[port])
+ spiAccumulators[port] = std::make_unique<SPIAccumulator>();
+ SPIAccumulator* accum = spiAccumulators[port].get();
+ if (bigEndian) {
+ for (int32_t i = xferSize - 1; i >= 0; --i) {
+ accum->cmd[i] = cmd & 0xff;
+ cmd >>= 8;
+ }
+ } else {
+ accum->cmd[0] = cmd & 0xff;
+ cmd >>= 8;
+ accum->cmd[1] = cmd & 0xff;
+ cmd >>= 8;
+ accum->cmd[2] = cmd & 0xff;
+ cmd >>= 8;
+ accum->cmd[3] = cmd & 0xff;
+ }
+ accum->period = period;
+ accum->xferSize = xferSize;
+ accum->validMask = validMask;
+ accum->validValue = validValue;
+ accum->dataShift = dataShift;
+ accum->dataMax = (1 << dataSize);
+ accum->dataMsbMask = (1 << (dataSize - 1));
+ accum->isSigned = isSigned;
+ accum->bigEndian = bigEndian;
+ if (!accum->notifier) {
+ accum->notifier =
+ HAL_InitializeNotifier(spiAccumulatorProcess, accum, status);
+ accum->triggerTime = HAL_GetFPGATime(status) + period;
+ if (*status != 0) return;
+ HAL_UpdateNotifierAlarm(accum->notifier, accum->triggerTime, status);
+ }
+}
+
+/**
+ * Frees a SPI accumulator.
+ */
+void HAL_FreeSPIAccumulator(int32_t port, int32_t* status) {
+ std::lock_guard<priority_recursive_mutex> sync(spiGetMutex(port));
+ SPIAccumulator* accum = spiAccumulators[port].get();
+ if (!accum) {
+ *status = NULL_PARAMETER;
+ return;
+ }
+ HAL_NotifierHandle handle = accum->notifier.exchange(0);
+ HAL_CleanNotifier(handle, status);
+ spiAccumulators[port] = nullptr;
+}
+
+/**
+ * Resets the accumulator to zero.
+ */
+void HAL_ResetSPIAccumulator(int32_t port, int32_t* status) {
+ std::lock_guard<priority_recursive_mutex> sync(spiGetMutex(port));
+ SPIAccumulator* accum = spiAccumulators[port].get();
+ if (!accum) {
+ *status = NULL_PARAMETER;
+ return;
+ }
+ accum->value = 0;
+ accum->count = 0;
+ accum->lastValue = 0;
+}
+
+/**
+ * Set the center value of the accumulator.
+ *
+ * The center value is subtracted from each 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.
+ */
+void HAL_SetSPIAccumulatorCenter(int32_t port, int32_t center,
+ int32_t* status) {
+ std::lock_guard<priority_recursive_mutex> sync(spiGetMutex(port));
+ SPIAccumulator* accum = spiAccumulators[port].get();
+ if (!accum) {
+ *status = NULL_PARAMETER;
+ return;
+ }
+ accum->center = center;
+}
+
+/**
+ * Set the accumulator's deadband.
+ */
+void HAL_SetSPIAccumulatorDeadband(int32_t port, int32_t deadband,
+ int32_t* status) {
+ std::lock_guard<priority_recursive_mutex> sync(spiGetMutex(port));
+ SPIAccumulator* accum = spiAccumulators[port].get();
+ if (!accum) {
+ *status = NULL_PARAMETER;
+ return;
+ }
+ accum->deadband = deadband;
+}
+
+/**
+ * Read the last value read by the accumulator engine.
+ */
+int32_t HAL_GetSPIAccumulatorLastValue(int32_t port, int32_t* status) {
+ std::lock_guard<priority_recursive_mutex> sync(spiGetMutex(port));
+ SPIAccumulator* accum = spiAccumulators[port].get();
+ if (!accum) {
+ *status = NULL_PARAMETER;
+ return 0;
+ }
+ return accum->lastValue;
+}
+
+/**
+ * Read the accumulated value.
+ *
+ * @return The 64-bit value accumulated since the last Reset().
+ */
+int64_t HAL_GetSPIAccumulatorValue(int32_t port, int32_t* status) {
+ std::lock_guard<priority_recursive_mutex> sync(spiGetMutex(port));
+ SPIAccumulator* accum = spiAccumulators[port].get();
+ if (!accum) {
+ *status = NULL_PARAMETER;
+ return 0;
+ }
+ return accum->value;
+}
+
+/**
+ * Read the number of accumulated values.
+ *
+ * Read the count of the accumulated values since the accumulator was last
+ * Reset().
+ *
+ * @return The number of times samples from the channel were accumulated.
+ */
+int64_t HAL_GetSPIAccumulatorCount(int32_t port, int32_t* status) {
+ std::lock_guard<priority_recursive_mutex> sync(spiGetMutex(port));
+ SPIAccumulator* accum = spiAccumulators[port].get();
+ if (!accum) {
+ *status = NULL_PARAMETER;
+ return 0;
+ }
+ return accum->count;
+}
+
+/**
+ * Read the average of the accumulated value.
+ *
+ * @return The accumulated average value (value / count).
+ */
+double HAL_GetSPIAccumulatorAverage(int32_t port, int32_t* status) {
+ int64_t value;
+ int64_t count;
+ HAL_GetSPIAccumulatorOutput(port, &value, &count, status);
+ if (count == 0) return 0.0;
+ return static_cast<double>(value) / count;
+}
+
+/**
+ * Read the accumulated value and the number of accumulated values atomically.
+ *
+ * This function reads the value and count atomically.
+ * This can be used for averaging.
+ *
+ * @param value Pointer to the 64-bit accumulated output.
+ * @param count Pointer to the number of accumulation cycles.
+ */
+void HAL_GetSPIAccumulatorOutput(int32_t port, int64_t* value, int64_t* count,
+ int32_t* status) {
+ std::lock_guard<priority_recursive_mutex> sync(spiGetMutex(port));
+ SPIAccumulator* accum = spiAccumulators[port].get();
+ if (!accum) {
+ *status = NULL_PARAMETER;
+ *value = 0;
+ *count = 0;
+ return;
+ }
+ *value = accum->value;
+ *count = accum->count;
+}
+}
diff --git a/hal/lib/athena/SerialPort.cpp b/hal/lib/athena/SerialPort.cpp
new file mode 100644
index 0000000..183e3cb
--- /dev/null
+++ b/hal/lib/athena/SerialPort.cpp
@@ -0,0 +1,159 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be 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 <string>
+
+#include "HAL/cpp/SerialHelper.h"
+#include "visa/visa.h"
+
+static int32_t resourceManagerHandle;
+static HAL_SerialPort portHandles[4];
+
+extern "C" {
+
+void HAL_InitializeSerialPort(HAL_SerialPort port, int32_t* status) {
+ std::string portName;
+
+ if (resourceManagerHandle == 0)
+ viOpenDefaultRM(reinterpret_cast<ViSession*>(&resourceManagerHandle));
+
+ hal::SerialHelper serialHelper;
+
+ portName = serialHelper.GetVISASerialPortName(port, status);
+
+ if (*status < 0) {
+ return;
+ }
+
+ *status = viOpen(resourceManagerHandle, const_cast<char*>(portName.c_str()),
+ VI_NULL, VI_NULL,
+ reinterpret_cast<ViSession*>(&portHandles[port]));
+ if (*status > 0) *status = 0;
+}
+
+void HAL_SetSerialBaudRate(HAL_SerialPort port, int32_t baud, int32_t* status) {
+ *status = viSetAttribute(portHandles[port], VI_ATTR_ASRL_BAUD, baud);
+ if (*status > 0) *status = 0;
+}
+
+void HAL_SetSerialDataBits(HAL_SerialPort port, int32_t bits, int32_t* status) {
+ *status = viSetAttribute(portHandles[port], VI_ATTR_ASRL_DATA_BITS, bits);
+ if (*status > 0) *status = 0;
+}
+
+void HAL_SetSerialParity(HAL_SerialPort port, int32_t parity, int32_t* status) {
+ *status = viSetAttribute(portHandles[port], VI_ATTR_ASRL_PARITY, parity);
+ if (*status > 0) *status = 0;
+}
+
+void HAL_SetSerialStopBits(HAL_SerialPort port, int32_t stopBits,
+ int32_t* status) {
+ *status = viSetAttribute(portHandles[port], VI_ATTR_ASRL_STOP_BITS, stopBits);
+ if (*status > 0) *status = 0;
+}
+
+void HAL_SetSerialWriteMode(HAL_SerialPort port, int32_t mode,
+ int32_t* status) {
+ *status = viSetAttribute(portHandles[port], VI_ATTR_WR_BUF_OPER_MODE, mode);
+ if (*status > 0) *status = 0;
+}
+
+void HAL_SetSerialFlowControl(HAL_SerialPort port, int32_t flow,
+ int32_t* status) {
+ *status = viSetAttribute(portHandles[port], VI_ATTR_ASRL_FLOW_CNTRL, flow);
+ if (*status > 0) *status = 0;
+}
+
+void HAL_SetSerialTimeout(HAL_SerialPort port, double timeout,
+ int32_t* status) {
+ *status = viSetAttribute(portHandles[port], VI_ATTR_TMO_VALUE,
+ static_cast<uint32_t>(timeout * 1e3));
+ if (*status > 0) *status = 0;
+}
+
+void HAL_EnableSerialTermination(HAL_SerialPort port, char terminator,
+ int32_t* status) {
+ viSetAttribute(portHandles[port], VI_ATTR_TERMCHAR_EN, VI_TRUE);
+ viSetAttribute(portHandles[port], VI_ATTR_TERMCHAR, terminator);
+ *status = viSetAttribute(portHandles[port], VI_ATTR_ASRL_END_IN,
+ VI_ASRL_END_TERMCHAR);
+ if (*status > 0) *status = 0;
+}
+
+void HAL_DisableSerialTermination(HAL_SerialPort port, int32_t* status) {
+ viSetAttribute(portHandles[port], VI_ATTR_TERMCHAR_EN, VI_FALSE);
+ *status =
+ viSetAttribute(portHandles[port], VI_ATTR_ASRL_END_IN, VI_ASRL_END_NONE);
+ if (*status > 0) *status = 0;
+}
+
+void HAL_SetSerialReadBufferSize(HAL_SerialPort port, int32_t size,
+ int32_t* status) {
+ *status = viSetBuf(portHandles[port], VI_READ_BUF, size);
+ if (*status > 0) *status = 0;
+}
+
+void HAL_SetSerialWriteBufferSize(HAL_SerialPort port, int32_t size,
+ int32_t* status) {
+ *status = viSetBuf(portHandles[port], VI_WRITE_BUF, size);
+ if (*status > 0) *status = 0;
+}
+
+int32_t HAL_GetSerialBytesReceived(HAL_SerialPort port, int32_t* status) {
+ int32_t bytes = 0;
+
+ *status = viGetAttribute(portHandles[port], VI_ATTR_ASRL_AVAIL_NUM, &bytes);
+ if (*status > 0) *status = 0;
+ return bytes;
+}
+
+int32_t HAL_ReadSerial(HAL_SerialPort port, char* buffer, int32_t count,
+ int32_t* status) {
+ uint32_t retCount = 0;
+
+ *status =
+ viRead(portHandles[port], (ViPBuf)buffer, count, (ViPUInt32)&retCount);
+
+ if (*status == VI_ERROR_IO || *status == VI_ERROR_ASRL_OVERRUN ||
+ *status == VI_ERROR_ASRL_FRAMING || *status == VI_ERROR_ASRL_PARITY) {
+ int32_t localStatus = 0;
+ HAL_ClearSerial(port, &localStatus);
+ }
+
+ if (*status == VI_ERROR_TMO || *status > 0) *status = 0;
+ return static_cast<int32_t>(retCount);
+}
+
+int32_t HAL_WriteSerial(HAL_SerialPort port, const char* buffer, int32_t count,
+ int32_t* status) {
+ uint32_t retCount = 0;
+
+ *status =
+ viWrite(portHandles[port], (ViPBuf)buffer, count, (ViPUInt32)&retCount);
+
+ if (*status > 0) *status = 0;
+ return static_cast<int32_t>(retCount);
+}
+
+void HAL_FlushSerial(HAL_SerialPort port, int32_t* status) {
+ *status = viFlush(portHandles[port], VI_WRITE_BUF);
+ if (*status > 0) *status = 0;
+}
+
+void HAL_ClearSerial(HAL_SerialPort port, int32_t* status) {
+ *status = viClear(portHandles[port]);
+ if (*status > 0) *status = 0;
+}
+
+void HAL_CloseSerial(HAL_SerialPort port, int32_t* status) {
+ *status = viClose(portHandles[port]);
+ if (*status > 0) *status = 0;
+}
+
+} // extern "C"
diff --git a/hal/lib/athena/Solenoid.cpp b/hal/lib/athena/Solenoid.cpp
new file mode 100644
index 0000000..8df8664
--- /dev/null
+++ b/hal/lib/athena/Solenoid.cpp
@@ -0,0 +1,154 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be 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 "HAL/ChipObject.h"
+#include "HAL/Errors.h"
+#include "HAL/Ports.h"
+#include "HAL/handles/HandlesInternal.h"
+#include "HAL/handles/IndexedHandleResource.h"
+#include "PCMInternal.h"
+#include "PortsInternal.h"
+#include "ctre/PCM.h"
+
+namespace {
+struct Solenoid {
+ uint8_t module;
+ uint8_t channel;
+};
+}
+
+using namespace hal;
+
+static IndexedHandleResource<HAL_SolenoidHandle, Solenoid,
+ kNumPCMModules * kNumSolenoidChannels,
+ HAL_HandleEnum::Solenoid>
+ solenoidHandles;
+
+extern "C" {
+
+HAL_SolenoidHandle HAL_InitializeSolenoidPort(HAL_PortHandle portHandle,
+ int32_t* status) {
+ 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();
+}
+
+} // extern "C"
diff --git a/hal/lib/athena/Threads.cpp b/hal/lib/athena/Threads.cpp
new file mode 100644
index 0000000..8e8bc89
--- /dev/null
+++ b/hal/lib/athena/Threads.cpp
@@ -0,0 +1,122 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be 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"
+
+extern "C" {
+/**
+ * Get 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) {
+ sched_param sch;
+ int policy;
+ int success = pthread_getschedparam(*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;
+ }
+}
+
+/**
+ * Get 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) {
+ auto thread = pthread_self();
+ return HAL_GetThreadPriority(&thread, isRealTime, 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) {
+ 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(*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(*handle, scheduler, &sch)) {
+ *status = HAL_THREAD_PRIORITY_ERROR;
+ return false;
+ } else {
+ *status = 0;
+ return true;
+ }
+}
+
+/**
+ * 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) {
+ auto thread = pthread_self();
+ return HAL_SetThreadPriority(&thread, realTime, priority, status);
+}
+}
diff --git a/hal/lib/athena/cpp/Semaphore.cpp b/hal/lib/athena/cpp/Semaphore.cpp
new file mode 100644
index 0000000..521c816
--- /dev/null
+++ b/hal/lib/athena/cpp/Semaphore.cpp
@@ -0,0 +1,32 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2015-2017. All Rights Reserved. */
+/* Open Source Software - may be 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/Semaphore.h"
+
+Semaphore::Semaphore(int32_t count) { m_count = count; }
+
+void Semaphore::give() {
+ std::lock_guard<priority_mutex> lock(m_mutex);
+ ++m_count;
+ m_condition.notify_one();
+}
+
+void Semaphore::take() {
+ std::unique_lock<priority_mutex> lock(m_mutex);
+ m_condition.wait(lock, [this] { return m_count; });
+ --m_count;
+}
+
+bool Semaphore::tryTake() {
+ std::lock_guard<priority_mutex> lock(m_mutex);
+ if (m_count) {
+ --m_count;
+ return true;
+ } else {
+ return false;
+ }
+}
diff --git a/hal/lib/athena/cpp/SerialHelper.cpp b/hal/lib/athena/cpp/SerialHelper.cpp
new file mode 100644
index 0000000..5b7e8be
--- /dev/null
+++ b/hal/lib/athena/cpp/SerialHelper.cpp
@@ -0,0 +1,343 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be 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 "../visa/visa.h"
+#include "HAL/Errors.h"
+#include "llvm/StringRef.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]{"", ""};
+
+priority_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 llvm::SmallVectorImpl<char>& lhs,
+ const llvm::SmallVectorImpl<char>& rhs) -> int {
+ llvm::StringRef lhsRef(lhs.begin(), lhs.size());
+ llvm::StringRef rhsRef(rhs.begin(), rhs.size());
+ return lhsRef.compare(rhsRef);
+ });
+}
+
+void SerialHelper::CoiteratedSort(
+ llvm::SmallVectorImpl<llvm::SmallString<16>>& vec) {
+ llvm::SmallVector<llvm::SmallString<16>, 4> sortedVec;
+ for (auto& str : m_sortedHubPath) {
+ for (size_t i = 0; i < m_unsortedHubPath.size(); i++) {
+ if (llvm::StringRef{m_unsortedHubPath[i].begin(),
+ m_unsortedHubPath[i].size()}
+ .equals(llvm::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 buffers for Visa calls and system exec calls
+ char osName[256];
+ char execBuffer[128];
+
+ // 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/
+ llvm::StringRef devNameRef = llvm::StringRef{osName}.split("(/dev/").second;
+ // String not found, continue
+ if (devNameRef.equals("")) continue;
+
+ // Split at )
+ llvm::StringRef matchString = devNameRef.split(')').first;
+ if (matchString.equals(devNameRef)) continue;
+
+ // Run find using pipe to get a list of system accessors
+ llvm::SmallString<128> val(
+ "sh -c \"find /sys/devices/soc0 | grep amba | grep usb | grep ");
+ val += matchString;
+ val += "\"";
+
+ // Pipe code found on StackOverflow
+ // http://stackoverflow.com/questions/478898/how-to-execute-a-command-and-get-output-of-command-within-c-using-posix
+
+ // Using std::string because this is guarenteed to be large
+ std::string output = "";
+
+ std::shared_ptr<FILE> pipe(popen(val.c_str(), "r"), pclose);
+ // Just check the next item on a pipe failure
+ if (!pipe) continue;
+ while (!feof(pipe.get())) {
+ if (std::fgets(execBuffer, 128, pipe.get()) != 0) output += execBuffer;
+ }
+
+ if (!output.empty()) {
+ llvm::SmallVector<llvm::StringRef, 16> pathSplitVec;
+ // Split output by line, grab first line, and split it into
+ // individual directories
+ llvm::StringRef{output}.split('\n').first.split(pathSplitVec, '/', -1,
+ false);
+
+ // Find each individual item index
+
+ const char* usb1 = "usb1";
+ const char* tty = "tty";
+
+ 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(
+ llvm::StringRef{pathSplitVec[hubIndex - 2]});
+ m_visaResource.emplace_back(desc);
+ m_osResource.emplace_back(
+ llvm::StringRef{osName}.split("(").second.split(")").first);
+ }
+ }
+
+ 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::lock_guard<priority_mutex> lock(m_nameMutex);
+
+ std::string portString = m_usbNames[port - 2];
+
+ llvm::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/lib/athena/cpp/priority_mutex.cpp b/hal/lib/athena/cpp/priority_mutex.cpp
new file mode 100644
index 0000000..1213898
--- /dev/null
+++ b/hal/lib/athena/cpp/priority_mutex.cpp
@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be 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/priority_mutex.h"
+
+void priority_recursive_mutex::lock() { pthread_mutex_lock(&m_mutex); }
+
+void priority_recursive_mutex::unlock() { pthread_mutex_unlock(&m_mutex); }
+
+bool priority_recursive_mutex::try_lock() noexcept {
+ return !pthread_mutex_trylock(&m_mutex);
+}
+
+pthread_mutex_t* priority_recursive_mutex::native_handle() { return &m_mutex; }
+
+void priority_mutex::lock() { pthread_mutex_lock(&m_mutex); }
+
+void priority_mutex::unlock() { pthread_mutex_unlock(&m_mutex); }
+
+bool priority_mutex::try_lock() noexcept {
+ return !pthread_mutex_trylock(&m_mutex);
+}
+
+pthread_mutex_t* priority_mutex::native_handle() { return &m_mutex; }
diff --git a/hal/lib/athena/ctre/CtreCanNode.cpp b/hal/lib/athena/ctre/CtreCanNode.cpp
new file mode 100644
index 0000000..2eb42ab
--- /dev/null
+++ b/hal/lib/athena/ctre/CtreCanNode.cpp
@@ -0,0 +1,163 @@
+#pragma GCC diagnostic ignored "-Wmissing-field-initializers"
+
+#include "ctre/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);
+ }
+}
+timespec diff(const timespec & start, const timespec & end)
+{
+ timespec temp;
+ if ((end.tv_nsec-start.tv_nsec)<0) {
+ temp.tv_sec = end.tv_sec-start.tv_sec-1;
+ temp.tv_nsec = 1000000000+end.tv_nsec-start.tv_nsec;
+ } else {
+ temp.tv_sec = end.tv_sec-start.tv_sec;
+ temp.tv_nsec = end.tv_nsec-start.tv_nsec;
+ }
+ return temp;
+}
+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);
+ if(status == 0){
+ /* fresh update */
+ rxEvent_t & r = _rxRxEvents[arbId]; /* lookup entry or make a default new one with all zeroes */
+ clock_gettime(2,&r.time); /* fill in time */
+ 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 */
+ struct timespec temp;
+ clock_gettime(2,&temp); /* get now */
+ /* how long has it been? */
+ temp = diff(i->second.time,temp); /* temp = now - last */
+ if(temp.tv_sec > 0){
+ retval = CTR_RxTimeout;
+ }else if(temp.tv_nsec > ((int32_t)timeoutMs*1000*1000)){
+ 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/lib/athena/ctre/PCM.cpp b/hal/lib/athena/ctre/PCM.cpp
new file mode 100644
index 0000000..2676743
--- /dev/null
+++ b/hal/lib/athena/ctre/PCM.cpp
@@ -0,0 +1,572 @@
+#pragma GCC diagnostic ignored "-Wmissing-field-initializers"
+
+#include "ctre/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 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 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/lib/athena/ctre/PDP.cpp b/hal/lib/athena/ctre/PDP.cpp
new file mode 100644
index 0000000..127b1dc
--- /dev/null
+++ b/hal/lib/athena/ctre/PDP.cpp
@@ -0,0 +1,230 @@
+#include "ctre/PDP.h"
+#include "FRC_NetworkCommunication/CANSessionMux.h" //CAN Comm
+#include <string.h> // memset
+
+#define STATUS_1 0x8041400
+#define STATUS_2 0x8041440
+#define STATUS_3 0x8041480
+#define STATUS_ENERGY 0x8041740
+
+#define CONTROL_1 0x08041C00 /* PDP_Control_ClearStats */
+
+#define EXPECTED_RESPONSE_TIMEOUT_MS (50)
+#define GET_STATUS1() CtreCanNode::recMsg<PdpStatus1_t> rx = GetRx<PdpStatus1_t>(STATUS_1|GetDeviceNumber(),EXPECTED_RESPONSE_TIMEOUT_MS)
+#define GET_STATUS2() CtreCanNode::recMsg<PdpStatus2_t> rx = GetRx<PdpStatus2_t>(STATUS_2|GetDeviceNumber(),EXPECTED_RESPONSE_TIMEOUT_MS)
+#define GET_STATUS3() CtreCanNode::recMsg<PdpStatus3_t> rx = GetRx<PdpStatus3_t>(STATUS_3|GetDeviceNumber(),EXPECTED_RESPONSE_TIMEOUT_MS)
+#define GET_STATUS_ENERGY() CtreCanNode::recMsg<PDP_Status_Energy_t> rx = GetRx<PDP_Status_Energy_t>(STATUS_ENERGY|GetDeviceNumber(),EXPECTED_RESPONSE_TIMEOUT_MS)
+
+/* encoder/decoders */
+typedef struct _PdpStatus1_t{
+ 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;
+}PdpStatus1_t;
+typedef struct _PdpStatus2_t{
+ 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;
+}PdpStatus2_t;
+typedef struct _PdpStatus3_t{
+ 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;
+}PdpStatus3_t;
+typedef struct _PDP_Status_Energy_t {
+ 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;
+} PDP_Status_Energy_t ;
+
+PDP::PDP(UINT8 deviceNumber): CtreCanNode(deviceNumber)
+{
+ RegisterRx(STATUS_1 | deviceNumber );
+ RegisterRx(STATUS_2 | deviceNumber );
+ RegisterRx(STATUS_3 | deviceNumber );
+}
+/* PDP D'tor
+ */
+PDP::~PDP()
+{
+}
+
+CTR_Code PDP::GetChannelCurrent(UINT8 idx, double ¤t)
+{
+ CTR_Code retval = CTR_InvalidParamValue;
+ uint32_t raw = 0;
+
+ if(idx <= 5){
+ GET_STATUS1();
+ retval = rx.err;
+ switch(idx){
+ case 0: raw = ((uint32_t)rx->chan1_h8 << 2) | rx->chan1_l2; break;
+ case 1: raw = ((uint32_t)rx->chan2_h6 << 4) | rx->chan2_l4; break;
+ case 2: raw = ((uint32_t)rx->chan3_h4 << 6) | rx->chan3_l6; break;
+ case 3: raw = ((uint32_t)rx->chan4_h2 << 8) | rx->chan4_l8; break;
+ case 4: raw = ((uint32_t)rx->chan5_h8 << 2) | rx->chan5_l2; break;
+ case 5: raw = ((uint32_t)rx->chan6_h6 << 4) | rx->chan6_l4; break;
+ default: retval = CTR_InvalidParamValue; break;
+ }
+ }else if(idx <= 11){
+ GET_STATUS2();
+ retval = rx.err;
+ switch(idx){
+ case 6: raw = ((uint32_t)rx->chan7_h8 << 2) | rx->chan7_l2; break;
+ case 7: raw = ((uint32_t)rx->chan8_h6 << 4) | rx->chan8_l4; break;
+ case 8: raw = ((uint32_t)rx->chan9_h4 << 6) | rx->chan9_l6; break;
+ case 9: raw = ((uint32_t)rx->chan10_h2 << 8) | rx->chan10_l8; break;
+ case 10: raw = ((uint32_t)rx->chan11_h8 << 2) | rx->chan11_l2; break;
+ case 11: raw = ((uint32_t)rx->chan12_h6 << 4) | rx->chan12_l4; break;
+ default: retval = CTR_InvalidParamValue; break;
+ }
+ }else if(idx <= 15){
+ GET_STATUS3();
+ retval = rx.err;
+ switch(idx){
+ case 12: raw = ((uint32_t)rx->chan13_h8 << 2) | rx->chan13_l2; break;
+ case 13: raw = ((uint32_t)rx->chan14_h6 << 4) | rx->chan14_l4; break;
+ case 14: raw = ((uint32_t)rx->chan15_h4 << 6) | rx->chan15_l6; break;
+ case 15: raw = ((uint32_t)rx->chan16_h2 << 8) | rx->chan16_l8; break;
+ default: retval = CTR_InvalidParamValue; break;
+ }
+ }
+ /* convert to amps */
+ current = (double)raw * 0.125; /* 7.3 fixed pt value in Amps */
+ /* signal caller with success */
+ return retval;
+}
+CTR_Code PDP::GetVoltage(double &voltage)
+{
+ GET_STATUS3();
+ uint32_t raw = rx->busVoltage;
+ voltage = (double)raw * 0.05 + 4.0; /* 50mV per unit plus 4V. */;
+ return rx.err;
+}
+CTR_Code PDP::GetTemperature(double &tempC)
+{
+ GET_STATUS3();
+ uint32_t raw = rx->temp;
+ tempC = (double)raw * 1.03250836957542 - 67.8564500484966;
+ return rx.err;
+}
+CTR_Code PDP::GetTotalCurrent(double ¤tAmps)
+{
+ GET_STATUS_ENERGY();
+ uint32_t raw;
+ raw = rx->TotalCurrent_125mAperunit_h8;
+ raw <<= 4;
+ raw |= rx->TotalCurrent_125mAperunit_l4;
+ currentAmps = 0.125 * raw;
+ return rx.err;
+}
+CTR_Code PDP::GetTotalPower(double &powerWatts)
+{
+ GET_STATUS_ENERGY();
+ uint32_t raw;
+ raw = rx->Power_125mWperunit_h4;
+ raw <<= 8;
+ raw |= rx->Power_125mWperunit_m8;
+ raw <<= 4;
+ raw |= rx->Power_125mWperunit_l4;
+ powerWatts = 0.125 * raw;
+ return rx.err;
+}
+CTR_Code PDP::GetTotalEnergy(double &energyJoules)
+{
+ GET_STATUS_ENERGY();
+ uint32_t raw;
+ raw = rx->Energy_125mWPerUnitXTmeas_h4;
+ raw <<= 8;
+ raw |= rx->Energy_125mWPerUnitXTmeas_mh8;
+ raw <<= 8;
+ raw |= rx->Energy_125mWPerUnitXTmeas_ml8;
+ raw <<= 8;
+ raw |= rx->Energy_125mWPerUnitXTmeas_l8;
+ energyJoules = 0.125 * raw; /* mW integrated every TmeasMs */
+ energyJoules *= 0.001; /* convert from mW to W */
+ energyJoules *= rx->TmeasMs_likelywillbe20ms_; /* multiplied by TmeasMs = joules */
+ return rx.err;
+}
+/* Clear sticky faults.
+ * @Return - CTR_Code - Error code (if any)
+ */
+CTR_Code PDP::ClearStickyFaults()
+{
+ int32_t status = 0;
+ uint8_t pdpControl[] = { 0x80 }; /* only bit set is ClearStickyFaults */
+ FRC_NetworkCommunication_CANSessionMux_sendMessage(CONTROL_1 | GetDeviceNumber(), pdpControl, sizeof(pdpControl), 0, &status);
+ if(status)
+ return CTR_TxFailed;
+ return CTR_OKAY;
+}
+
+/* Reset Energy Signals
+ * @Return - CTR_Code - Error code (if any)
+ */
+CTR_Code PDP::ResetEnergy()
+{
+ int32_t status = 0;
+ uint8_t pdpControl[] = { 0x40 }; /* only bit set is ResetEnergy */
+ FRC_NetworkCommunication_CANSessionMux_sendMessage(CONTROL_1 | GetDeviceNumber(), pdpControl, sizeof(pdpControl), 0, &status);
+ if(status)
+ return CTR_TxFailed;
+ return CTR_OKAY;
+}
+//------------------ C interface --------------------------------------------//
+extern "C" {
+ void * c_PDP_Init(void)
+ {
+ return new PDP();
+ }
+ CTR_Code c_GetChannelCurrent(void * handle,UINT8 idx, double *status)
+ {
+ return ((PDP*)handle)-> GetChannelCurrent(idx,*status);
+ }
+ CTR_Code c_GetVoltage(void * handle,double *status)
+ {
+ return ((PDP*)handle)-> GetVoltage(*status);
+ }
+ CTR_Code c_GetTemperature(void * handle,double *status)
+ {
+ return ((PDP*)handle)-> GetTemperature(*status);
+ }
+ void c_SetDeviceNumber_PDP(void * handle,UINT8 deviceNumber)
+ {
+ }
+}
diff --git a/hal/lib/athena/frccansae/CANDeviceInterface.h b/hal/lib/athena/frccansae/CANDeviceInterface.h
new file mode 100644
index 0000000..62a38cb
--- /dev/null
+++ b/hal/lib/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/lib/athena/visa/visa.h b/hal/lib/athena/visa/visa.h
new file mode 100644
index 0000000..3c6ad30
--- /dev/null
+++ b/hal/lib/athena/visa/visa.h
@@ -0,0 +1,1064 @@
+/*---------------------------------------------------------------------------*/
+/* Distributed by IVI Foundation Inc. */
+/* Contains National Instruments extensions. */
+/* Do not modify the contents of this file. */
+/*---------------------------------------------------------------------------*/
+/* */
+/* Title : VISA.H */
+/* Date : 10-09-2006 */
+/* Purpose : Include file for the VISA Library 4.0 specification */
+/* */
+/*---------------------------------------------------------------------------*/
+/* When using NI-VISA extensions, you must link with the VISA library that */
+/* comes with NI-VISA. Currently, the extensions provided by NI-VISA are: */
+/* */
+/* PXI (Compact PCI eXtensions for Instrumentation) and PCI support. To use */
+/* this, you must define the macro NIVISA_PXI before including this header. */
+/* You must also create an INF file with the VISA Driver Development Wizard. */
+/* */
+/* A fast set of macros for viPeekXX/viPokeXX that guarantees binary */
+/* compatibility with other implementations of VISA. To use this, you must */
+/* define the macro NIVISA_PEEKPOKE before including this header. */
+/* */
+/* Support for USB devices that do not conform to a specific class. To use */
+/* this, you must define the macro NIVISA_USB before including this header. */
+/* You must also create an INF file with the VISA Driver Development Wizard. */
+/*---------------------------------------------------------------------------*/
+
+#ifndef __VISA_HEADER__
+#define __VISA_HEADER__
+
+#include <stdarg.h>
+
+#if !defined(__VISATYPE_HEADER__)
+#include "visatype.h"
+#endif
+
+#define VI_SPEC_VERSION (0x00400000UL)
+
+#if defined(__cplusplus) || defined(__cplusplus__)
+ extern "C" {
+#endif
+
+#if defined(_CVI_)
+#pragma EnableLibraryRuntimeChecking
+#endif
+
+/*- VISA Types --------------------------------------------------------------*/
+
+typedef ViObject ViEvent;
+typedef ViEvent _VI_PTR ViPEvent;
+typedef ViObject ViFindList;
+typedef ViFindList _VI_PTR ViPFindList;
+
+#if defined(_VI_INT64_UINT64_DEFINED) && defined(_VISA_ENV_IS_64_BIT)
+typedef ViUInt64 ViBusAddress;
+typedef ViUInt64 ViBusSize;
+typedef ViUInt64 ViAttrState;
+#else
+typedef ViUInt32 ViBusAddress;
+typedef ViUInt32 ViBusSize;
+typedef ViUInt32 ViAttrState;
+#endif
+
+#if defined(_VI_INT64_UINT64_DEFINED)
+typedef ViUInt64 ViBusAddress64;
+typedef ViBusAddress64 _VI_PTR ViPBusAddress64;
+#endif
+
+typedef ViUInt32 ViEventType;
+typedef ViEventType _VI_PTR ViPEventType;
+typedef ViEventType _VI_PTR ViAEventType;
+typedef void _VI_PTR ViPAttrState;
+typedef ViAttr _VI_PTR ViPAttr;
+typedef ViAttr _VI_PTR ViAAttr;
+
+typedef ViString ViKeyId;
+typedef ViPString ViPKeyId;
+typedef ViUInt32 ViJobId;
+typedef ViJobId _VI_PTR ViPJobId;
+typedef ViUInt32 ViAccessMode;
+typedef ViAccessMode _VI_PTR ViPAccessMode;
+typedef ViBusAddress _VI_PTR ViPBusAddress;
+typedef ViUInt32 ViEventFilter;
+
+typedef va_list ViVAList;
+
+typedef ViStatus (_VI_FUNCH _VI_PTR ViHndlr)
+ (ViSession vi, ViEventType eventType, ViEvent event, ViAddr userHandle);
+
+/*- Resource Manager Functions and Operations -------------------------------*/
+
+ViStatus _VI_FUNC viOpenDefaultRM (ViPSession vi);
+ViStatus _VI_FUNC viFindRsrc (ViSession sesn, ViString expr, ViPFindList vi,
+ ViPUInt32 retCnt, ViChar _VI_FAR desc[]);
+ViStatus _VI_FUNC viFindNext (ViFindList vi, ViChar _VI_FAR desc[]);
+ViStatus _VI_FUNC viParseRsrc (ViSession rmSesn, ViRsrc rsrcName,
+ ViPUInt16 intfType, ViPUInt16 intfNum);
+ViStatus _VI_FUNC viParseRsrcEx (ViSession rmSesn, ViRsrc rsrcName, ViPUInt16 intfType,
+ ViPUInt16 intfNum, ViChar _VI_FAR rsrcClass[],
+ ViChar _VI_FAR expandedUnaliasedName[],
+ ViChar _VI_FAR aliasIfExists[]);
+ViStatus _VI_FUNC viOpen (ViSession sesn, ViRsrc name, ViAccessMode mode,
+ ViUInt32 timeout, ViPSession vi);
+
+/*- Resource Template Operations --------------------------------------------*/
+
+ViStatus _VI_FUNC viClose (ViObject vi);
+ViStatus _VI_FUNC viSetAttribute (ViObject vi, ViAttr attrName, ViAttrState attrValue);
+ViStatus _VI_FUNC viGetAttribute (ViObject vi, ViAttr attrName, void _VI_PTR attrValue);
+ViStatus _VI_FUNC viStatusDesc (ViObject vi, ViStatus status, ViChar _VI_FAR desc[]);
+ViStatus _VI_FUNC viTerminate (ViObject vi, ViUInt16 degree, ViJobId jobId);
+
+ViStatus _VI_FUNC viLock (ViSession vi, ViAccessMode lockType, ViUInt32 timeout,
+ ViKeyId requestedKey, ViChar _VI_FAR accessKey[]);
+ViStatus _VI_FUNC viUnlock (ViSession vi);
+ViStatus _VI_FUNC viEnableEvent (ViSession vi, ViEventType eventType, ViUInt16 mechanism,
+ ViEventFilter context);
+ViStatus _VI_FUNC viDisableEvent (ViSession vi, ViEventType eventType, ViUInt16 mechanism);
+ViStatus _VI_FUNC viDiscardEvents (ViSession vi, ViEventType eventType, ViUInt16 mechanism);
+ViStatus _VI_FUNC viWaitOnEvent (ViSession vi, ViEventType inEventType, ViUInt32 timeout,
+ ViPEventType outEventType, ViPEvent outContext);
+ViStatus _VI_FUNC viInstallHandler(ViSession vi, ViEventType eventType, ViHndlr handler,
+ ViAddr userHandle);
+ViStatus _VI_FUNC viUninstallHandler(ViSession vi, ViEventType eventType, ViHndlr handler,
+ ViAddr userHandle);
+
+/*- Basic I/O Operations ----------------------------------------------------*/
+
+ViStatus _VI_FUNC viRead (ViSession vi, ViPBuf buf, ViUInt32 cnt, ViPUInt32 retCnt);
+ViStatus _VI_FUNC viReadAsync (ViSession vi, ViPBuf buf, ViUInt32 cnt, ViPJobId jobId);
+ViStatus _VI_FUNC viReadToFile (ViSession vi, ViConstString filename, ViUInt32 cnt,
+ ViPUInt32 retCnt);
+ViStatus _VI_FUNC viWrite (ViSession vi, ViBuf buf, ViUInt32 cnt, ViPUInt32 retCnt);
+ViStatus _VI_FUNC viWriteAsync (ViSession vi, ViBuf buf, ViUInt32 cnt, ViPJobId jobId);
+ViStatus _VI_FUNC viWriteFromFile (ViSession vi, ViConstString filename, ViUInt32 cnt,
+ ViPUInt32 retCnt);
+ViStatus _VI_FUNC viAssertTrigger (ViSession vi, ViUInt16 protocol);
+ViStatus _VI_FUNC viReadSTB (ViSession vi, ViPUInt16 status);
+ViStatus _VI_FUNC viClear (ViSession vi);
+
+/*- Formatted and Buffered I/O Operations -----------------------------------*/
+
+ViStatus _VI_FUNC viSetBuf (ViSession vi, ViUInt16 mask, ViUInt32 size);
+ViStatus _VI_FUNC viFlush (ViSession vi, ViUInt16 mask);
+
+ViStatus _VI_FUNC viBufWrite (ViSession vi, ViBuf buf, ViUInt32 cnt, ViPUInt32 retCnt);
+ViStatus _VI_FUNC viBufRead (ViSession vi, ViPBuf buf, ViUInt32 cnt, ViPUInt32 retCnt);
+
+ViStatus _VI_FUNCC viPrintf (ViSession vi, ViString writeFmt, ...);
+ViStatus _VI_FUNC viVPrintf (ViSession vi, ViString writeFmt, ViVAList params);
+ViStatus _VI_FUNCC viSPrintf (ViSession vi, ViPBuf buf, ViString writeFmt, ...);
+ViStatus _VI_FUNC viVSPrintf (ViSession vi, ViPBuf buf, ViString writeFmt,
+ ViVAList parms);
+
+ViStatus _VI_FUNCC viScanf (ViSession vi, ViString readFmt, ...);
+ViStatus _VI_FUNC viVScanf (ViSession vi, ViString readFmt, ViVAList params);
+ViStatus _VI_FUNCC viSScanf (ViSession vi, ViBuf buf, ViString readFmt, ...);
+ViStatus _VI_FUNC viVSScanf (ViSession vi, ViBuf buf, ViString readFmt,
+ ViVAList parms);
+
+ViStatus _VI_FUNCC viQueryf (ViSession vi, ViString writeFmt, ViString readFmt, ...);
+ViStatus _VI_FUNC viVQueryf (ViSession vi, ViString writeFmt, ViString readFmt,
+ ViVAList params);
+
+/*- Memory I/O Operations ---------------------------------------------------*/
+
+ViStatus _VI_FUNC viIn8 (ViSession vi, ViUInt16 space,
+ ViBusAddress offset, ViPUInt8 val8);
+ViStatus _VI_FUNC viOut8 (ViSession vi, ViUInt16 space,
+ ViBusAddress offset, ViUInt8 val8);
+ViStatus _VI_FUNC viIn16 (ViSession vi, ViUInt16 space,
+ ViBusAddress offset, ViPUInt16 val16);
+ViStatus _VI_FUNC viOut16 (ViSession vi, ViUInt16 space,
+ ViBusAddress offset, ViUInt16 val16);
+ViStatus _VI_FUNC viIn32 (ViSession vi, ViUInt16 space,
+ ViBusAddress offset, ViPUInt32 val32);
+ViStatus _VI_FUNC viOut32 (ViSession vi, ViUInt16 space,
+ ViBusAddress offset, ViUInt32 val32);
+
+#if defined(_VI_INT64_UINT64_DEFINED)
+ViStatus _VI_FUNC viIn64 (ViSession vi, ViUInt16 space,
+ ViBusAddress offset, ViPUInt64 val64);
+ViStatus _VI_FUNC viOut64 (ViSession vi, ViUInt16 space,
+ ViBusAddress offset, ViUInt64 val64);
+
+ViStatus _VI_FUNC viIn8Ex (ViSession vi, ViUInt16 space,
+ ViBusAddress64 offset, ViPUInt8 val8);
+ViStatus _VI_FUNC viOut8Ex (ViSession vi, ViUInt16 space,
+ ViBusAddress64 offset, ViUInt8 val8);
+ViStatus _VI_FUNC viIn16Ex (ViSession vi, ViUInt16 space,
+ ViBusAddress64 offset, ViPUInt16 val16);
+ViStatus _VI_FUNC viOut16Ex (ViSession vi, ViUInt16 space,
+ ViBusAddress64 offset, ViUInt16 val16);
+ViStatus _VI_FUNC viIn32Ex (ViSession vi, ViUInt16 space,
+ ViBusAddress64 offset, ViPUInt32 val32);
+ViStatus _VI_FUNC viOut32Ex (ViSession vi, ViUInt16 space,
+ ViBusAddress64 offset, ViUInt32 val32);
+ViStatus _VI_FUNC viIn64Ex (ViSession vi, ViUInt16 space,
+ ViBusAddress64 offset, ViPUInt64 val64);
+ViStatus _VI_FUNC viOut64Ex (ViSession vi, ViUInt16 space,
+ ViBusAddress64 offset, ViUInt64 val64);
+#endif
+
+ViStatus _VI_FUNC viMoveIn8 (ViSession vi, ViUInt16 space, ViBusAddress offset,
+ ViBusSize length, ViAUInt8 buf8);
+ViStatus _VI_FUNC viMoveOut8 (ViSession vi, ViUInt16 space, ViBusAddress offset,
+ ViBusSize length, ViAUInt8 buf8);
+ViStatus _VI_FUNC viMoveIn16 (ViSession vi, ViUInt16 space, ViBusAddress offset,
+ ViBusSize length, ViAUInt16 buf16);
+ViStatus _VI_FUNC viMoveOut16 (ViSession vi, ViUInt16 space, ViBusAddress offset,
+ ViBusSize length, ViAUInt16 buf16);
+ViStatus _VI_FUNC viMoveIn32 (ViSession vi, ViUInt16 space, ViBusAddress offset,
+ ViBusSize length, ViAUInt32 buf32);
+ViStatus _VI_FUNC viMoveOut32 (ViSession vi, ViUInt16 space, ViBusAddress offset,
+ ViBusSize length, ViAUInt32 buf32);
+
+#if defined(_VI_INT64_UINT64_DEFINED)
+ViStatus _VI_FUNC viMoveIn64 (ViSession vi, ViUInt16 space, ViBusAddress offset,
+ ViBusSize length, ViAUInt64 buf64);
+ViStatus _VI_FUNC viMoveOut64 (ViSession vi, ViUInt16 space, ViBusAddress offset,
+ ViBusSize length, ViAUInt64 buf64);
+
+ViStatus _VI_FUNC viMoveIn8Ex (ViSession vi, ViUInt16 space, ViBusAddress64 offset,
+ ViBusSize length, ViAUInt8 buf8);
+ViStatus _VI_FUNC viMoveOut8Ex (ViSession vi, ViUInt16 space, ViBusAddress64 offset,
+ ViBusSize length, ViAUInt8 buf8);
+ViStatus _VI_FUNC viMoveIn16Ex (ViSession vi, ViUInt16 space, ViBusAddress64 offset,
+ ViBusSize length, ViAUInt16 buf16);
+ViStatus _VI_FUNC viMoveOut16Ex (ViSession vi, ViUInt16 space, ViBusAddress64 offset,
+ ViBusSize length, ViAUInt16 buf16);
+ViStatus _VI_FUNC viMoveIn32Ex (ViSession vi, ViUInt16 space, ViBusAddress64 offset,
+ ViBusSize length, ViAUInt32 buf32);
+ViStatus _VI_FUNC viMoveOut32Ex (ViSession vi, ViUInt16 space, ViBusAddress64 offset,
+ ViBusSize length, ViAUInt32 buf32);
+ViStatus _VI_FUNC viMoveIn64Ex (ViSession vi, ViUInt16 space, ViBusAddress64 offset,
+ ViBusSize length, ViAUInt64 buf64);
+ViStatus _VI_FUNC viMoveOut64Ex (ViSession vi, ViUInt16 space, ViBusAddress64 offset,
+ ViBusSize length, ViAUInt64 buf64);
+#endif
+
+ViStatus _VI_FUNC viMove (ViSession vi, ViUInt16 srcSpace, ViBusAddress srcOffset,
+ ViUInt16 srcWidth, ViUInt16 destSpace,
+ ViBusAddress destOffset, ViUInt16 destWidth,
+ ViBusSize srcLength);
+ViStatus _VI_FUNC viMoveAsync (ViSession vi, ViUInt16 srcSpace, ViBusAddress srcOffset,
+ ViUInt16 srcWidth, ViUInt16 destSpace,
+ ViBusAddress destOffset, ViUInt16 destWidth,
+ ViBusSize srcLength, ViPJobId jobId);
+
+#if defined(_VI_INT64_UINT64_DEFINED)
+ViStatus _VI_FUNC viMoveEx (ViSession vi, ViUInt16 srcSpace, ViBusAddress64 srcOffset,
+ ViUInt16 srcWidth, ViUInt16 destSpace,
+ ViBusAddress64 destOffset, ViUInt16 destWidth,
+ ViBusSize srcLength);
+ViStatus _VI_FUNC viMoveAsyncEx (ViSession vi, ViUInt16 srcSpace, ViBusAddress64 srcOffset,
+ ViUInt16 srcWidth, ViUInt16 destSpace,
+ ViBusAddress64 destOffset, ViUInt16 destWidth,
+ ViBusSize srcLength, ViPJobId jobId);
+#endif
+
+ViStatus _VI_FUNC viMapAddress (ViSession vi, ViUInt16 mapSpace, ViBusAddress mapOffset,
+ ViBusSize mapSize, ViBoolean access,
+ ViAddr suggested, ViPAddr address);
+ViStatus _VI_FUNC viUnmapAddress (ViSession vi);
+
+#if defined(_VI_INT64_UINT64_DEFINED)
+ViStatus _VI_FUNC viMapAddressEx (ViSession vi, ViUInt16 mapSpace, ViBusAddress64 mapOffset,
+ ViBusSize mapSize, ViBoolean access,
+ ViAddr suggested, ViPAddr address);
+#endif
+
+void _VI_FUNC viPeek8 (ViSession vi, ViAddr address, ViPUInt8 val8);
+void _VI_FUNC viPoke8 (ViSession vi, ViAddr address, ViUInt8 val8);
+void _VI_FUNC viPeek16 (ViSession vi, ViAddr address, ViPUInt16 val16);
+void _VI_FUNC viPoke16 (ViSession vi, ViAddr address, ViUInt16 val16);
+void _VI_FUNC viPeek32 (ViSession vi, ViAddr address, ViPUInt32 val32);
+void _VI_FUNC viPoke32 (ViSession vi, ViAddr address, ViUInt32 val32);
+
+#if defined(_VI_INT64_UINT64_DEFINED)
+void _VI_FUNC viPeek64 (ViSession vi, ViAddr address, ViPUInt64 val64);
+void _VI_FUNC viPoke64 (ViSession vi, ViAddr address, ViUInt64 val64);
+#endif
+
+/*- Shared Memory Operations ------------------------------------------------*/
+
+ViStatus _VI_FUNC viMemAlloc (ViSession vi, ViBusSize size, ViPBusAddress offset);
+ViStatus _VI_FUNC viMemFree (ViSession vi, ViBusAddress offset);
+
+#if defined(_VI_INT64_UINT64_DEFINED)
+ViStatus _VI_FUNC viMemAllocEx (ViSession vi, ViBusSize size, ViPBusAddress64 offset);
+ViStatus _VI_FUNC viMemFreeEx (ViSession vi, ViBusAddress64 offset);
+#endif
+
+/*- Interface Specific Operations -------------------------------------------*/
+
+ViStatus _VI_FUNC viGpibControlREN(ViSession vi, ViUInt16 mode);
+ViStatus _VI_FUNC viGpibControlATN(ViSession vi, ViUInt16 mode);
+ViStatus _VI_FUNC viGpibSendIFC (ViSession vi);
+ViStatus _VI_FUNC viGpibCommand (ViSession vi, ViBuf cmd, ViUInt32 cnt, ViPUInt32 retCnt);
+ViStatus _VI_FUNC viGpibPassControl(ViSession vi, ViUInt16 primAddr, ViUInt16 secAddr);
+
+ViStatus _VI_FUNC viVxiCommandQuery(ViSession vi, ViUInt16 mode, ViUInt32 cmd,
+ ViPUInt32 response);
+ViStatus _VI_FUNC viAssertUtilSignal(ViSession vi, ViUInt16 line);
+ViStatus _VI_FUNC viAssertIntrSignal(ViSession vi, ViInt16 mode, ViUInt32 statusID);
+ViStatus _VI_FUNC viMapTrigger (ViSession vi, ViInt16 trigSrc, ViInt16 trigDest,
+ ViUInt16 mode);
+ViStatus _VI_FUNC viUnmapTrigger (ViSession vi, ViInt16 trigSrc, ViInt16 trigDest);
+ViStatus _VI_FUNC viUsbControlOut (ViSession vi, ViInt16 bmRequestType, ViInt16 bRequest,
+ ViUInt16 wValue, ViUInt16 wIndex, ViUInt16 wLength,
+ ViBuf buf);
+ViStatus _VI_FUNC viUsbControlIn (ViSession vi, ViInt16 bmRequestType, ViInt16 bRequest,
+ ViUInt16 wValue, ViUInt16 wIndex, ViUInt16 wLength,
+ ViPBuf buf, ViPUInt16 retCnt);
+
+/*- Attributes (platform independent size) ----------------------------------*/
+
+#define VI_ATTR_RSRC_CLASS (0xBFFF0001UL)
+#define VI_ATTR_RSRC_NAME (0xBFFF0002UL)
+#define VI_ATTR_RSRC_IMPL_VERSION (0x3FFF0003UL)
+#define VI_ATTR_RSRC_LOCK_STATE (0x3FFF0004UL)
+#define VI_ATTR_MAX_QUEUE_LENGTH (0x3FFF0005UL)
+#define VI_ATTR_USER_DATA_32 (0x3FFF0007UL)
+#define VI_ATTR_FDC_CHNL (0x3FFF000DUL)
+#define VI_ATTR_FDC_MODE (0x3FFF000FUL)
+#define VI_ATTR_FDC_GEN_SIGNAL_EN (0x3FFF0011UL)
+#define VI_ATTR_FDC_USE_PAIR (0x3FFF0013UL)
+#define VI_ATTR_SEND_END_EN (0x3FFF0016UL)
+#define VI_ATTR_TERMCHAR (0x3FFF0018UL)
+#define VI_ATTR_TMO_VALUE (0x3FFF001AUL)
+#define VI_ATTR_GPIB_READDR_EN (0x3FFF001BUL)
+#define VI_ATTR_IO_PROT (0x3FFF001CUL)
+#define VI_ATTR_DMA_ALLOW_EN (0x3FFF001EUL)
+#define VI_ATTR_ASRL_BAUD (0x3FFF0021UL)
+#define VI_ATTR_ASRL_DATA_BITS (0x3FFF0022UL)
+#define VI_ATTR_ASRL_PARITY (0x3FFF0023UL)
+#define VI_ATTR_ASRL_STOP_BITS (0x3FFF0024UL)
+#define VI_ATTR_ASRL_FLOW_CNTRL (0x3FFF0025UL)
+#define VI_ATTR_RD_BUF_OPER_MODE (0x3FFF002AUL)
+#define VI_ATTR_RD_BUF_SIZE (0x3FFF002BUL)
+#define VI_ATTR_WR_BUF_OPER_MODE (0x3FFF002DUL)
+#define VI_ATTR_WR_BUF_SIZE (0x3FFF002EUL)
+#define VI_ATTR_SUPPRESS_END_EN (0x3FFF0036UL)
+#define VI_ATTR_TERMCHAR_EN (0x3FFF0038UL)
+#define VI_ATTR_DEST_ACCESS_PRIV (0x3FFF0039UL)
+#define VI_ATTR_DEST_BYTE_ORDER (0x3FFF003AUL)
+#define VI_ATTR_SRC_ACCESS_PRIV (0x3FFF003CUL)
+#define VI_ATTR_SRC_BYTE_ORDER (0x3FFF003DUL)
+#define VI_ATTR_SRC_INCREMENT (0x3FFF0040UL)
+#define VI_ATTR_DEST_INCREMENT (0x3FFF0041UL)
+#define VI_ATTR_WIN_ACCESS_PRIV (0x3FFF0045UL)
+#define VI_ATTR_WIN_BYTE_ORDER (0x3FFF0047UL)
+#define VI_ATTR_GPIB_ATN_STATE (0x3FFF0057UL)
+#define VI_ATTR_GPIB_ADDR_STATE (0x3FFF005CUL)
+#define VI_ATTR_GPIB_CIC_STATE (0x3FFF005EUL)
+#define VI_ATTR_GPIB_NDAC_STATE (0x3FFF0062UL)
+#define VI_ATTR_GPIB_SRQ_STATE (0x3FFF0067UL)
+#define VI_ATTR_GPIB_SYS_CNTRL_STATE (0x3FFF0068UL)
+#define VI_ATTR_GPIB_HS488_CBL_LEN (0x3FFF0069UL)
+#define VI_ATTR_CMDR_LA (0x3FFF006BUL)
+#define VI_ATTR_VXI_DEV_CLASS (0x3FFF006CUL)
+#define VI_ATTR_MAINFRAME_LA (0x3FFF0070UL)
+#define VI_ATTR_MANF_NAME (0xBFFF0072UL)
+#define VI_ATTR_MODEL_NAME (0xBFFF0077UL)
+#define VI_ATTR_VXI_VME_INTR_STATUS (0x3FFF008BUL)
+#define VI_ATTR_VXI_TRIG_STATUS (0x3FFF008DUL)
+#define VI_ATTR_VXI_VME_SYSFAIL_STATE (0x3FFF0094UL)
+#define VI_ATTR_WIN_BASE_ADDR_32 (0x3FFF0098UL)
+#define VI_ATTR_WIN_SIZE_32 (0x3FFF009AUL)
+#define VI_ATTR_ASRL_AVAIL_NUM (0x3FFF00ACUL)
+#define VI_ATTR_MEM_BASE_32 (0x3FFF00ADUL)
+#define VI_ATTR_ASRL_CTS_STATE (0x3FFF00AEUL)
+#define VI_ATTR_ASRL_DCD_STATE (0x3FFF00AFUL)
+#define VI_ATTR_ASRL_DSR_STATE (0x3FFF00B1UL)
+#define VI_ATTR_ASRL_DTR_STATE (0x3FFF00B2UL)
+#define VI_ATTR_ASRL_END_IN (0x3FFF00B3UL)
+#define VI_ATTR_ASRL_END_OUT (0x3FFF00B4UL)
+#define VI_ATTR_ASRL_REPLACE_CHAR (0x3FFF00BEUL)
+#define VI_ATTR_ASRL_RI_STATE (0x3FFF00BFUL)
+#define VI_ATTR_ASRL_RTS_STATE (0x3FFF00C0UL)
+#define VI_ATTR_ASRL_XON_CHAR (0x3FFF00C1UL)
+#define VI_ATTR_ASRL_XOFF_CHAR (0x3FFF00C2UL)
+#define VI_ATTR_WIN_ACCESS (0x3FFF00C3UL)
+#define VI_ATTR_RM_SESSION (0x3FFF00C4UL)
+#define VI_ATTR_VXI_LA (0x3FFF00D5UL)
+#define VI_ATTR_MANF_ID (0x3FFF00D9UL)
+#define VI_ATTR_MEM_SIZE_32 (0x3FFF00DDUL)
+#define VI_ATTR_MEM_SPACE (0x3FFF00DEUL)
+#define VI_ATTR_MODEL_CODE (0x3FFF00DFUL)
+#define VI_ATTR_SLOT (0x3FFF00E8UL)
+#define VI_ATTR_INTF_INST_NAME (0xBFFF00E9UL)
+#define VI_ATTR_IMMEDIATE_SERV (0x3FFF0100UL)
+#define VI_ATTR_INTF_PARENT_NUM (0x3FFF0101UL)
+#define VI_ATTR_RSRC_SPEC_VERSION (0x3FFF0170UL)
+#define VI_ATTR_INTF_TYPE (0x3FFF0171UL)
+#define VI_ATTR_GPIB_PRIMARY_ADDR (0x3FFF0172UL)
+#define VI_ATTR_GPIB_SECONDARY_ADDR (0x3FFF0173UL)
+#define VI_ATTR_RSRC_MANF_NAME (0xBFFF0174UL)
+#define VI_ATTR_RSRC_MANF_ID (0x3FFF0175UL)
+#define VI_ATTR_INTF_NUM (0x3FFF0176UL)
+#define VI_ATTR_TRIG_ID (0x3FFF0177UL)
+#define VI_ATTR_GPIB_REN_STATE (0x3FFF0181UL)
+#define VI_ATTR_GPIB_UNADDR_EN (0x3FFF0184UL)
+#define VI_ATTR_DEV_STATUS_BYTE (0x3FFF0189UL)
+#define VI_ATTR_FILE_APPEND_EN (0x3FFF0192UL)
+#define VI_ATTR_VXI_TRIG_SUPPORT (0x3FFF0194UL)
+#define VI_ATTR_TCPIP_ADDR (0xBFFF0195UL)
+#define VI_ATTR_TCPIP_HOSTNAME (0xBFFF0196UL)
+#define VI_ATTR_TCPIP_PORT (0x3FFF0197UL)
+#define VI_ATTR_TCPIP_DEVICE_NAME (0xBFFF0199UL)
+#define VI_ATTR_TCPIP_NODELAY (0x3FFF019AUL)
+#define VI_ATTR_TCPIP_KEEPALIVE (0x3FFF019BUL)
+#define VI_ATTR_4882_COMPLIANT (0x3FFF019FUL)
+#define VI_ATTR_USB_SERIAL_NUM (0xBFFF01A0UL)
+#define VI_ATTR_USB_INTFC_NUM (0x3FFF01A1UL)
+#define VI_ATTR_USB_PROTOCOL (0x3FFF01A7UL)
+#define VI_ATTR_USB_MAX_INTR_SIZE (0x3FFF01AFUL)
+#define VI_ATTR_PXI_DEV_NUM (0x3FFF0201UL)
+#define VI_ATTR_PXI_FUNC_NUM (0x3FFF0202UL)
+#define VI_ATTR_PXI_BUS_NUM (0x3FFF0205UL)
+#define VI_ATTR_PXI_CHASSIS (0x3FFF0206UL)
+#define VI_ATTR_PXI_SLOTPATH (0xBFFF0207UL)
+#define VI_ATTR_PXI_SLOT_LBUS_LEFT (0x3FFF0208UL)
+#define VI_ATTR_PXI_SLOT_LBUS_RIGHT (0x3FFF0209UL)
+#define VI_ATTR_PXI_TRIG_BUS (0x3FFF020AUL)
+#define VI_ATTR_PXI_STAR_TRIG_BUS (0x3FFF020BUL)
+#define VI_ATTR_PXI_STAR_TRIG_LINE (0x3FFF020CUL)
+#define VI_ATTR_PXI_MEM_TYPE_BAR0 (0x3FFF0211UL)
+#define VI_ATTR_PXI_MEM_TYPE_BAR1 (0x3FFF0212UL)
+#define VI_ATTR_PXI_MEM_TYPE_BAR2 (0x3FFF0213UL)
+#define VI_ATTR_PXI_MEM_TYPE_BAR3 (0x3FFF0214UL)
+#define VI_ATTR_PXI_MEM_TYPE_BAR4 (0x3FFF0215UL)
+#define VI_ATTR_PXI_MEM_TYPE_BAR5 (0x3FFF0216UL)
+#define VI_ATTR_PXI_MEM_BASE_BAR0 (0x3FFF0221UL)
+#define VI_ATTR_PXI_MEM_BASE_BAR1 (0x3FFF0222UL)
+#define VI_ATTR_PXI_MEM_BASE_BAR2 (0x3FFF0223UL)
+#define VI_ATTR_PXI_MEM_BASE_BAR3 (0x3FFF0224UL)
+#define VI_ATTR_PXI_MEM_BASE_BAR4 (0x3FFF0225UL)
+#define VI_ATTR_PXI_MEM_BASE_BAR5 (0x3FFF0226UL)
+#define VI_ATTR_PXI_MEM_SIZE_BAR0 (0x3FFF0231UL)
+#define VI_ATTR_PXI_MEM_SIZE_BAR1 (0x3FFF0232UL)
+#define VI_ATTR_PXI_MEM_SIZE_BAR2 (0x3FFF0233UL)
+#define VI_ATTR_PXI_MEM_SIZE_BAR3 (0x3FFF0234UL)
+#define VI_ATTR_PXI_MEM_SIZE_BAR4 (0x3FFF0235UL)
+#define VI_ATTR_PXI_MEM_SIZE_BAR5 (0x3FFF0236UL)
+#define VI_ATTR_PXI_IS_EXPRESS (0x3FFF0240UL)
+#define VI_ATTR_PXI_SLOT_LWIDTH (0x3FFF0241UL)
+#define VI_ATTR_PXI_MAX_LWIDTH (0x3FFF0242UL)
+#define VI_ATTR_PXI_ACTUAL_LWIDTH (0x3FFF0243UL)
+#define VI_ATTR_PXI_DSTAR_BUS (0x3FFF0244UL)
+#define VI_ATTR_PXI_DSTAR_SET (0x3FFF0245UL)
+
+#define VI_ATTR_JOB_ID (0x3FFF4006UL)
+#define VI_ATTR_EVENT_TYPE (0x3FFF4010UL)
+#define VI_ATTR_SIGP_STATUS_ID (0x3FFF4011UL)
+#define VI_ATTR_RECV_TRIG_ID (0x3FFF4012UL)
+#define VI_ATTR_INTR_STATUS_ID (0x3FFF4023UL)
+#define VI_ATTR_STATUS (0x3FFF4025UL)
+#define VI_ATTR_RET_COUNT_32 (0x3FFF4026UL)
+#define VI_ATTR_BUFFER (0x3FFF4027UL)
+#define VI_ATTR_RECV_INTR_LEVEL (0x3FFF4041UL)
+#define VI_ATTR_OPER_NAME (0xBFFF4042UL)
+#define VI_ATTR_GPIB_RECV_CIC_STATE (0x3FFF4193UL)
+#define VI_ATTR_RECV_TCPIP_ADDR (0xBFFF4198UL)
+#define VI_ATTR_USB_RECV_INTR_SIZE (0x3FFF41B0UL)
+#define VI_ATTR_USB_RECV_INTR_DATA (0xBFFF41B1UL)
+
+/*- Attributes (platform dependent size) ------------------------------------*/
+
+#if defined(_VI_INT64_UINT64_DEFINED) && defined(_VISA_ENV_IS_64_BIT)
+#define VI_ATTR_USER_DATA_64 (0x3FFF000AUL)
+#define VI_ATTR_RET_COUNT_64 (0x3FFF4028UL)
+#define VI_ATTR_USER_DATA (VI_ATTR_USER_DATA_64)
+#define VI_ATTR_RET_COUNT (VI_ATTR_RET_COUNT_64)
+#else
+#define VI_ATTR_USER_DATA (VI_ATTR_USER_DATA_32)
+#define VI_ATTR_RET_COUNT (VI_ATTR_RET_COUNT_32)
+#endif
+
+#if defined(_VI_INT64_UINT64_DEFINED)
+#define VI_ATTR_WIN_BASE_ADDR_64 (0x3FFF009BUL)
+#define VI_ATTR_WIN_SIZE_64 (0x3FFF009CUL)
+#define VI_ATTR_MEM_BASE_64 (0x3FFF00D0UL)
+#define VI_ATTR_MEM_SIZE_64 (0x3FFF00D1UL)
+#endif
+#if defined(_VI_INT64_UINT64_DEFINED) && defined(_VISA_ENV_IS_64_BIT)
+#define VI_ATTR_WIN_BASE_ADDR (VI_ATTR_WIN_BASE_ADDR_64)
+#define VI_ATTR_WIN_SIZE (VI_ATTR_WIN_SIZE_64)
+#define VI_ATTR_MEM_BASE (VI_ATTR_MEM_BASE_64)
+#define VI_ATTR_MEM_SIZE (VI_ATTR_MEM_SIZE_64)
+#else
+#define VI_ATTR_WIN_BASE_ADDR (VI_ATTR_WIN_BASE_ADDR_32)
+#define VI_ATTR_WIN_SIZE (VI_ATTR_WIN_SIZE_32)
+#define VI_ATTR_MEM_BASE (VI_ATTR_MEM_BASE_32)
+#define VI_ATTR_MEM_SIZE (VI_ATTR_MEM_SIZE_32)
+#endif
+
+/*- Event Types -------------------------------------------------------------*/
+
+#define VI_EVENT_IO_COMPLETION (0x3FFF2009UL)
+#define VI_EVENT_TRIG (0xBFFF200AUL)
+#define VI_EVENT_SERVICE_REQ (0x3FFF200BUL)
+#define VI_EVENT_CLEAR (0x3FFF200DUL)
+#define VI_EVENT_EXCEPTION (0xBFFF200EUL)
+#define VI_EVENT_GPIB_CIC (0x3FFF2012UL)
+#define VI_EVENT_GPIB_TALK (0x3FFF2013UL)
+#define VI_EVENT_GPIB_LISTEN (0x3FFF2014UL)
+#define VI_EVENT_VXI_VME_SYSFAIL (0x3FFF201DUL)
+#define VI_EVENT_VXI_VME_SYSRESET (0x3FFF201EUL)
+#define VI_EVENT_VXI_SIGP (0x3FFF2020UL)
+#define VI_EVENT_VXI_VME_INTR (0xBFFF2021UL)
+#define VI_EVENT_PXI_INTR (0x3FFF2022UL)
+#define VI_EVENT_TCPIP_CONNECT (0x3FFF2036UL)
+#define VI_EVENT_USB_INTR (0x3FFF2037UL)
+
+#define VI_ALL_ENABLED_EVENTS (0x3FFF7FFFUL)
+
+/*- Completion and Error Codes ----------------------------------------------*/
+
+#define VI_SUCCESS_EVENT_EN (0x3FFF0002L) /* 3FFF0002, 1073676290 */
+#define VI_SUCCESS_EVENT_DIS (0x3FFF0003L) /* 3FFF0003, 1073676291 */
+#define VI_SUCCESS_QUEUE_EMPTY (0x3FFF0004L) /* 3FFF0004, 1073676292 */
+#define VI_SUCCESS_TERM_CHAR (0x3FFF0005L) /* 3FFF0005, 1073676293 */
+#define VI_SUCCESS_MAX_CNT (0x3FFF0006L) /* 3FFF0006, 1073676294 */
+#define VI_SUCCESS_DEV_NPRESENT (0x3FFF007DL) /* 3FFF007D, 1073676413 */
+#define VI_SUCCESS_TRIG_MAPPED (0x3FFF007EL) /* 3FFF007E, 1073676414 */
+#define VI_SUCCESS_QUEUE_NEMPTY (0x3FFF0080L) /* 3FFF0080, 1073676416 */
+#define VI_SUCCESS_NCHAIN (0x3FFF0098L) /* 3FFF0098, 1073676440 */
+#define VI_SUCCESS_NESTED_SHARED (0x3FFF0099L) /* 3FFF0099, 1073676441 */
+#define VI_SUCCESS_NESTED_EXCLUSIVE (0x3FFF009AL) /* 3FFF009A, 1073676442 */
+#define VI_SUCCESS_SYNC (0x3FFF009BL) /* 3FFF009B, 1073676443 */
+
+#define VI_WARN_QUEUE_OVERFLOW (0x3FFF000CL) /* 3FFF000C, 1073676300 */
+#define VI_WARN_CONFIG_NLOADED (0x3FFF0077L) /* 3FFF0077, 1073676407 */
+#define VI_WARN_NULL_OBJECT (0x3FFF0082L) /* 3FFF0082, 1073676418 */
+#define VI_WARN_NSUP_ATTR_STATE (0x3FFF0084L) /* 3FFF0084, 1073676420 */
+#define VI_WARN_UNKNOWN_STATUS (0x3FFF0085L) /* 3FFF0085, 1073676421 */
+#define VI_WARN_NSUP_BUF (0x3FFF0088L) /* 3FFF0088, 1073676424 */
+#define VI_WARN_EXT_FUNC_NIMPL (0x3FFF00A9L) /* 3FFF00A9, 1073676457 */
+
+#define VI_ERROR_SYSTEM_ERROR (_VI_ERROR+0x3FFF0000L) /* BFFF0000, -1073807360 */
+#define VI_ERROR_INV_OBJECT (_VI_ERROR+0x3FFF000EL) /* BFFF000E, -1073807346 */
+#define VI_ERROR_RSRC_LOCKED (_VI_ERROR+0x3FFF000FL) /* BFFF000F, -1073807345 */
+#define VI_ERROR_INV_EXPR (_VI_ERROR+0x3FFF0010L) /* BFFF0010, -1073807344 */
+#define VI_ERROR_RSRC_NFOUND (_VI_ERROR+0x3FFF0011L) /* BFFF0011, -1073807343 */
+#define VI_ERROR_INV_RSRC_NAME (_VI_ERROR+0x3FFF0012L) /* BFFF0012, -1073807342 */
+#define VI_ERROR_INV_ACC_MODE (_VI_ERROR+0x3FFF0013L) /* BFFF0013, -1073807341 */
+#define VI_ERROR_TMO (_VI_ERROR+0x3FFF0015L) /* BFFF0015, -1073807339 */
+#define VI_ERROR_CLOSING_FAILED (_VI_ERROR+0x3FFF0016L) /* BFFF0016, -1073807338 */
+#define VI_ERROR_INV_DEGREE (_VI_ERROR+0x3FFF001BL) /* BFFF001B, -1073807333 */
+#define VI_ERROR_INV_JOB_ID (_VI_ERROR+0x3FFF001CL) /* BFFF001C, -1073807332 */
+#define VI_ERROR_NSUP_ATTR (_VI_ERROR+0x3FFF001DL) /* BFFF001D, -1073807331 */
+#define VI_ERROR_NSUP_ATTR_STATE (_VI_ERROR+0x3FFF001EL) /* BFFF001E, -1073807330 */
+#define VI_ERROR_ATTR_READONLY (_VI_ERROR+0x3FFF001FL) /* BFFF001F, -1073807329 */
+#define VI_ERROR_INV_LOCK_TYPE (_VI_ERROR+0x3FFF0020L) /* BFFF0020, -1073807328 */
+#define VI_ERROR_INV_ACCESS_KEY (_VI_ERROR+0x3FFF0021L) /* BFFF0021, -1073807327 */
+#define VI_ERROR_INV_EVENT (_VI_ERROR+0x3FFF0026L) /* BFFF0026, -1073807322 */
+#define VI_ERROR_INV_MECH (_VI_ERROR+0x3FFF0027L) /* BFFF0027, -1073807321 */
+#define VI_ERROR_HNDLR_NINSTALLED (_VI_ERROR+0x3FFF0028L) /* BFFF0028, -1073807320 */
+#define VI_ERROR_INV_HNDLR_REF (_VI_ERROR+0x3FFF0029L) /* BFFF0029, -1073807319 */
+#define VI_ERROR_INV_CONTEXT (_VI_ERROR+0x3FFF002AL) /* BFFF002A, -1073807318 */
+#define VI_ERROR_QUEUE_OVERFLOW (_VI_ERROR+0x3FFF002DL) /* BFFF002D, -1073807315 */
+#define VI_ERROR_NENABLED (_VI_ERROR+0x3FFF002FL) /* BFFF002F, -1073807313 */
+#define VI_ERROR_ABORT (_VI_ERROR+0x3FFF0030L) /* BFFF0030, -1073807312 */
+#define VI_ERROR_RAW_WR_PROT_VIOL (_VI_ERROR+0x3FFF0034L) /* BFFF0034, -1073807308 */
+#define VI_ERROR_RAW_RD_PROT_VIOL (_VI_ERROR+0x3FFF0035L) /* BFFF0035, -1073807307 */
+#define VI_ERROR_OUTP_PROT_VIOL (_VI_ERROR+0x3FFF0036L) /* BFFF0036, -1073807306 */
+#define VI_ERROR_INP_PROT_VIOL (_VI_ERROR+0x3FFF0037L) /* BFFF0037, -1073807305 */
+#define VI_ERROR_BERR (_VI_ERROR+0x3FFF0038L) /* BFFF0038, -1073807304 */
+#define VI_ERROR_IN_PROGRESS (_VI_ERROR+0x3FFF0039L) /* BFFF0039, -1073807303 */
+#define VI_ERROR_INV_SETUP (_VI_ERROR+0x3FFF003AL) /* BFFF003A, -1073807302 */
+#define VI_ERROR_QUEUE_ERROR (_VI_ERROR+0x3FFF003BL) /* BFFF003B, -1073807301 */
+#define VI_ERROR_ALLOC (_VI_ERROR+0x3FFF003CL) /* BFFF003C, -1073807300 */
+#define VI_ERROR_INV_MASK (_VI_ERROR+0x3FFF003DL) /* BFFF003D, -1073807299 */
+#define VI_ERROR_IO (_VI_ERROR+0x3FFF003EL) /* BFFF003E, -1073807298 */
+#define VI_ERROR_INV_FMT (_VI_ERROR+0x3FFF003FL) /* BFFF003F, -1073807297 */
+#define VI_ERROR_NSUP_FMT (_VI_ERROR+0x3FFF0041L) /* BFFF0041, -1073807295 */
+#define VI_ERROR_LINE_IN_USE (_VI_ERROR+0x3FFF0042L) /* BFFF0042, -1073807294 */
+#define VI_ERROR_NSUP_MODE (_VI_ERROR+0x3FFF0046L) /* BFFF0046, -1073807290 */
+#define VI_ERROR_SRQ_NOCCURRED (_VI_ERROR+0x3FFF004AL) /* BFFF004A, -1073807286 */
+#define VI_ERROR_INV_SPACE (_VI_ERROR+0x3FFF004EL) /* BFFF004E, -1073807282 */
+#define VI_ERROR_INV_OFFSET (_VI_ERROR+0x3FFF0051L) /* BFFF0051, -1073807279 */
+#define VI_ERROR_INV_WIDTH (_VI_ERROR+0x3FFF0052L) /* BFFF0052, -1073807278 */
+#define VI_ERROR_NSUP_OFFSET (_VI_ERROR+0x3FFF0054L) /* BFFF0054, -1073807276 */
+#define VI_ERROR_NSUP_VAR_WIDTH (_VI_ERROR+0x3FFF0055L) /* BFFF0055, -1073807275 */
+#define VI_ERROR_WINDOW_NMAPPED (_VI_ERROR+0x3FFF0057L) /* BFFF0057, -1073807273 */
+#define VI_ERROR_RESP_PENDING (_VI_ERROR+0x3FFF0059L) /* BFFF0059, -1073807271 */
+#define VI_ERROR_NLISTENERS (_VI_ERROR+0x3FFF005FL) /* BFFF005F, -1073807265 */
+#define VI_ERROR_NCIC (_VI_ERROR+0x3FFF0060L) /* BFFF0060, -1073807264 */
+#define VI_ERROR_NSYS_CNTLR (_VI_ERROR+0x3FFF0061L) /* BFFF0061, -1073807263 */
+#define VI_ERROR_NSUP_OPER (_VI_ERROR+0x3FFF0067L) /* BFFF0067, -1073807257 */
+#define VI_ERROR_INTR_PENDING (_VI_ERROR+0x3FFF0068L) /* BFFF0068, -1073807256 */
+#define VI_ERROR_ASRL_PARITY (_VI_ERROR+0x3FFF006AL) /* BFFF006A, -1073807254 */
+#define VI_ERROR_ASRL_FRAMING (_VI_ERROR+0x3FFF006BL) /* BFFF006B, -1073807253 */
+#define VI_ERROR_ASRL_OVERRUN (_VI_ERROR+0x3FFF006CL) /* BFFF006C, -1073807252 */
+#define VI_ERROR_TRIG_NMAPPED (_VI_ERROR+0x3FFF006EL) /* BFFF006E, -1073807250 */
+#define VI_ERROR_NSUP_ALIGN_OFFSET (_VI_ERROR+0x3FFF0070L) /* BFFF0070, -1073807248 */
+#define VI_ERROR_USER_BUF (_VI_ERROR+0x3FFF0071L) /* BFFF0071, -1073807247 */
+#define VI_ERROR_RSRC_BUSY (_VI_ERROR+0x3FFF0072L) /* BFFF0072, -1073807246 */
+#define VI_ERROR_NSUP_WIDTH (_VI_ERROR+0x3FFF0076L) /* BFFF0076, -1073807242 */
+#define VI_ERROR_INV_PARAMETER (_VI_ERROR+0x3FFF0078L) /* BFFF0078, -1073807240 */
+#define VI_ERROR_INV_PROT (_VI_ERROR+0x3FFF0079L) /* BFFF0079, -1073807239 */
+#define VI_ERROR_INV_SIZE (_VI_ERROR+0x3FFF007BL) /* BFFF007B, -1073807237 */
+#define VI_ERROR_WINDOW_MAPPED (_VI_ERROR+0x3FFF0080L) /* BFFF0080, -1073807232 */
+#define VI_ERROR_NIMPL_OPER (_VI_ERROR+0x3FFF0081L) /* BFFF0081, -1073807231 */
+#define VI_ERROR_INV_LENGTH (_VI_ERROR+0x3FFF0083L) /* BFFF0083, -1073807229 */
+#define VI_ERROR_INV_MODE (_VI_ERROR+0x3FFF0091L) /* BFFF0091, -1073807215 */
+#define VI_ERROR_SESN_NLOCKED (_VI_ERROR+0x3FFF009CL) /* BFFF009C, -1073807204 */
+#define VI_ERROR_MEM_NSHARED (_VI_ERROR+0x3FFF009DL) /* BFFF009D, -1073807203 */
+#define VI_ERROR_LIBRARY_NFOUND (_VI_ERROR+0x3FFF009EL) /* BFFF009E, -1073807202 */
+#define VI_ERROR_NSUP_INTR (_VI_ERROR+0x3FFF009FL) /* BFFF009F, -1073807201 */
+#define VI_ERROR_INV_LINE (_VI_ERROR+0x3FFF00A0L) /* BFFF00A0, -1073807200 */
+#define VI_ERROR_FILE_ACCESS (_VI_ERROR+0x3FFF00A1L) /* BFFF00A1, -1073807199 */
+#define VI_ERROR_FILE_IO (_VI_ERROR+0x3FFF00A2L) /* BFFF00A2, -1073807198 */
+#define VI_ERROR_NSUP_LINE (_VI_ERROR+0x3FFF00A3L) /* BFFF00A3, -1073807197 */
+#define VI_ERROR_NSUP_MECH (_VI_ERROR+0x3FFF00A4L) /* BFFF00A4, -1073807196 */
+#define VI_ERROR_INTF_NUM_NCONFIG (_VI_ERROR+0x3FFF00A5L) /* BFFF00A5, -1073807195 */
+#define VI_ERROR_CONN_LOST (_VI_ERROR+0x3FFF00A6L) /* BFFF00A6, -1073807194 */
+#define VI_ERROR_MACHINE_NAVAIL (_VI_ERROR+0x3FFF00A7L) /* BFFF00A7, -1073807193 */
+#define VI_ERROR_NPERMISSION (_VI_ERROR+0x3FFF00A8L) /* BFFF00A8, -1073807192 */
+
+/*- Other VISA Definitions --------------------------------------------------*/
+
+#define VI_VERSION_MAJOR(ver) ((((ViVersion)ver) & 0xFFF00000UL) >> 20)
+#define VI_VERSION_MINOR(ver) ((((ViVersion)ver) & 0x000FFF00UL) >> 8)
+#define VI_VERSION_SUBMINOR(ver) ((((ViVersion)ver) & 0x000000FFUL) )
+
+#define VI_FIND_BUFLEN (256)
+
+#define VI_INTF_GPIB (1)
+#define VI_INTF_VXI (2)
+#define VI_INTF_GPIB_VXI (3)
+#define VI_INTF_ASRL (4)
+#define VI_INTF_PXI (5)
+#define VI_INTF_TCPIP (6)
+#define VI_INTF_USB (7)
+
+#define VI_PROT_NORMAL (1)
+#define VI_PROT_FDC (2)
+#define VI_PROT_HS488 (3)
+#define VI_PROT_4882_STRS (4)
+#define VI_PROT_USBTMC_VENDOR (5)
+
+#define VI_FDC_NORMAL (1)
+#define VI_FDC_STREAM (2)
+
+#define VI_LOCAL_SPACE (0)
+#define VI_A16_SPACE (1)
+#define VI_A24_SPACE (2)
+#define VI_A32_SPACE (3)
+#define VI_A64_SPACE (4)
+#define VI_PXI_ALLOC_SPACE (9)
+#define VI_PXI_CFG_SPACE (10)
+#define VI_PXI_BAR0_SPACE (11)
+#define VI_PXI_BAR1_SPACE (12)
+#define VI_PXI_BAR2_SPACE (13)
+#define VI_PXI_BAR3_SPACE (14)
+#define VI_PXI_BAR4_SPACE (15)
+#define VI_PXI_BAR5_SPACE (16)
+#define VI_OPAQUE_SPACE (0xFFFF)
+
+#define VI_UNKNOWN_LA (-1)
+#define VI_UNKNOWN_SLOT (-1)
+#define VI_UNKNOWN_LEVEL (-1)
+#define VI_UNKNOWN_CHASSIS (-1)
+
+#define VI_QUEUE (1)
+#define VI_HNDLR (2)
+#define VI_SUSPEND_HNDLR (4)
+#define VI_ALL_MECH (0xFFFF)
+
+#define VI_ANY_HNDLR (0)
+
+#define VI_TRIG_ALL (-2)
+#define VI_TRIG_SW (-1)
+#define VI_TRIG_TTL0 (0)
+#define VI_TRIG_TTL1 (1)
+#define VI_TRIG_TTL2 (2)
+#define VI_TRIG_TTL3 (3)
+#define VI_TRIG_TTL4 (4)
+#define VI_TRIG_TTL5 (5)
+#define VI_TRIG_TTL6 (6)
+#define VI_TRIG_TTL7 (7)
+#define VI_TRIG_ECL0 (8)
+#define VI_TRIG_ECL1 (9)
+#define VI_TRIG_PANEL_IN (27)
+#define VI_TRIG_PANEL_OUT (28)
+
+#define VI_TRIG_PROT_DEFAULT (0)
+#define VI_TRIG_PROT_ON (1)
+#define VI_TRIG_PROT_OFF (2)
+#define VI_TRIG_PROT_SYNC (5)
+#define VI_TRIG_PROT_RESERVE (6)
+#define VI_TRIG_PROT_UNRESERVE (7)
+
+#define VI_READ_BUF (1)
+#define VI_WRITE_BUF (2)
+#define VI_READ_BUF_DISCARD (4)
+#define VI_WRITE_BUF_DISCARD (8)
+#define VI_IO_IN_BUF (16)
+#define VI_IO_OUT_BUF (32)
+#define VI_IO_IN_BUF_DISCARD (64)
+#define VI_IO_OUT_BUF_DISCARD (128)
+
+#define VI_FLUSH_ON_ACCESS (1)
+#define VI_FLUSH_WHEN_FULL (2)
+#define VI_FLUSH_DISABLE (3)
+
+#define VI_NMAPPED (1)
+#define VI_USE_OPERS (2)
+#define VI_DEREF_ADDR (3)
+#define VI_DEREF_ADDR_BYTE_SWAP (4)
+
+#define VI_TMO_IMMEDIATE (0L)
+#define VI_TMO_INFINITE (0xFFFFFFFFUL)
+
+#define VI_NO_LOCK (0)
+#define VI_EXCLUSIVE_LOCK (1)
+#define VI_SHARED_LOCK (2)
+#define VI_LOAD_CONFIG (4)
+
+#define VI_NO_SEC_ADDR (0xFFFF)
+
+#define VI_ASRL_PAR_NONE (0)
+#define VI_ASRL_PAR_ODD (1)
+#define VI_ASRL_PAR_EVEN (2)
+#define VI_ASRL_PAR_MARK (3)
+#define VI_ASRL_PAR_SPACE (4)
+
+#define VI_ASRL_STOP_ONE (10)
+#define VI_ASRL_STOP_ONE5 (15)
+#define VI_ASRL_STOP_TWO (20)
+
+#define VI_ASRL_FLOW_NONE (0)
+#define VI_ASRL_FLOW_XON_XOFF (1)
+#define VI_ASRL_FLOW_RTS_CTS (2)
+#define VI_ASRL_FLOW_DTR_DSR (4)
+
+#define VI_ASRL_END_NONE (0)
+#define VI_ASRL_END_LAST_BIT (1)
+#define VI_ASRL_END_TERMCHAR (2)
+#define VI_ASRL_END_BREAK (3)
+
+#define VI_STATE_ASSERTED (1)
+#define VI_STATE_UNASSERTED (0)
+#define VI_STATE_UNKNOWN (-1)
+
+#define VI_BIG_ENDIAN (0)
+#define VI_LITTLE_ENDIAN (1)
+
+#define VI_DATA_PRIV (0)
+#define VI_DATA_NPRIV (1)
+#define VI_PROG_PRIV (2)
+#define VI_PROG_NPRIV (3)
+#define VI_BLCK_PRIV (4)
+#define VI_BLCK_NPRIV (5)
+#define VI_D64_PRIV (6)
+#define VI_D64_NPRIV (7)
+
+#define VI_WIDTH_8 (1)
+#define VI_WIDTH_16 (2)
+#define VI_WIDTH_32 (4)
+#define VI_WIDTH_64 (8)
+
+#define VI_GPIB_REN_DEASSERT (0)
+#define VI_GPIB_REN_ASSERT (1)
+#define VI_GPIB_REN_DEASSERT_GTL (2)
+#define VI_GPIB_REN_ASSERT_ADDRESS (3)
+#define VI_GPIB_REN_ASSERT_LLO (4)
+#define VI_GPIB_REN_ASSERT_ADDRESS_LLO (5)
+#define VI_GPIB_REN_ADDRESS_GTL (6)
+
+#define VI_GPIB_ATN_DEASSERT (0)
+#define VI_GPIB_ATN_ASSERT (1)
+#define VI_GPIB_ATN_DEASSERT_HANDSHAKE (2)
+#define VI_GPIB_ATN_ASSERT_IMMEDIATE (3)
+
+#define VI_GPIB_HS488_DISABLED (0)
+#define VI_GPIB_HS488_NIMPL (-1)
+
+#define VI_GPIB_UNADDRESSED (0)
+#define VI_GPIB_TALKER (1)
+#define VI_GPIB_LISTENER (2)
+
+#define VI_VXI_CMD16 (0x0200)
+#define VI_VXI_CMD16_RESP16 (0x0202)
+#define VI_VXI_RESP16 (0x0002)
+#define VI_VXI_CMD32 (0x0400)
+#define VI_VXI_CMD32_RESP16 (0x0402)
+#define VI_VXI_CMD32_RESP32 (0x0404)
+#define VI_VXI_RESP32 (0x0004)
+
+#define VI_ASSERT_SIGNAL (-1)
+#define VI_ASSERT_USE_ASSIGNED (0)
+#define VI_ASSERT_IRQ1 (1)
+#define VI_ASSERT_IRQ2 (2)
+#define VI_ASSERT_IRQ3 (3)
+#define VI_ASSERT_IRQ4 (4)
+#define VI_ASSERT_IRQ5 (5)
+#define VI_ASSERT_IRQ6 (6)
+#define VI_ASSERT_IRQ7 (7)
+
+#define VI_UTIL_ASSERT_SYSRESET (1)
+#define VI_UTIL_ASSERT_SYSFAIL (2)
+#define VI_UTIL_DEASSERT_SYSFAIL (3)
+
+#define VI_VXI_CLASS_MEMORY (0)
+#define VI_VXI_CLASS_EXTENDED (1)
+#define VI_VXI_CLASS_MESSAGE (2)
+#define VI_VXI_CLASS_REGISTER (3)
+#define VI_VXI_CLASS_OTHER (4)
+
+#define VI_PXI_ADDR_NONE (0)
+#define VI_PXI_ADDR_MEM (1)
+#define VI_PXI_ADDR_IO (2)
+#define VI_PXI_ADDR_CFG (3)
+
+#define VI_TRIG_UNKNOWN (-1)
+
+#define VI_PXI_LBUS_UNKNOWN (-1)
+#define VI_PXI_LBUS_NONE (0)
+#define VI_PXI_LBUS_STAR_TRIG_BUS_0 (1000)
+#define VI_PXI_LBUS_STAR_TRIG_BUS_1 (1001)
+#define VI_PXI_LBUS_STAR_TRIG_BUS_2 (1002)
+#define VI_PXI_LBUS_STAR_TRIG_BUS_3 (1003)
+#define VI_PXI_LBUS_STAR_TRIG_BUS_4 (1004)
+#define VI_PXI_LBUS_STAR_TRIG_BUS_5 (1005)
+#define VI_PXI_LBUS_STAR_TRIG_BUS_6 (1006)
+#define VI_PXI_LBUS_STAR_TRIG_BUS_7 (1007)
+#define VI_PXI_LBUS_STAR_TRIG_BUS_8 (1008)
+#define VI_PXI_LBUS_STAR_TRIG_BUS_9 (1009)
+#define VI_PXI_STAR_TRIG_CONTROLLER (1413)
+
+/*- Backward Compatibility Macros -------------------------------------------*/
+
+#define viGetDefaultRM(vi) viOpenDefaultRM(vi)
+#define VI_ERROR_INV_SESSION (VI_ERROR_INV_OBJECT)
+#define VI_INFINITE (VI_TMO_INFINITE)
+#define VI_NORMAL (VI_PROT_NORMAL)
+#define VI_FDC (VI_PROT_FDC)
+#define VI_HS488 (VI_PROT_HS488)
+#define VI_ASRL488 (VI_PROT_4882_STRS)
+#define VI_ASRL_IN_BUF (VI_IO_IN_BUF)
+#define VI_ASRL_OUT_BUF (VI_IO_OUT_BUF)
+#define VI_ASRL_IN_BUF_DISCARD (VI_IO_IN_BUF_DISCARD)
+#define VI_ASRL_OUT_BUF_DISCARD (VI_IO_OUT_BUF_DISCARD)
+
+/*- National Instruments ----------------------------------------------------*/
+
+#define VI_INTF_RIO (8)
+#define VI_INTF_FIREWIRE (9)
+
+#define VI_ATTR_SYNC_MXI_ALLOW_EN (0x3FFF0161UL) /* ViBoolean, read/write */
+
+/* This is for VXI SERVANT resources */
+
+#define VI_EVENT_VXI_DEV_CMD (0xBFFF200FUL)
+#define VI_ATTR_VXI_DEV_CMD_TYPE (0x3FFF4037UL) /* ViInt16, read-only */
+#define VI_ATTR_VXI_DEV_CMD_VALUE (0x3FFF4038UL) /* ViUInt32, read-only */
+
+#define VI_VXI_DEV_CMD_TYPE_16 (16)
+#define VI_VXI_DEV_CMD_TYPE_32 (32)
+
+ViStatus _VI_FUNC viVxiServantResponse(ViSession vi, ViInt16 mode, ViUInt32 resp);
+/* mode values include VI_VXI_RESP16, VI_VXI_RESP32, and the next 2 values */
+#define VI_VXI_RESP_NONE (0)
+#define VI_VXI_RESP_PROT_ERROR (-1)
+
+/* This allows extended Serial support on Win32 and on NI ENET Serial products */
+
+#define VI_ATTR_ASRL_DISCARD_NULL (0x3FFF00B0UL)
+#define VI_ATTR_ASRL_CONNECTED (0x3FFF01BBUL)
+#define VI_ATTR_ASRL_BREAK_STATE (0x3FFF01BCUL)
+#define VI_ATTR_ASRL_BREAK_LEN (0x3FFF01BDUL)
+#define VI_ATTR_ASRL_ALLOW_TRANSMIT (0x3FFF01BEUL)
+#define VI_ATTR_ASRL_WIRE_MODE (0x3FFF01BFUL)
+
+#define VI_ASRL_WIRE_485_4 (0)
+#define VI_ASRL_WIRE_485_2_DTR_ECHO (1)
+#define VI_ASRL_WIRE_485_2_DTR_CTRL (2)
+#define VI_ASRL_WIRE_485_2_AUTO (3)
+#define VI_ASRL_WIRE_232_DTE (128)
+#define VI_ASRL_WIRE_232_DCE (129)
+#define VI_ASRL_WIRE_232_AUTO (130)
+
+#define VI_EVENT_ASRL_BREAK (0x3FFF2023UL)
+#define VI_EVENT_ASRL_CTS (0x3FFF2029UL)
+#define VI_EVENT_ASRL_DSR (0x3FFF202AUL)
+#define VI_EVENT_ASRL_DCD (0x3FFF202CUL)
+#define VI_EVENT_ASRL_RI (0x3FFF202EUL)
+#define VI_EVENT_ASRL_CHAR (0x3FFF2035UL)
+#define VI_EVENT_ASRL_TERMCHAR (0x3FFF2024UL)
+
+/* This is for fast viPeek/viPoke macros */
+
+#if defined(NIVISA_PEEKPOKE)
+
+#if defined(NIVISA_PEEKPOKE_SUPP)
+#undef NIVISA_PEEKPOKE_SUPP
+#endif
+
+#if (defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__)) && !defined(_NI_mswin16_)
+/* This macro is supported for all Win32 compilers, including CVI. */
+#define NIVISA_PEEKPOKE_SUPP
+#elif (defined(_WINDOWS) || defined(_Windows)) && !defined(_CVI_) && !defined(_NI_mswin16_)
+/* This macro is supported for Borland and Microsoft compilers on Win16, but not CVI. */
+#define NIVISA_PEEKPOKE_SUPP
+#elif defined(_CVI_) && defined(_NI_sparc_)
+/* This macro is supported for Solaris 1 and 2, from CVI only. */
+#define NIVISA_PEEKPOKE_SUPP
+#else
+/* This macro is not supported on other platforms. */
+#endif
+
+#if defined(NIVISA_PEEKPOKE_SUPP)
+
+extern ViBoolean NI_viImplVISA1;
+ViStatus _VI_FUNC NI_viOpenDefaultRM (ViPSession vi);
+#define viOpenDefaultRM(vi) NI_viOpenDefaultRM(vi)
+
+#define viPeek8(vi,addr,val) \
+ { \
+ if ((NI_viImplVISA1) && (*((ViPUInt32)(vi)))) \
+ { \
+ do (*((ViPUInt8)(val)) = *((volatile ViUInt8 _VI_PTR)(addr))); \
+ while (**((volatile ViUInt8 _VI_PTR _VI_PTR)(vi)) & 0x10); \
+ } \
+ else \
+ { \
+ (viPeek8)((vi),(addr),(val)); \
+ } \
+ }
+
+#define viPoke8(vi,addr,val) \
+ { \
+ if ((NI_viImplVISA1) && (*((ViPUInt32)(vi)))) \
+ { \
+ do (*((volatile ViUInt8 _VI_PTR)(addr)) = ((ViUInt8)(val))); \
+ while (**((volatile ViUInt8 _VI_PTR _VI_PTR)(vi)) & 0x10); \
+ } \
+ else \
+ { \
+ (viPoke8)((vi),(addr),(val)); \
+ } \
+ }
+
+#define viPeek16(vi,addr,val) \
+ { \
+ if ((NI_viImplVISA1) && (*((ViPUInt32)(vi)))) \
+ { \
+ do (*((ViPUInt16)(val)) = *((volatile ViUInt16 _VI_PTR)(addr))); \
+ while (**((volatile ViUInt8 _VI_PTR _VI_PTR)(vi)) & 0x10); \
+ } \
+ else \
+ { \
+ (viPeek16)((vi),(addr),(val)); \
+ } \
+ }
+
+#define viPoke16(vi,addr,val) \
+ { \
+ if ((NI_viImplVISA1) && (*((ViPUInt32)(vi)))) \
+ { \
+ do (*((volatile ViUInt16 _VI_PTR)(addr)) = ((ViUInt16)(val))); \
+ while (**((volatile ViUInt8 _VI_PTR _VI_PTR)(vi)) & 0x10); \
+ } \
+ else \
+ { \
+ (viPoke16)((vi),(addr),(val)); \
+ } \
+ }
+
+#define viPeek32(vi,addr,val) \
+ { \
+ if ((NI_viImplVISA1) && (*((ViPUInt32)(vi)))) \
+ { \
+ do (*((ViPUInt32)(val)) = *((volatile ViUInt32 _VI_PTR)(addr))); \
+ while (**((volatile ViUInt8 _VI_PTR _VI_PTR)(vi)) & 0x10); \
+ } \
+ else \
+ { \
+ (viPeek32)((vi),(addr),(val)); \
+ } \
+ }
+
+#define viPoke32(vi,addr,val) \
+ { \
+ if ((NI_viImplVISA1) && (*((ViPUInt32)(vi)))) \
+ { \
+ do (*((volatile ViUInt32 _VI_PTR)(addr)) = ((ViUInt32)(val))); \
+ while (**((volatile ViUInt8 _VI_PTR _VI_PTR)(vi)) & 0x10); \
+ } \
+ else \
+ { \
+ (viPoke32)((vi),(addr),(val)); \
+ } \
+ }
+
+#endif
+
+#endif
+
+#if defined(NIVISA_PXI) || defined(PXISAVISA_PXI)
+
+#if 0
+/* The following 2 attributes were incorrectly implemented in earlier
+ versions of NI-VISA. You should now query VI_ATTR_MANF_ID or
+ VI_ATTR_MODEL_CODE. Those attributes contain sub-vendor information
+ when it exists. To get both the actual primary and subvendor codes
+ from the device, you should call viIn16 using VI_PXI_CFG_SPACE. */
+#define VI_ATTR_PXI_SUB_MANF_ID (0x3FFF0203UL)
+#define VI_ATTR_PXI_SUB_MODEL_CODE (0x3FFF0204UL)
+#endif
+
+#define VI_ATTR_PXI_SRC_TRIG_BUS (0x3FFF020DUL)
+#define VI_ATTR_PXI_DEST_TRIG_BUS (0x3FFF020EUL)
+
+#define VI_ATTR_PXI_RECV_INTR_SEQ (0x3FFF4240UL)
+#define VI_ATTR_PXI_RECV_INTR_DATA (0x3FFF4241UL)
+
+#endif
+
+#if defined(NIVISA_USB)
+
+#define VI_ATTR_USB_BULK_OUT_PIPE (0x3FFF01A2UL)
+#define VI_ATTR_USB_BULK_IN_PIPE (0x3FFF01A3UL)
+#define VI_ATTR_USB_INTR_IN_PIPE (0x3FFF01A4UL)
+#define VI_ATTR_USB_CLASS (0x3FFF01A5UL)
+#define VI_ATTR_USB_SUBCLASS (0x3FFF01A6UL)
+#define VI_ATTR_USB_ALT_SETTING (0x3FFF01A8UL)
+#define VI_ATTR_USB_END_IN (0x3FFF01A9UL)
+#define VI_ATTR_USB_NUM_INTFCS (0x3FFF01AAUL)
+#define VI_ATTR_USB_NUM_PIPES (0x3FFF01ABUL)
+#define VI_ATTR_USB_BULK_OUT_STATUS (0x3FFF01ACUL)
+#define VI_ATTR_USB_BULK_IN_STATUS (0x3FFF01ADUL)
+#define VI_ATTR_USB_INTR_IN_STATUS (0x3FFF01AEUL)
+#define VI_ATTR_USB_CTRL_PIPE (0x3FFF01B0UL)
+
+#define VI_USB_PIPE_STATE_UNKNOWN (-1)
+#define VI_USB_PIPE_READY (0)
+#define VI_USB_PIPE_STALLED (1)
+
+#define VI_USB_END_NONE (0)
+#define VI_USB_END_SHORT (4)
+#define VI_USB_END_SHORT_OR_COUNT (5)
+
+#endif
+
+#define VI_ATTR_FIREWIRE_DEST_UPPER_OFFSET (0x3FFF01F0UL)
+#define VI_ATTR_FIREWIRE_SRC_UPPER_OFFSET (0x3FFF01F1UL)
+#define VI_ATTR_FIREWIRE_WIN_UPPER_OFFSET (0x3FFF01F2UL)
+#define VI_ATTR_FIREWIRE_VENDOR_ID (0x3FFF01F3UL)
+#define VI_ATTR_FIREWIRE_LOWER_CHIP_ID (0x3FFF01F4UL)
+#define VI_ATTR_FIREWIRE_UPPER_CHIP_ID (0x3FFF01F5UL)
+
+#define VI_FIREWIRE_DFLT_SPACE (5)
+
+#if defined(__cplusplus) || defined(__cplusplus__)
+ }
+#endif
+
+#endif
+
+/*- The End -----------------------------------------------------------------*/
diff --git a/hal/lib/athena/visa/visatype.h b/hal/lib/athena/visa/visatype.h
new file mode 100644
index 0000000..ef089dd
--- /dev/null
+++ b/hal/lib/athena/visa/visatype.h
@@ -0,0 +1,201 @@
+/*---------------------------------------------------------------------------*/
+/* Distributed by IVI Foundation Inc. */
+/* */
+/* Do not modify the contents of this file. */
+/*---------------------------------------------------------------------------*/
+/* */
+/* Title : VISATYPE.H */
+/* Date : 04-14-2006 */
+/* Purpose : Fundamental VISA data types and macro definitions */
+/* */
+/*---------------------------------------------------------------------------*/
+
+#ifndef __VISATYPE_HEADER__
+#define __VISATYPE_HEADER__
+
+#if defined(_WIN64)
+#define _VI_FAR
+#define _VI_FUNC __fastcall
+#define _VI_FUNCC __fastcall
+#define _VI_FUNCH __fastcall
+#define _VI_SIGNED signed
+#elif (defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__)) && !defined(_NI_mswin16_)
+#define _VI_FAR
+#define _VI_FUNC __stdcall
+#define _VI_FUNCC __cdecl
+#define _VI_FUNCH __stdcall
+#define _VI_SIGNED signed
+#elif defined(_CVI_) && defined(_NI_i386_)
+#define _VI_FAR
+#define _VI_FUNC _pascal
+#define _VI_FUNCC
+#define _VI_FUNCH _pascal
+#define _VI_SIGNED signed
+#elif (defined(_WINDOWS) || defined(_Windows)) && !defined(_NI_mswin16_)
+#define _VI_FAR _far
+#define _VI_FUNC _far _pascal _export
+#define _VI_FUNCC _far _cdecl _export
+#define _VI_FUNCH _far _pascal
+#define _VI_SIGNED signed
+#elif (defined(hpux) || defined(__hpux)) && (defined(__cplusplus) || defined(__cplusplus__))
+#define _VI_FAR
+#define _VI_FUNC
+#define _VI_FUNCC
+#define _VI_FUNCH
+#define _VI_SIGNED
+#else
+#define _VI_FAR
+#define _VI_FUNC
+#define _VI_FUNCC
+#define _VI_FUNCH
+#define _VI_SIGNED signed
+#endif
+
+#define _VI_ERROR (-2147483647L-1) /* 0x80000000 */
+#define _VI_PTR _VI_FAR *
+
+/*- VISA Types --------------------------------------------------------------*/
+
+#ifndef _VI_INT64_UINT64_DEFINED
+#if defined(_WIN64) || ((defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__)) && !defined(_NI_mswin16_))
+#if (defined(_MSC_VER) && (_MSC_VER >= 1200)) || (defined(_CVI_) && (_CVI_ >= 700)) || (defined(__BORLANDC__) && (__BORLANDC__ >= 0x0520))
+typedef unsigned __int64 ViUInt64;
+typedef _VI_SIGNED __int64 ViInt64;
+#define _VI_INT64_UINT64_DEFINED
+#if defined(_WIN64)
+#define _VISA_ENV_IS_64_BIT
+#else
+/* This is a 32-bit OS, not a 64-bit OS */
+#endif
+#endif
+#elif defined(__GNUC__) && (__GNUC__ >= 3)
+#include <limits.h>
+#include <sys/types.h>
+typedef u_int64_t ViUInt64;
+typedef int64_t ViInt64;
+#define _VI_INT64_UINT64_DEFINED
+#if defined(LONG_MAX) && (LONG_MAX > 0x7FFFFFFFL)
+#define _VISA_ENV_IS_64_BIT
+#else
+/* This is a 32-bit OS, not a 64-bit OS */
+#endif
+#else
+/* This platform does not support 64-bit types */
+#endif
+#endif
+
+#if defined(_VI_INT64_UINT64_DEFINED)
+typedef ViUInt64 _VI_PTR ViPUInt64;
+typedef ViUInt64 _VI_PTR ViAUInt64;
+typedef ViInt64 _VI_PTR ViPInt64;
+typedef ViInt64 _VI_PTR ViAInt64;
+#endif
+
+#if defined(LONG_MAX) && (LONG_MAX > 0x7FFFFFFFL)
+typedef unsigned int ViUInt32;
+typedef _VI_SIGNED int ViInt32;
+#else
+typedef unsigned long ViUInt32;
+typedef _VI_SIGNED long ViInt32;
+#endif
+
+typedef ViUInt32 _VI_PTR ViPUInt32;
+typedef ViUInt32 _VI_PTR ViAUInt32;
+typedef ViInt32 _VI_PTR ViPInt32;
+typedef ViInt32 _VI_PTR ViAInt32;
+
+typedef unsigned short ViUInt16;
+typedef ViUInt16 _VI_PTR ViPUInt16;
+typedef ViUInt16 _VI_PTR ViAUInt16;
+
+typedef _VI_SIGNED short ViInt16;
+typedef ViInt16 _VI_PTR ViPInt16;
+typedef ViInt16 _VI_PTR ViAInt16;
+
+typedef unsigned char ViUInt8;
+typedef ViUInt8 _VI_PTR ViPUInt8;
+typedef ViUInt8 _VI_PTR ViAUInt8;
+
+typedef _VI_SIGNED char ViInt8;
+typedef ViInt8 _VI_PTR ViPInt8;
+typedef ViInt8 _VI_PTR ViAInt8;
+
+typedef char ViChar;
+typedef ViChar _VI_PTR ViPChar;
+typedef ViChar _VI_PTR ViAChar;
+
+typedef unsigned char ViByte;
+typedef ViByte _VI_PTR ViPByte;
+typedef ViByte _VI_PTR ViAByte;
+
+typedef void _VI_PTR ViAddr;
+typedef ViAddr _VI_PTR ViPAddr;
+typedef ViAddr _VI_PTR ViAAddr;
+
+typedef float ViReal32;
+typedef ViReal32 _VI_PTR ViPReal32;
+typedef ViReal32 _VI_PTR ViAReal32;
+
+typedef double ViReal64;
+typedef ViReal64 _VI_PTR ViPReal64;
+typedef ViReal64 _VI_PTR ViAReal64;
+
+typedef ViPByte ViBuf;
+typedef ViPByte ViPBuf;
+typedef ViPByte _VI_PTR ViABuf;
+
+typedef ViPChar ViString;
+typedef ViPChar ViPString;
+typedef ViPChar _VI_PTR ViAString;
+
+typedef ViString ViRsrc;
+typedef ViString ViPRsrc;
+typedef ViString _VI_PTR ViARsrc;
+
+typedef ViUInt16 ViBoolean;
+typedef ViBoolean _VI_PTR ViPBoolean;
+typedef ViBoolean _VI_PTR ViABoolean;
+
+typedef ViInt32 ViStatus;
+typedef ViStatus _VI_PTR ViPStatus;
+typedef ViStatus _VI_PTR ViAStatus;
+
+typedef ViUInt32 ViVersion;
+typedef ViVersion _VI_PTR ViPVersion;
+typedef ViVersion _VI_PTR ViAVersion;
+
+typedef ViUInt32 ViObject;
+typedef ViObject _VI_PTR ViPObject;
+typedef ViObject _VI_PTR ViAObject;
+
+typedef ViObject ViSession;
+typedef ViSession _VI_PTR ViPSession;
+typedef ViSession _VI_PTR ViASession;
+
+typedef ViUInt32 ViAttr;
+
+#ifndef _VI_CONST_STRING_DEFINED
+typedef const ViChar * ViConstString;
+#define _VI_CONST_STRING_DEFINED
+#endif
+
+/*- Completion and Error Codes ----------------------------------------------*/
+
+#define VI_SUCCESS (0L)
+
+/*- Other VISA Definitions --------------------------------------------------*/
+
+#define VI_NULL (0)
+
+#define VI_TRUE (1)
+#define VI_FALSE (0)
+
+/*- Backward Compatibility Macros -------------------------------------------*/
+
+#define VISAFN _VI_FUNC
+#define ViPtr _VI_PTR
+
+#endif
+
+/*- The End -----------------------------------------------------------------*/
+
diff --git a/hal/lib/shared/handles/HandlesInternal.cpp b/hal/lib/shared/handles/HandlesInternal.cpp
new file mode 100644
index 0000000..26441d3
--- /dev/null
+++ b/hal/lib/shared/handles/HandlesInternal.cpp
@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be 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"
+
+namespace hal {
+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) {
+ 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 << 24;
+ // add index to set last 16 bits
+ handle += index;
+ return handle;
+}
+} // namespace hal
diff --git a/hal/lib/sim/HALDesktop.cpp b/hal/lib/sim/HALDesktop.cpp
new file mode 100644
index 0000000..adbc0dc
--- /dev/null
+++ b/hal/lib/sim/HALDesktop.cpp
@@ -0,0 +1,8 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+// nothing here yet!